diff --git a/.gitattributes b/.gitattributes index bacd87f65f86a7697e65b9cacd50ad26cd69b997..f56b6b60114af56df04366d3294193668893316d 100644 --- a/.gitattributes +++ b/.gitattributes @@ -43,3 +43,9 @@ src/exe/CMakeFiles/colmap_exe.dir/vocab_tree.cc.o filter=lfs diff=lfs merge=lfs src/exe/colmap filter=lfs diff=lfs merge=lfs -text src/libcolmap.a filter=lfs diff=lfs merge=lfs -text src/libcolmap_cuda.a filter=lfs diff=lfs merge=lfs -text +colmap/bin/colmap filter=lfs diff=lfs merge=lfs -text +colmap/lib/libcolmap.a filter=lfs diff=lfs merge=lfs -text +colmap/lib/libcolmap_cuda.a filter=lfs diff=lfs merge=lfs -text +colmap/lib/libpba.a filter=lfs diff=lfs merge=lfs -text +colmap/lib/libpoisson_recon.a filter=lfs diff=lfs merge=lfs -text +colmap/lib/libsift_gpu.a filter=lfs diff=lfs merge=lfs -text diff --git a/colmap/bin/colmap b/colmap/bin/colmap new file mode 100644 index 0000000000000000000000000000000000000000..b988d5ce2b10bbe2beede80bfbd49e646179916e --- /dev/null +++ b/colmap/bin/colmap @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9efc313d53067e8a32ff66555e20318dc8382bd06ccc9ea769440f20da5d74be +size 31933416 diff --git a/colmap/include/colmap/base/camera.h b/colmap/include/colmap/base/camera.h new file mode 100644 index 0000000000000000000000000000000000000000..7238b7e069b5e879a3434bd53f73af212e885996 --- /dev/null +++ b/colmap/include/colmap/base/camera.h @@ -0,0 +1,211 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_CAMERA_H_ +#define COLMAP_SRC_BASE_CAMERA_H_ + +#include + +#include "util/types.h" + +namespace colmap { + +// Camera class that holds the intrinsic parameters. Cameras may be shared +// between multiple images, e.g., if the same "physical" camera took multiple +// pictures with the exact same lens and intrinsics (focal length, etc.). +// This class has a specific distortion model defined by a camera model class. +class Camera { + public: + Camera(); + + // Access the unique identifier of the camera. + inline camera_t CameraId() const; + inline void SetCameraId(const camera_t camera_id); + + // Access the camera model. + inline int ModelId() const; + std::string ModelName() const; + void SetModelId(const int model_id); + void SetModelIdFromName(const std::string& model_name); + + // Access dimensions of the camera sensor. + inline size_t Width() const; + inline size_t Height() const; + inline void SetWidth(const size_t width); + inline void SetHeight(const size_t height); + + // Access focal length parameters. + double MeanFocalLength() const; + double FocalLength() const; + double FocalLengthX() const; + double FocalLengthY() const; + void SetFocalLength(const double focal_length); + void SetFocalLengthX(const double focal_length_x); + void SetFocalLengthY(const double focal_length_y); + + // Check if camera has prior focal length. + inline bool HasPriorFocalLength() const; + inline void SetPriorFocalLength(const bool prior); + + // Access principal point parameters. Only works if there are two + // principal point parameters. + double PrincipalPointX() const; + double PrincipalPointY() const; + void SetPrincipalPointX(const double ppx); + void SetPrincipalPointY(const double ppy); + + // Get the indices of the parameter groups in the parameter vector. + const std::vector& FocalLengthIdxs() const; + const std::vector& PrincipalPointIdxs() const; + const std::vector& ExtraParamsIdxs() const; + + // Get intrinsic calibration matrix composed from focal length and principal + // point parameters, excluding distortion parameters. + Eigen::Matrix3d CalibrationMatrix() const; + + // Get human-readable information about the parameter vector ordering. + std::string ParamsInfo() const; + + // Access the raw parameter vector. + inline size_t NumParams() const; + inline const std::vector& Params() const; + inline std::vector& Params(); + inline double Params(const size_t idx) const; + inline double& Params(const size_t idx); + inline const double* ParamsData() const; + inline double* ParamsData(); + inline void SetParams(const std::vector& params); + + // Concatenate parameters as comma-separated list. + std::string ParamsToString() const; + + // Set camera parameters from comma-separated list. + bool SetParamsFromString(const std::string& string); + + // Check whether parameters are valid, i.e. the parameter vector has + // the correct dimensions that match the specified camera model. + bool VerifyParams() const; + + // Check whether camera is already undistorted + bool IsUndistorted() const; + + // Check whether camera has bogus parameters. + bool HasBogusParams(const double min_focal_length_ratio, + const double max_focal_length_ratio, + const double max_extra_param) const; + + // Initialize parameters for given camera model and focal length, and set + // the principal point to be the image center. + void InitializeWithId(const int model_id, const double focal_length, + const size_t width, const size_t height); + void InitializeWithName(const std::string& model_name, + const double focal_length, const size_t width, + const size_t height); + + // Project point in image plane to world / infinity. + Eigen::Vector2d ImageToWorld(const Eigen::Vector2d& image_point) const; + + // Convert pixel threshold in image plane to world space. + double ImageToWorldThreshold(const double threshold) const; + + // Project point from world / infinity to image plane. + Eigen::Vector2d WorldToImage(const Eigen::Vector2d& world_point) const; + + // Rescale camera dimensions and accordingly the focal length and + // and the principal point. + void Rescale(const double scale); + void Rescale(const size_t width, const size_t height); + + private: + // The unique identifier of the camera. If the identifier is not specified + // it is set to `kInvalidCameraId`. + camera_t camera_id_; + + // The identifier of the camera model. If the camera model is not specified + // the identifier is `kInvalidCameraModelId`. + int model_id_; + + // The dimensions of the image, 0 if not initialized. + size_t width_; + size_t height_; + + // The focal length, principal point, and extra parameters. If the camera + // model is not specified, this vector is empty. + std::vector params_; + + // Whether there is a safe prior for the focal length, + // e.g. manually provided or extracted from EXIF + bool prior_focal_length_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +camera_t Camera::CameraId() const { return camera_id_; } + +void Camera::SetCameraId(const camera_t camera_id) { camera_id_ = camera_id; } + +int Camera::ModelId() const { return model_id_; } + +size_t Camera::Width() const { return width_; } + +size_t Camera::Height() const { return height_; } + +void Camera::SetWidth(const size_t width) { width_ = width; } + +void Camera::SetHeight(const size_t height) { height_ = height; } + +bool Camera::HasPriorFocalLength() const { return prior_focal_length_; } + +void Camera::SetPriorFocalLength(const bool prior) { + prior_focal_length_ = prior; +} + +size_t Camera::NumParams() const { return params_.size(); } + +const std::vector& Camera::Params() const { return params_; } + +std::vector& Camera::Params() { return params_; } + +double Camera::Params(const size_t idx) const { return params_[idx]; } + +double& Camera::Params(const size_t idx) { return params_[idx]; } + +const double* Camera::ParamsData() const { return params_.data(); } + +double* Camera::ParamsData() { return params_.data(); } + +void Camera::SetParams(const std::vector& params) { params_ = params; } + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_CAMERA_H_ diff --git a/colmap/include/colmap/base/camera_database.h b/colmap/include/colmap/base/camera_database.h new file mode 100644 index 0000000000000000000000000000000000000000..3b391d4a18f80d375045f4bef6ae8e91d83a310a --- /dev/null +++ b/colmap/include/colmap/base/camera_database.h @@ -0,0 +1,58 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_CAMERA_DATABASE_H_ +#define COLMAP_SRC_BASE_CAMERA_DATABASE_H_ + +#include + +#include "util/camera_specs.h" + +namespace colmap { + +// Database that contains sensor widths for many cameras, which is useful +// to automatically extract the focal length if EXIF information is incomplete. +class CameraDatabase { + public: + CameraDatabase(); + + size_t NumEntries() const { return specs_.size(); } + + bool QuerySensorWidth(const std::string& make, const std::string& model, + double* sensor_width); + + private: + static const camera_specs_t specs_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_CAMERA_DATABASE_H_ diff --git a/colmap/include/colmap/base/camera_models.h b/colmap/include/colmap/base/camera_models.h new file mode 100644 index 0000000000000000000000000000000000000000..2db6bb3576b9b209d4992dc2c4dfb4cdfb7e7166 --- /dev/null +++ b/colmap/include/colmap/base/camera_models.h @@ -0,0 +1,1533 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_CAMERA_MODELS_H_ +#define COLMAP_SRC_BASE_CAMERA_MODELS_H_ + +#include +#include +#include + +#include +#include + +#include + +namespace colmap { + +// This file defines several different camera models and arbitrary new camera +// models can be added by the following steps: +// +// 1. Add a new struct in this file which implements all the necessary methods. +// 2. Define an unique model_name and model_id for the camera model. +// 3. Add camera model to `CAMERA_MODEL_CASES` macro in this file. +// 4. Add new template specialization of test case for camera model to +// `camera_models_test.cc`. +// +// A camera model can have three different types of camera parameters: focal +// length, principal point, extra parameters (distortion parameters). The +// parameter array is split into different groups, so that we can enable or +// disable the refinement of the individual groups during bundle adjustment. It +// is up to the camera model to access the parameters correctly (it is free to +// do so in an arbitrary manner) - the parameters are not accessed from outside. +// +// A camera model must have the following methods: +// +// - `WorldToImage`: transform normalized camera coordinates to image +// coordinates (the inverse of `ImageToWorld`). Assumes that the world +// coordinates are given as (u, v, 1). +// - `ImageToWorld`: transform image coordinates to normalized camera +// coordinates (the inverse of `WorldToImage`). Produces world coordinates +// as (u, v, 1). +// - `ImageToWorldThreshold`: transform a threshold given in pixels to +// normalized units (e.g. useful for reprojection error thresholds). +// +// Whenever you specify the camera parameters in a list, they must appear +// exactly in the order as they are accessed in the defined model struct. +// +// The camera models follow the convention that the upper left image corner has +// the coordinate (0, 0), the lower right corner (width, height), i.e. that +// the upper left pixel center has coordinate (0.5, 0.5) and the lower right +// pixel center has the coordinate (width - 0.5, height - 0.5). + +static const int kInvalidCameraModelId = -1; + +#ifndef CAMERA_MODEL_DEFINITIONS +#define CAMERA_MODEL_DEFINITIONS(model_id_value, model_name_value, \ + num_params_value) \ + static const int kModelId = model_id_value; \ + static const size_t kNumParams = num_params_value; \ + static const int model_id; \ + static const std::string model_name; \ + static const size_t num_params; \ + static const std::string params_info; \ + static const std::vector focal_length_idxs; \ + static const std::vector principal_point_idxs; \ + static const std::vector extra_params_idxs; \ + \ + static inline int InitializeModelId() { return model_id_value; }; \ + static inline std::string InitializeModelName() { \ + return model_name_value; \ + }; \ + static inline size_t InitializeNumParams() { return num_params_value; }; \ + static inline std::string InitializeParamsInfo(); \ + static inline std::vector InitializeFocalLengthIdxs(); \ + static inline std::vector InitializePrincipalPointIdxs(); \ + static inline std::vector InitializeExtraParamsIdxs(); \ + static inline std::vector InitializeParams( \ + const double focal_length, const size_t width, const size_t height); \ + \ + template \ + static void WorldToImage(const T* params, const T u, const T v, T* x, T* y); \ + template \ + static void ImageToWorld(const T* params, const T x, const T y, T* u, T* v); \ + template \ + static void Distortion(const T* extra_params, const T u, const T v, T* du, \ + T* dv); +#endif + +#ifndef CAMERA_MODEL_CASES +#define CAMERA_MODEL_CASES \ + CAMERA_MODEL_CASE(SimplePinholeCameraModel) \ + CAMERA_MODEL_CASE(PinholeCameraModel) \ + CAMERA_MODEL_CASE(SimpleRadialCameraModel) \ + CAMERA_MODEL_CASE(SimpleRadialFisheyeCameraModel) \ + CAMERA_MODEL_CASE(RadialCameraModel) \ + CAMERA_MODEL_CASE(RadialFisheyeCameraModel) \ + CAMERA_MODEL_CASE(OpenCVCameraModel) \ + CAMERA_MODEL_CASE(OpenCVFisheyeCameraModel) \ + CAMERA_MODEL_CASE(FullOpenCVCameraModel) \ + CAMERA_MODEL_CASE(FOVCameraModel) \ + CAMERA_MODEL_CASE(ThinPrismFisheyeCameraModel) +#endif + +#ifndef CAMERA_MODEL_SWITCH_CASES +#define CAMERA_MODEL_SWITCH_CASES \ + CAMERA_MODEL_CASES \ + default: \ + CAMERA_MODEL_DOES_NOT_EXIST_EXCEPTION \ + break; +#endif + +#define CAMERA_MODEL_DOES_NOT_EXIST_EXCEPTION \ + throw std::domain_error("Camera model does not exist"); + +// The "Curiously Recurring Template Pattern" (CRTP) is used here, so that we +// can reuse some shared functionality between all camera models - +// defined in the BaseCameraModel. +template +struct BaseCameraModel { + template + static inline bool HasBogusParams(const std::vector& params, + const size_t width, const size_t height, + const T min_focal_length_ratio, + const T max_focal_length_ratio, + const T max_extra_param); + + template + static inline bool HasBogusFocalLength(const std::vector& params, + const size_t width, + const size_t height, + const T min_focal_length_ratio, + const T max_focal_length_ratio); + + template + static inline bool HasBogusPrincipalPoint(const std::vector& params, + const size_t width, + const size_t height); + + template + static inline bool HasBogusExtraParams(const std::vector& params, + const T max_extra_param); + + template + static inline T ImageToWorldThreshold(const T* params, const T threshold); + + template + static inline void IterativeUndistortion(const T* params, T* u, T* v); +}; + +// Simple Pinhole camera model. +// +// No Distortion is assumed. Only focal length and principal point is modeled. +// +// Parameter list is expected in the following order: +// +// f, cx, cy +// +// See https://en.wikipedia.org/wiki/Pinhole_camera_model +struct SimplePinholeCameraModel + : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(0, "SIMPLE_PINHOLE", 3) +}; + +// Pinhole camera model. +// +// No Distortion is assumed. Only focal length and principal point is modeled. +// +// Parameter list is expected in the following order: +// +// fx, fy, cx, cy +// +// See https://en.wikipedia.org/wiki/Pinhole_camera_model +struct PinholeCameraModel : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(1, "PINHOLE", 4) +}; + +// Simple camera model with one focal length and one radial distortion +// parameter. +// +// This model is similar to the camera model that VisualSfM uses with the +// difference that the distortion here is applied to the projections and +// not to the measurements. +// +// Parameter list is expected in the following order: +// +// f, cx, cy, k +// +struct SimpleRadialCameraModel + : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(2, "SIMPLE_RADIAL", 4) +}; + +// Simple camera model with one focal length and two radial distortion +// parameters. +// +// This model is equivalent to the camera model that Bundler uses +// (except for an inverse z-axis in the camera coordinate system). +// +// Parameter list is expected in the following order: +// +// f, cx, cy, k1, k2 +// +struct RadialCameraModel : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(3, "RADIAL", 5) +}; + +// OpenCV camera model. +// +// Based on the pinhole camera model. Additionally models radial and +// tangential distortion (up to 2nd degree of coefficients). Not suitable for +// large radial distortions of fish-eye cameras. +// +// Parameter list is expected in the following order: +// +// fx, fy, cx, cy, k1, k2, p1, p2 +// +// See +// http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html +struct OpenCVCameraModel : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(4, "OPENCV", 8) +}; + +// OpenCV fish-eye camera model. +// +// Based on the pinhole camera model. Additionally models radial and +// tangential Distortion (up to 2nd degree of coefficients). Suitable for +// large radial distortions of fish-eye cameras. +// +// Parameter list is expected in the following order: +// +// fx, fy, cx, cy, k1, k2, k3, k4 +// +// See +// http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html +struct OpenCVFisheyeCameraModel + : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(5, "OPENCV_FISHEYE", 8) +}; + +// Full OpenCV camera model. +// +// Based on the pinhole camera model. Additionally models radial and +// tangential Distortion. +// +// Parameter list is expected in the following order: +// +// fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, k5, k6 +// +// See +// http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html +struct FullOpenCVCameraModel : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(6, "FULL_OPENCV", 12) +}; + +// FOV camera model. +// +// Based on the pinhole camera model. Additionally models radial distortion. +// This model is for example used by Project Tango for its equidistant +// calibration type. +// +// Parameter list is expected in the following order: +// +// fx, fy, cx, cy, omega +// +// See: +// Frederic Devernay, Olivier Faugeras. Straight lines have to be straight: +// Automatic calibration and removal of distortion from scenes of structured +// environments. Machine vision and applications, 2001. +struct FOVCameraModel : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(7, "FOV", 5) + + template + static void Undistortion(const T* extra_params, const T u, const T v, T* du, + T* dv); +}; + +// Simple camera model with one focal length and one radial distortion +// parameter, suitable for fish-eye cameras. +// +// This model is equivalent to the OpenCVFisheyeCameraModel but has only one +// radial distortion coefficient. +// +// Parameter list is expected in the following order: +// +// f, cx, cy, k +// +struct SimpleRadialFisheyeCameraModel + : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(8, "SIMPLE_RADIAL_FISHEYE", 4) +}; + +// Simple camera model with one focal length and two radial distortion +// parameters, suitable for fish-eye cameras. +// +// This model is equivalent to the OpenCVFisheyeCameraModel but has only two +// radial distortion coefficients. +// +// Parameter list is expected in the following order: +// +// f, cx, cy, k1, k2 +// +struct RadialFisheyeCameraModel + : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(9, "RADIAL_FISHEYE", 5) +}; + +// Camera model with radial and tangential distortion coefficients and +// additional coefficients accounting for thin-prism distortion. +// +// This camera model is described in +// +// "Camera Calibration with Distortion Models and Accuracy Evaluation", +// J Weng et al., TPAMI, 1992. +// +// Parameter list is expected in the following order: +// +// fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, sx1, sy1 +// +struct ThinPrismFisheyeCameraModel + : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(10, "THIN_PRISM_FISHEYE", 12) +}; + +// Check whether camera model with given name or identifier exists. +bool ExistsCameraModelWithName(const std::string& model_name); +bool ExistsCameraModelWithId(const int model_id); + +// Convert camera name to unique camera model identifier. +// +// @param name Unique name of camera model. +// +// @return Unique identifier of camera model. +int CameraModelNameToId(const std::string& model_name); + +// Convert camera model identifier to unique camera model name. +// +// @param model_id Unique identifier of camera model. +// +// @return Unique name of camera model. +std::string CameraModelIdToName(const int model_id); + +// Initialize camera parameters using given image properties. +// +// Initializes all focal length parameters to the same given focal length and +// sets the principal point to the image center. +// +// @param model_id Unique identifier of camera model. +// @param focal_length Focal length, equal for all focal length parameters. +// @param width Sensor width of the camera. +// @param height Sensor height of the camera. +std::vector CameraModelInitializeParams(const int model_id, + const double focal_length, + const size_t width, + const size_t height); + +// Get human-readable information about the parameter vector order. +// +// @param model_id Unique identifier of camera model. +std::string CameraModelParamsInfo(const int model_id); + +// Get the indices of the parameter groups in the parameter vector. +// +// @param model_id Unique identifier of camera model. +const std::vector& CameraModelFocalLengthIdxs(const int model_id); +const std::vector& CameraModelPrincipalPointIdxs(const int model_id); +const std::vector& CameraModelExtraParamsIdxs(const int model_id); + +// Get the total number of parameters of a camera model. +size_t CameraModelNumParams(const int model_id); + +// Check whether parameters are valid, i.e. the parameter vector has +// the correct dimensions that match the specified camera model. +// +// @param model_id Unique identifier of camera model. +// @param params Array of camera parameters. +bool CameraModelVerifyParams(const int model_id, + const std::vector& params); + +// Check whether camera has bogus parameters. +// +// @param model_id Unique identifier of camera model. +// @param params Array of camera parameters. +// @param width Sensor width of the camera. +// @param height Sensor height of the camera. +// @param min_focal_length_ratio Minimum ratio of focal length over +// maximum sensor dimension. +// @param min_focal_length_ratio Maximum ratio of focal length over +// maximum sensor dimension. +// @param max_extra_param Maximum magnitude of each extra parameter. +bool CameraModelHasBogusParams(const int model_id, + const std::vector& params, + const size_t width, const size_t height, + const double min_focal_length_ratio, + const double max_focal_length_ratio, + const double max_extra_param); + +// Transform world coordinates in camera coordinate system to image coordinates. +// +// This is the inverse of `CameraModelImageToWorld`. +// +// @param model_id Unique model_id of camera model as defined in +// `CAMERA_MODEL_NAME_TO_CODE`. +// @param params Array of camera parameters. +// @param u, v Coordinates in camera system as (u, v, 1). +// @param x, y Output image coordinates in pixels. +inline void CameraModelWorldToImage(const int model_id, + const std::vector& params, + const double u, const double v, double* x, + double* y); + +// Transform image coordinates to world coordinates in camera coordinate system. +// +// This is the inverse of `CameraModelWorldToImage`. +// +// @param model_id Unique identifier of camera model. +// @param params Array of camera parameters. +// @param x, y Image coordinates in pixels. +// @param v, u Output Coordinates in camera system as (u, v, 1). +inline void CameraModelImageToWorld(const int model_id, + const std::vector& params, + const double x, const double y, double* u, + double* v); + +// Convert pixel threshold in image plane to world space by dividing +// the threshold through the mean focal length. +// +// @param model_id Unique identifier of camera model. +// @param params Array of camera parameters. +// @param threshold Image space threshold in pixels. +// +// @ return World space threshold. +inline double CameraModelImageToWorldThreshold( + const int model_id, const std::vector& params, + const double threshold); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////////////////// +// BaseCameraModel + +template +template +bool BaseCameraModel::HasBogusParams( + const std::vector& params, const size_t width, const size_t height, + const T min_focal_length_ratio, const T max_focal_length_ratio, + const T max_extra_param) { + if (HasBogusPrincipalPoint(params, width, height)) { + return true; + } + + if (HasBogusFocalLength(params, width, height, min_focal_length_ratio, + max_focal_length_ratio)) { + return true; + } + + if (HasBogusExtraParams(params, max_extra_param)) { + return true; + } + + return false; +} + +template +template +bool BaseCameraModel::HasBogusFocalLength( + const std::vector& params, const size_t width, const size_t height, + const T min_focal_length_ratio, const T max_focal_length_ratio) { + const size_t max_size = std::max(width, height); + + for (const auto& idx : CameraModel::focal_length_idxs) { + const T focal_length_ratio = params[idx] / max_size; + if (focal_length_ratio < min_focal_length_ratio || + focal_length_ratio > max_focal_length_ratio) { + return true; + } + } + + return false; +} + +template +template +bool BaseCameraModel::HasBogusPrincipalPoint( + const std::vector& params, const size_t width, const size_t height) { + const T cx = params[CameraModel::principal_point_idxs[0]]; + const T cy = params[CameraModel::principal_point_idxs[1]]; + return cx < 0 || cx > width || cy < 0 || cy > height; +} + +template +template +bool BaseCameraModel::HasBogusExtraParams( + const std::vector& params, const T max_extra_param) { + for (const auto& idx : CameraModel::extra_params_idxs) { + if (std::abs(params[idx]) > max_extra_param) { + return true; + } + } + + return false; +} + +template +template +T BaseCameraModel::ImageToWorldThreshold(const T* params, + const T threshold) { + T mean_focal_length = 0; + for (const auto& idx : CameraModel::focal_length_idxs) { + mean_focal_length += params[idx]; + } + mean_focal_length /= CameraModel::focal_length_idxs.size(); + return threshold / mean_focal_length; +} + +template +template +void BaseCameraModel::IterativeUndistortion(const T* params, T* u, + T* v) { + // Parameters for Newton iteration using numerical differentiation with + // central differences, 100 iterations should be enough even for complex + // camera models with higher order terms. + const size_t kNumIterations = 100; + const double kMaxStepNorm = 1e-10; + const double kRelStepSize = 1e-6; + + Eigen::Matrix2d J; + const Eigen::Vector2d x0(*u, *v); + Eigen::Vector2d x(*u, *v); + Eigen::Vector2d dx; + Eigen::Vector2d dx_0b; + Eigen::Vector2d dx_0f; + Eigen::Vector2d dx_1b; + Eigen::Vector2d dx_1f; + + for (size_t i = 0; i < kNumIterations; ++i) { + const double step0 = std::max(std::numeric_limits::epsilon(), + std::abs(kRelStepSize * x(0))); + const double step1 = std::max(std::numeric_limits::epsilon(), + std::abs(kRelStepSize * x(1))); + CameraModel::Distortion(params, x(0), x(1), &dx(0), &dx(1)); + CameraModel::Distortion(params, x(0) - step0, x(1), &dx_0b(0), &dx_0b(1)); + CameraModel::Distortion(params, x(0) + step0, x(1), &dx_0f(0), &dx_0f(1)); + CameraModel::Distortion(params, x(0), x(1) - step1, &dx_1b(0), &dx_1b(1)); + CameraModel::Distortion(params, x(0), x(1) + step1, &dx_1f(0), &dx_1f(1)); + J(0, 0) = 1 + (dx_0f(0) - dx_0b(0)) / (2 * step0); + J(0, 1) = (dx_1f(0) - dx_1b(0)) / (2 * step1); + J(1, 0) = (dx_0f(1) - dx_0b(1)) / (2 * step0); + J(1, 1) = 1 + (dx_1f(1) - dx_1b(1)) / (2 * step1); + const Eigen::Vector2d step_x = J.inverse() * (x + dx - x0); + x -= step_x; + if (step_x.squaredNorm() < kMaxStepNorm) { + break; + } + } + + *u = x(0); + *v = x(1); +} + +//////////////////////////////////////////////////////////////////////////////// +// SimplePinholeCameraModel + +std::string SimplePinholeCameraModel::InitializeParamsInfo() { + return "f, cx, cy"; +} + +std::vector SimplePinholeCameraModel::InitializeFocalLengthIdxs() { + return {0}; +} + +std::vector SimplePinholeCameraModel::InitializePrincipalPointIdxs() { + return {1, 2}; +} + +std::vector SimplePinholeCameraModel::InitializeExtraParamsIdxs() { + return {}; +} + +std::vector SimplePinholeCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, width / 2.0, height / 2.0}; +} + +template +void SimplePinholeCameraModel::WorldToImage(const T* params, const T u, + const T v, T* x, T* y) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // No Distortion + + // Transform to image coordinates + *x = f * u + c1; + *y = f * v + c2; +} + +template +void SimplePinholeCameraModel::ImageToWorld(const T* params, const T x, + const T y, T* u, T* v) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + *u = (x - c1) / f; + *v = (y - c2) / f; +} + +//////////////////////////////////////////////////////////////////////////////// +// PinholeCameraModel + +std::string PinholeCameraModel::InitializeParamsInfo() { + return "fx, fy, cx, cy"; +} + +std::vector PinholeCameraModel::InitializeFocalLengthIdxs() { + return {0, 1}; +} + +std::vector PinholeCameraModel::InitializePrincipalPointIdxs() { + return {2, 3}; +} + +std::vector PinholeCameraModel::InitializeExtraParamsIdxs() { + return {}; +} + +std::vector PinholeCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, focal_length, width / 2.0, height / 2.0}; +} + +template +void PinholeCameraModel::WorldToImage(const T* params, const T u, const T v, + T* x, T* y) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // No Distortion + + // Transform to image coordinates + *x = f1 * u + c1; + *y = f2 * v + c2; +} + +template +void PinholeCameraModel::ImageToWorld(const T* params, const T x, const T y, + T* u, T* v) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + *u = (x - c1) / f1; + *v = (y - c2) / f2; +} + +//////////////////////////////////////////////////////////////////////////////// +// SimpleRadialCameraModel + +std::string SimpleRadialCameraModel::InitializeParamsInfo() { + return "f, cx, cy, k"; +} + +std::vector SimpleRadialCameraModel::InitializeFocalLengthIdxs() { + return {0}; +} + +std::vector SimpleRadialCameraModel::InitializePrincipalPointIdxs() { + return {1, 2}; +} + +std::vector SimpleRadialCameraModel::InitializeExtraParamsIdxs() { + return {3}; +} + +std::vector SimpleRadialCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, width / 2.0, height / 2.0, 0}; +} + +template +void SimpleRadialCameraModel::WorldToImage(const T* params, const T u, + const T v, T* x, T* y) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Distortion + T du, dv; + Distortion(¶ms[3], u, v, &du, &dv); + *x = u + du; + *y = v + dv; + + // Transform to image coordinates + *x = f * *x + c1; + *y = f * *y + c2; +} + +template +void SimpleRadialCameraModel::ImageToWorld(const T* params, const T x, + const T y, T* u, T* v) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Lift points to normalized plane + *u = (x - c1) / f; + *v = (y - c2) / f; + + IterativeUndistortion(¶ms[3], u, v); +} + +template +void SimpleRadialCameraModel::Distortion(const T* extra_params, const T u, + const T v, T* du, T* dv) { + const T k = extra_params[0]; + + const T u2 = u * u; + const T v2 = v * v; + const T r2 = u2 + v2; + const T radial = k * r2; + *du = u * radial; + *dv = v * radial; +} + +//////////////////////////////////////////////////////////////////////////////// +// RadialCameraModel + +std::string RadialCameraModel::InitializeParamsInfo() { + return "f, cx, cy, k1, k2"; +} + +std::vector RadialCameraModel::InitializeFocalLengthIdxs() { + return {0}; +} + +std::vector RadialCameraModel::InitializePrincipalPointIdxs() { + return {1, 2}; +} + +std::vector RadialCameraModel::InitializeExtraParamsIdxs() { + return {3, 4}; +} + +std::vector RadialCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, width / 2.0, height / 2.0, 0, 0}; +} + +template +void RadialCameraModel::WorldToImage(const T* params, const T u, const T v, + T* x, T* y) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Distortion + T du, dv; + Distortion(¶ms[3], u, v, &du, &dv); + *x = u + du; + *y = v + dv; + + // Transform to image coordinates + *x = f * *x + c1; + *y = f * *y + c2; +} + +template +void RadialCameraModel::ImageToWorld(const T* params, const T x, const T y, + T* u, T* v) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Lift points to normalized plane + *u = (x - c1) / f; + *v = (y - c2) / f; + + IterativeUndistortion(¶ms[3], u, v); +} + +template +void RadialCameraModel::Distortion(const T* extra_params, const T u, const T v, + T* du, T* dv) { + const T k1 = extra_params[0]; + const T k2 = extra_params[1]; + + const T u2 = u * u; + const T v2 = v * v; + const T r2 = u2 + v2; + const T radial = k1 * r2 + k2 * r2 * r2; + *du = u * radial; + *dv = v * radial; +} + +//////////////////////////////////////////////////////////////////////////////// +// OpenCVCameraModel + +std::string OpenCVCameraModel::InitializeParamsInfo() { + return "fx, fy, cx, cy, k1, k2, p1, p2"; +} + +std::vector OpenCVCameraModel::InitializeFocalLengthIdxs() { + return {0, 1}; +} + +std::vector OpenCVCameraModel::InitializePrincipalPointIdxs() { + return {2, 3}; +} + +std::vector OpenCVCameraModel::InitializeExtraParamsIdxs() { + return {4, 5, 6, 7}; +} + +std::vector OpenCVCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, focal_length, width / 2.0, height / 2.0, 0, 0, 0, 0}; +} + +template +void OpenCVCameraModel::WorldToImage(const T* params, const T u, const T v, + T* x, T* y) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Distortion + T du, dv; + Distortion(¶ms[4], u, v, &du, &dv); + *x = u + du; + *y = v + dv; + + // Transform to image coordinates + *x = f1 * *x + c1; + *y = f2 * *y + c2; +} + +template +void OpenCVCameraModel::ImageToWorld(const T* params, const T x, const T y, + T* u, T* v) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Lift points to normalized plane + *u = (x - c1) / f1; + *v = (y - c2) / f2; + + IterativeUndistortion(¶ms[4], u, v); +} + +template +void OpenCVCameraModel::Distortion(const T* extra_params, const T u, const T v, + T* du, T* dv) { + const T k1 = extra_params[0]; + const T k2 = extra_params[1]; + const T p1 = extra_params[2]; + const T p2 = extra_params[3]; + + const T u2 = u * u; + const T uv = u * v; + const T v2 = v * v; + const T r2 = u2 + v2; + const T radial = k1 * r2 + k2 * r2 * r2; + *du = u * radial + T(2) * p1 * uv + p2 * (r2 + T(2) * u2); + *dv = v * radial + T(2) * p2 * uv + p1 * (r2 + T(2) * v2); +} + +//////////////////////////////////////////////////////////////////////////////// +// OpenCVFisheyeCameraModel + +std::string OpenCVFisheyeCameraModel::InitializeParamsInfo() { + return "fx, fy, cx, cy, k1, k2, k3, k4"; +} + +std::vector OpenCVFisheyeCameraModel::InitializeFocalLengthIdxs() { + return {0, 1}; +} + +std::vector OpenCVFisheyeCameraModel::InitializePrincipalPointIdxs() { + return {2, 3}; +} + +std::vector OpenCVFisheyeCameraModel::InitializeExtraParamsIdxs() { + return {4, 5, 6, 7}; +} + +std::vector OpenCVFisheyeCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, focal_length, width / 2.0, height / 2.0, 0, 0, 0, 0}; +} + +template +void OpenCVFisheyeCameraModel::WorldToImage(const T* params, const T u, + const T v, T* x, T* y) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Distortion + T du, dv; + Distortion(¶ms[4], u, v, &du, &dv); + *x = u + du; + *y = v + dv; + + // Transform to image coordinates + *x = f1 * *x + c1; + *y = f2 * *y + c2; +} + +template +void OpenCVFisheyeCameraModel::ImageToWorld(const T* params, const T x, + const T y, T* u, T* v) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Lift points to normalized plane + *u = (x - c1) / f1; + *v = (y - c2) / f2; + + IterativeUndistortion(¶ms[4], u, v); +} + +template +void OpenCVFisheyeCameraModel::Distortion(const T* extra_params, const T u, + const T v, T* du, T* dv) { + const T k1 = extra_params[0]; + const T k2 = extra_params[1]; + const T k3 = extra_params[2]; + const T k4 = extra_params[3]; + + const T r = ceres::sqrt(u * u + v * v); + + if (r > T(std::numeric_limits::epsilon())) { + const T theta = ceres::atan(r); + const T theta2 = theta * theta; + const T theta4 = theta2 * theta2; + const T theta6 = theta4 * theta2; + const T theta8 = theta4 * theta4; + const T thetad = + theta * (T(1) + k1 * theta2 + k2 * theta4 + k3 * theta6 + k4 * theta8); + *du = u * thetad / r - u; + *dv = v * thetad / r - v; + } else { + *du = T(0); + *dv = T(0); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// FullOpenCVCameraModel + +std::string FullOpenCVCameraModel::InitializeParamsInfo() { + return "fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, k5, k6"; +} + +std::vector FullOpenCVCameraModel::InitializeFocalLengthIdxs() { + return {0, 1}; +} + +std::vector FullOpenCVCameraModel::InitializePrincipalPointIdxs() { + return {2, 3}; +} + +std::vector FullOpenCVCameraModel::InitializeExtraParamsIdxs() { + return {4, 5, 6, 7, 8, 9, 10, 11}; +} + +std::vector FullOpenCVCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, + focal_length, + width / 2.0, + height / 2.0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0}; +} + +template +void FullOpenCVCameraModel::WorldToImage(const T* params, const T u, const T v, + T* x, T* y) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Distortion + T du, dv; + Distortion(¶ms[4], u, v, &du, &dv); + *x = u + du; + *y = v + dv; + + // Transform to image coordinates + *x = f1 * *x + c1; + *y = f2 * *y + c2; +} + +template +void FullOpenCVCameraModel::ImageToWorld(const T* params, const T x, const T y, + T* u, T* v) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Lift points to normalized plane + *u = (x - c1) / f1; + *v = (y - c2) / f2; + + IterativeUndistortion(¶ms[4], u, v); +} + +template +void FullOpenCVCameraModel::Distortion(const T* extra_params, const T u, + const T v, T* du, T* dv) { + const T k1 = extra_params[0]; + const T k2 = extra_params[1]; + const T p1 = extra_params[2]; + const T p2 = extra_params[3]; + const T k3 = extra_params[4]; + const T k4 = extra_params[5]; + const T k5 = extra_params[6]; + const T k6 = extra_params[7]; + + const T u2 = u * u; + const T uv = u * v; + const T v2 = v * v; + const T r2 = u2 + v2; + const T r4 = r2 * r2; + const T r6 = r4 * r2; + const T radial = (T(1) + k1 * r2 + k2 * r4 + k3 * r6) / + (T(1) + k4 * r2 + k5 * r4 + k6 * r6); + *du = u * radial + T(2) * p1 * uv + p2 * (r2 + T(2) * u2) - u; + *dv = v * radial + T(2) * p2 * uv + p1 * (r2 + T(2) * v2) - v; +} + +//////////////////////////////////////////////////////////////////////////////// +// FOVCameraModel + +std::string FOVCameraModel::InitializeParamsInfo() { + return "fx, fy, cx, cy, omega"; +} + +std::vector FOVCameraModel::InitializeFocalLengthIdxs() { + return {0, 1}; +} + +std::vector FOVCameraModel::InitializePrincipalPointIdxs() { + return {2, 3}; +} + +std::vector FOVCameraModel::InitializeExtraParamsIdxs() { return {4}; } + +std::vector FOVCameraModel::InitializeParams(const double focal_length, + const size_t width, + const size_t height) { + return {focal_length, focal_length, width / 2.0, height / 2.0, 1e-2}; +} + +template +void FOVCameraModel::WorldToImage(const T* params, const T u, const T v, T* x, + T* y) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Distortion + Distortion(¶ms[4], u, v, x, y); + + // Transform to image coordinates + *x = f1 * *x + c1; + *y = f2 * *y + c2; +} + +template +void FOVCameraModel::ImageToWorld(const T* params, const T x, const T y, T* u, + T* v) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Lift points to normalized plane + const T uu = (x - c1) / f1; + const T vv = (y - c2) / f2; + + // Undistortion + Undistortion(¶ms[4], uu, vv, u, v); +} + +template +void FOVCameraModel::Distortion(const T* extra_params, const T u, const T v, + T* du, T* dv) { + const T omega = extra_params[0]; + + // Chosen arbitrarily. + const T kEpsilon = T(1e-4); + + const T radius2 = u * u + v * v; + const T omega2 = omega * omega; + + T factor; + if (omega2 < kEpsilon) { + // Derivation of this case with Matlab: + // syms radius omega; + // factor(radius) = atan(radius * 2 * tan(omega / 2)) / ... + // (radius * omega); + // simplify(taylor(factor, omega, 'order', 3)) + factor = (omega2 * radius2) / T(3) - omega2 / T(12) + T(1); + } else if (radius2 < kEpsilon) { + // Derivation of this case with Matlab: + // syms radius omega; + // factor(radius) = atan(radius * 2 * tan(omega / 2)) / ... + // (radius * omega); + // simplify(taylor(factor, radius, 'order', 3)) + const T tan_half_omega = ceres::tan(omega / T(2)); + factor = (T(-2) * tan_half_omega * + (T(4) * radius2 * tan_half_omega * tan_half_omega - T(3))) / + (T(3) * omega); + } else { + const T radius = ceres::sqrt(radius2); + const T numerator = ceres::atan(radius * T(2) * ceres::tan(omega / T(2))); + factor = numerator / (radius * omega); + } + + *du = u * factor; + *dv = v * factor; +} + +template +void FOVCameraModel::Undistortion(const T* extra_params, const T u, const T v, + T* du, T* dv) { + T omega = extra_params[0]; + + // Chosen arbitrarily. + const T kEpsilon = T(1e-4); + + const T radius2 = u * u + v * v; + const T omega2 = omega * omega; + + T factor; + if (omega2 < kEpsilon) { + // Derivation of this case with Matlab: + // syms radius omega; + // factor(radius) = tan(radius * omega) / ... + // (radius * 2*tan(omega/2)); + // simplify(taylor(factor, omega, 'order', 3)) + factor = (omega2 * radius2) / T(3) - omega2 / T(12) + T(1); + } else if (radius2 < kEpsilon) { + // Derivation of this case with Matlab: + // syms radius omega; + // factor(radius) = tan(radius * omega) / ... + // (radius * 2*tan(omega/2)); + // simplify(taylor(factor, radius, 'order', 3)) + factor = (omega * (omega * omega * radius2 + T(3))) / + (T(6) * ceres::tan(omega / T(2))); + } else { + const T radius = ceres::sqrt(radius2); + const T numerator = ceres::tan(radius * omega); + factor = numerator / (radius * T(2) * ceres::tan(omega / T(2))); + } + + *du = u * factor; + *dv = v * factor; +} + +//////////////////////////////////////////////////////////////////////////////// +// SimpleRadialFisheyeCameraModel + +std::string SimpleRadialFisheyeCameraModel::InitializeParamsInfo() { + return "f, cx, cy, k"; +} + +std::vector +SimpleRadialFisheyeCameraModel::InitializeFocalLengthIdxs() { + return {0}; +} + +std::vector +SimpleRadialFisheyeCameraModel::InitializePrincipalPointIdxs() { + return {1, 2}; +} + +std::vector +SimpleRadialFisheyeCameraModel::InitializeExtraParamsIdxs() { + return {3}; +} + +std::vector SimpleRadialFisheyeCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, width / 2.0, height / 2.0, 0}; +} + +template +void SimpleRadialFisheyeCameraModel::WorldToImage(const T* params, const T u, + const T v, T* x, T* y) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Distortion + T du, dv; + Distortion(¶ms[3], u, v, &du, &dv); + *x = u + du; + *y = v + dv; + + // Transform to image coordinates + *x = f * *x + c1; + *y = f * *y + c2; +} + +template +void SimpleRadialFisheyeCameraModel::ImageToWorld(const T* params, const T x, + const T y, T* u, T* v) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Lift points to normalized plane + *u = (x - c1) / f; + *v = (y - c2) / f; + + IterativeUndistortion(¶ms[3], u, v); +} + +template +void SimpleRadialFisheyeCameraModel::Distortion(const T* extra_params, + const T u, const T v, T* du, + T* dv) { + const T k = extra_params[0]; + + const T r = ceres::sqrt(u * u + v * v); + + if (r > T(std::numeric_limits::epsilon())) { + const T theta = ceres::atan(r); + const T theta2 = theta * theta; + const T thetad = theta * (T(1) + k * theta2); + *du = u * thetad / r - u; + *dv = v * thetad / r - v; + } else { + *du = T(0); + *dv = T(0); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// RadialFisheyeCameraModel + +std::string RadialFisheyeCameraModel::InitializeParamsInfo() { + return "f, cx, cy, k1, k2"; +} + +std::vector RadialFisheyeCameraModel::InitializeFocalLengthIdxs() { + return {0}; +} + +std::vector RadialFisheyeCameraModel::InitializePrincipalPointIdxs() { + return {1, 2}; +} + +std::vector RadialFisheyeCameraModel::InitializeExtraParamsIdxs() { + return {3, 4}; +} + +std::vector RadialFisheyeCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, width / 2.0, height / 2.0, 0, 0}; +} + +template +void RadialFisheyeCameraModel::WorldToImage(const T* params, const T u, + const T v, T* x, T* y) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Distortion + T du, dv; + Distortion(¶ms[3], u, v, &du, &dv); + *x = u + du; + *y = v + dv; + + // Transform to image coordinates + *x = f * *x + c1; + *y = f * *y + c2; +} + +template +void RadialFisheyeCameraModel::ImageToWorld(const T* params, const T x, + const T y, T* u, T* v) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Lift points to normalized plane + *u = (x - c1) / f; + *v = (y - c2) / f; + + IterativeUndistortion(¶ms[3], u, v); +} + +template +void RadialFisheyeCameraModel::Distortion(const T* extra_params, const T u, + const T v, T* du, T* dv) { + const T k1 = extra_params[0]; + const T k2 = extra_params[1]; + + const T r = ceres::sqrt(u * u + v * v); + + if (r > T(std::numeric_limits::epsilon())) { + const T theta = ceres::atan(r); + const T theta2 = theta * theta; + const T theta4 = theta2 * theta2; + const T thetad = theta * (T(1) + k1 * theta2 + k2 * theta4); + *du = u * thetad / r - u; + *dv = v * thetad / r - v; + } else { + *du = T(0); + *dv = T(0); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// ThinPrismFisheyeCameraModel + +std::string ThinPrismFisheyeCameraModel::InitializeParamsInfo() { + return "fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, sx1, sy1"; +} + +std::vector ThinPrismFisheyeCameraModel::InitializeFocalLengthIdxs() { + return {0, 1}; +} + +std::vector +ThinPrismFisheyeCameraModel::InitializePrincipalPointIdxs() { + return {2, 3}; +} + +std::vector ThinPrismFisheyeCameraModel::InitializeExtraParamsIdxs() { + return {4, 5, 6, 7, 8, 9, 10, 11}; +} + +std::vector ThinPrismFisheyeCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, + focal_length, + width / 2.0, + height / 2.0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0}; +} + +template +void ThinPrismFisheyeCameraModel::WorldToImage(const T* params, const T u, + const T v, T* x, T* y) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + const T r = ceres::sqrt(u * u + v * v); + + T uu, vv; + if (r > T(std::numeric_limits::epsilon())) { + const T theta = ceres::atan(r); + uu = theta * u / r; + vv = theta * v / r; + } else { + uu = u; + vv = v; + } + + // Distortion + T du, dv; + Distortion(¶ms[4], uu, vv, &du, &dv); + *x = uu + du; + *y = vv + dv; + + // Transform to image coordinates + *x = f1 * *x + c1; + *y = f2 * *y + c2; +} + +template +void ThinPrismFisheyeCameraModel::ImageToWorld(const T* params, const T x, + const T y, T* u, T* v) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Lift points to normalized plane + *u = (x - c1) / f1; + *v = (y - c2) / f2; + + IterativeUndistortion(¶ms[4], u, v); + + const T theta = ceres::sqrt(*u * *u + *v * *v); + const T theta_cos_theta = theta * ceres::cos(theta); + if (theta_cos_theta > T(std::numeric_limits::epsilon())) { + const T scale = ceres::sin(theta) / theta_cos_theta; + *u *= scale; + *v *= scale; + } +} + +template +void ThinPrismFisheyeCameraModel::Distortion(const T* extra_params, const T u, + const T v, T* du, T* dv) { + const T k1 = extra_params[0]; + const T k2 = extra_params[1]; + const T p1 = extra_params[2]; + const T p2 = extra_params[3]; + const T k3 = extra_params[4]; + const T k4 = extra_params[5]; + const T sx1 = extra_params[6]; + const T sy1 = extra_params[7]; + + const T u2 = u * u; + const T uv = u * v; + const T v2 = v * v; + const T r2 = u2 + v2; + const T r4 = r2 * r2; + const T r6 = r4 * r2; + const T r8 = r6 * r2; + const T radial = k1 * r2 + k2 * r4 + k3 * r6 + k4 * r8; + *du = u * radial + T(2) * p1 * uv + p2 * (r2 + T(2) * u2) + sx1 * r2; + *dv = v * radial + T(2) * p2 * uv + p1 * (r2 + T(2) * v2) + sy1 * r2; +} + +//////////////////////////////////////////////////////////////////////////////// + +void CameraModelWorldToImage(const int model_id, + const std::vector& params, const double u, + const double v, double* x, double* y) { + switch (model_id) { +#define CAMERA_MODEL_CASE(CameraModel) \ + case CameraModel::kModelId: \ + CameraModel::WorldToImage(params.data(), u, v, x, y); \ + break; + + CAMERA_MODEL_SWITCH_CASES + +#undef CAMERA_MODEL_CASE + } +} + +void CameraModelImageToWorld(const int model_id, + const std::vector& params, const double x, + const double y, double* u, double* v) { + switch (model_id) { +#define CAMERA_MODEL_CASE(CameraModel) \ + case CameraModel::kModelId: \ + CameraModel::ImageToWorld(params.data(), x, y, u, v); \ + break; + + CAMERA_MODEL_SWITCH_CASES + +#undef CAMERA_MODEL_CASE + } +} + +double CameraModelImageToWorldThreshold(const int model_id, + const std::vector& params, + const double threshold) { + switch (model_id) { +#define CAMERA_MODEL_CASE(CameraModel) \ + case CameraModel::kModelId: \ + return CameraModel::ImageToWorldThreshold(params.data(), threshold); \ + break; + + CAMERA_MODEL_SWITCH_CASES + +#undef CAMERA_MODEL_CASE + } + + return -1; +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_CAMERA_MODELS_H_ diff --git a/colmap/include/colmap/base/camera_rig.h b/colmap/include/colmap/base/camera_rig.h new file mode 100644 index 0000000000000000000000000000000000000000..a99850e618321c0dd894aa068727b2ed95f8d039 --- /dev/null +++ b/colmap/include/colmap/base/camera_rig.h @@ -0,0 +1,128 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_CAMERA_RIG_H_ +#define COLMAP_SRC_BASE_CAMERA_RIG_H_ + +#include +#include + +#include "base/camera.h" +#include "base/pose.h" +#include "base/reconstruction.h" +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// This class holds information about the relative configuration of camera rigs. +// Camera rigs are composed of multiple cameras with a rigid relative extrinsic +// configuration over multiple snapshots. Snapshots are defined as the +// collection of images captured simultaneously by all cameras in the rig. +class CameraRig { + public: + CameraRig(); + + // The number of cameras in the rig. + size_t NumCameras() const; + + // The number of snapshots captured by this rig. + size_t NumSnapshots() const; + + // Check whether the given camera is part of the rig. + bool HasCamera(const camera_t camera_id) const; + + // Access the reference camera. + camera_t RefCameraId() const; + void SetRefCameraId(const camera_t camera_id); + + // Get the identifiers of the cameras in the rig. + std::vector GetCameraIds() const; + + // Get the snapshots of the camera rig. + const std::vector>& Snapshots() const; + + // Add a new camera to the rig. The relative pose may contain dummy values and + // can then be computed automatically from a given reconstruction using the + // method `ComputeRelativePoses`. + void AddCamera(const camera_t camera_id, const Eigen::Vector4d& rel_qvec, + const Eigen::Vector3d& rel_tvec); + + // Add the images of a single snapshot to rig. A snapshot consists of the + // captured images of all cameras of the rig. All images of a snapshot share + // the same global camera rig pose, i.e. all images in the camera rig are + // captured simultaneously. + void AddSnapshot(const std::vector& image_ids); + + // Check whether the camera rig setup is valid. + void Check(const Reconstruction& reconstruction) const; + + // Get the relative poses of the cameras in the rig. + Eigen::Vector4d& RelativeQvec(const camera_t camera_id); + const Eigen::Vector4d& RelativeQvec(const camera_t camera_id) const; + Eigen::Vector3d& RelativeTvec(const camera_t camera_id); + const Eigen::Vector3d& RelativeTvec(const camera_t camera_id) const; + + // Compute the scaling factor from the reconstruction to the camera rig + // dimensions by averaging over the distances between the projection centers. + // Note that this assumes that there is at least one camera pair in the rig + // with non-zero baseline, otherwise the function returns NaN. + double ComputeScale(const Reconstruction& reconstruction) const; + + // Compute the relative poses in the rig from the reconstruction by averaging + // the relative poses over all snapshots. The pose of the reference camera + // will be the identity transformation. This assumes that the camera rig has + // snapshots that are registered in the reconstruction. + bool ComputeRelativePoses(const Reconstruction& reconstruction); + + // Compute the absolute camera pose of the rig. The absolute camera pose of + // the rig is computed as the average of all relative camera poses in the rig + // and their corresponding image poses in the reconstruction. + void ComputeAbsolutePose(const size_t snapshot_idx, + const Reconstruction& reconstruction, + Eigen::Vector4d* abs_qvec, + Eigen::Vector3d* abs_tvec) const; + + private: + struct RigCamera { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::Vector4d rel_qvec = ComposeIdentityQuaternion(); + Eigen::Vector3d rel_tvec = Eigen::Vector3d(0, 0, 0); + }; + + camera_t ref_camera_id_ = kInvalidCameraId; + EIGEN_STL_UMAP(camera_t, RigCamera) rig_cameras_; + std::vector> snapshots_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_CAMERA_RIG_H_ diff --git a/colmap/include/colmap/base/correspondence_graph.h b/colmap/include/colmap/base/correspondence_graph.h new file mode 100644 index 0000000000000000000000000000000000000000..3c9714f3ca73f3eb6063c680eec25822d6eedbd4 --- /dev/null +++ b/colmap/include/colmap/base/correspondence_graph.h @@ -0,0 +1,206 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_CORRESPONDENCE_GRAPH_H_ +#define COLMAP_SRC_BASE_CORRESPONDENCE_GRAPH_H_ + +#include +#include + +#include "base/database.h" +#include "util/types.h" + +namespace colmap { + +// Scene graph represents the graph of image to image and feature to feature +// correspondences of a dataset. It should be accessed from the DatabaseCache. +class CorrespondenceGraph { + public: + struct Correspondence { + Correspondence() + : image_id(kInvalidImageId), point2D_idx(kInvalidPoint2DIdx) {} + Correspondence(const image_t image_id, const point2D_t point2D_idx) + : image_id(image_id), point2D_idx(point2D_idx) {} + + // The identifier of the corresponding image. + image_t image_id; + + // The index of the corresponding point in the corresponding image. + point2D_t point2D_idx; + }; + + CorrespondenceGraph(); + + // Number of added images. + inline size_t NumImages() const; + + // Number of added images. + inline size_t NumImagePairs() const; + + // Check whether image exists. + inline bool ExistsImage(const image_t image_id) const; + + // Get the number of observations in an image. An observation is an image + // point that has at least one correspondence. + inline point2D_t NumObservationsForImage(const image_t image_id) const; + + // Get the number of correspondences per image. + inline point2D_t NumCorrespondencesForImage(const image_t image_id) const; + + // Get the number of correspondences between a pair of images. + inline point2D_t NumCorrespondencesBetweenImages( + const image_t image_id1, const image_t image_id2) const; + + // Get the number of correspondences between all images. + std::unordered_map NumCorrespondencesBetweenImages() + const; + + // Finalize the database manager. + // + // - Calculates the number of observations per image by counting the number + // of image points that have at least one correspondence. + // - Deletes images without observations, as they are useless for SfM. + // - Shrinks the correspondence vectors to their size to save memory. + void Finalize(); + + // Add new image to the correspondence graph. + void AddImage(const image_t image_id, const size_t num_points2D); + + // Add correspondences between images. This function ignores invalid + // correspondences where the point indices are out of bounds or duplicate + // correspondences between the same image points. Whenever either of the two + // cases occur this function prints a warning to the standard output. + void AddCorrespondences(const image_t image_id1, const image_t image_id2, + const FeatureMatches& matches); + + // Find the correspondence of an image observation to all other images. + inline const std::vector& FindCorrespondences( + const image_t image_id, const point2D_t point2D_idx) const; + + // Find correspondences to the given observation. + // + // Transitively collects correspondences to the given observation by first + // finding correspondences to the given observation, then looking for + // correspondences to the collected correspondences in the first step, and so + // forth until the transitivity is exhausted or no more correspondences are + // found. The returned list does not contain duplicates and contains + // the given observation. + void FindTransitiveCorrespondences( + const image_t image_id, const point2D_t point2D_idx, + const size_t transitivity, + std::vector* found_corrs) const; + + // Find all correspondences between two images. + FeatureMatches FindCorrespondencesBetweenImages( + const image_t image_id1, const image_t image_id2) const; + + // Check whether the image point has correspondences. + inline bool HasCorrespondences(const image_t image_id, + const point2D_t point2D_idx) const; + + // Check whether the given observation is part of a two-view track, i.e. + // it only has one correspondence and that correspondence has the given + // observation as its only correspondence. + bool IsTwoViewObservation(const image_t image_id, + const point2D_t point2D_idx) const; + + private: + struct Image { + // Number of 2D points with at least one correspondence to another image. + point2D_t num_observations = 0; + + // Total number of correspondences to other images. This measure is useful + // to find a good initial pair, that is connected to many images. + point2D_t num_correspondences = 0; + + // Correspondences to other images per image point. + std::vector> corrs; + }; + + struct ImagePair { + // The number of correspondences between pairs of images. + point2D_t num_correspondences = 0; + }; + + EIGEN_STL_UMAP(image_t, Image) images_; + std::unordered_map image_pairs_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +size_t CorrespondenceGraph::NumImages() const { return images_.size(); } + +size_t CorrespondenceGraph::NumImagePairs() const { + return image_pairs_.size(); +} + +bool CorrespondenceGraph::ExistsImage(const image_t image_id) const { + return images_.find(image_id) != images_.end(); +} + +point2D_t CorrespondenceGraph::NumObservationsForImage( + const image_t image_id) const { + return images_.at(image_id).num_observations; +} + +point2D_t CorrespondenceGraph::NumCorrespondencesForImage( + const image_t image_id) const { + return images_.at(image_id).num_correspondences; +} + +point2D_t CorrespondenceGraph::NumCorrespondencesBetweenImages( + const image_t image_id1, const image_t image_id2) const { + const image_pair_t pair_id = + Database::ImagePairToPairId(image_id1, image_id2); + const auto it = image_pairs_.find(pair_id); + if (it == image_pairs_.end()) { + return 0; + } else { + return static_cast(it->second.num_correspondences); + } +} + +const std::vector& +CorrespondenceGraph::FindCorrespondences(const image_t image_id, + const point2D_t point2D_idx) const { + return images_.at(image_id).corrs.at(point2D_idx); +} + +bool CorrespondenceGraph::HasCorrespondences( + const image_t image_id, const point2D_t point2D_idx) const { + return !images_.at(image_id).corrs.at(point2D_idx).empty(); +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_CORRESPONDENCE_GRAPH_H_ diff --git a/colmap/include/colmap/base/cost_functions.h b/colmap/include/colmap/base/cost_functions.h new file mode 100644 index 0000000000000000000000000000000000000000..e7d0a5d300aad82b90d1e0e334251827e64b72e2 --- /dev/null +++ b/colmap/include/colmap/base/cost_functions.h @@ -0,0 +1,301 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_COST_FUNCTIONS_H_ +#define COLMAP_SRC_BASE_COST_FUNCTIONS_H_ + +#include + +#include +#include + +namespace colmap { + +// Standard bundle adjustment cost function for variable +// camera pose and calibration and point parameters. +template +class BundleAdjustmentCostFunction { + public: + explicit BundleAdjustmentCostFunction(const Eigen::Vector2d& point2D) + : observed_x_(point2D(0)), observed_y_(point2D(1)) {} + + static ceres::CostFunction* Create(const Eigen::Vector2d& point2D) { + return (new ceres::AutoDiffCostFunction< + BundleAdjustmentCostFunction, 2, 4, 3, 3, + CameraModel::kNumParams>( + new BundleAdjustmentCostFunction(point2D))); + } + + template + bool operator()(const T* const qvec, const T* const tvec, + const T* const point3D, const T* const camera_params, + T* residuals) const { + // Rotate and translate. + T projection[3]; + ceres::UnitQuaternionRotatePoint(qvec, point3D, projection); + projection[0] += tvec[0]; + projection[1] += tvec[1]; + projection[2] += tvec[2]; + + // Project to image plane. + projection[0] /= projection[2]; + projection[1] /= projection[2]; + + // Distort and transform to pixel space. + CameraModel::WorldToImage(camera_params, projection[0], projection[1], + &residuals[0], &residuals[1]); + + // Re-projection error. + residuals[0] -= T(observed_x_); + residuals[1] -= T(observed_y_); + + return true; + } + + private: + const double observed_x_; + const double observed_y_; +}; + +// Bundle adjustment cost function for variable +// camera calibration and point parameters, and fixed camera pose. +template +class BundleAdjustmentConstantPoseCostFunction { + public: + BundleAdjustmentConstantPoseCostFunction(const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec, + const Eigen::Vector2d& point2D) + : qw_(qvec(0)), + qx_(qvec(1)), + qy_(qvec(2)), + qz_(qvec(3)), + tx_(tvec(0)), + ty_(tvec(1)), + tz_(tvec(2)), + observed_x_(point2D(0)), + observed_y_(point2D(1)) {} + + static ceres::CostFunction* Create(const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec, + const Eigen::Vector2d& point2D) { + return (new ceres::AutoDiffCostFunction< + BundleAdjustmentConstantPoseCostFunction, 2, 3, + CameraModel::kNumParams>( + new BundleAdjustmentConstantPoseCostFunction(qvec, tvec, point2D))); + } + + template + bool operator()(const T* const point3D, const T* const camera_params, + T* residuals) const { + const T qvec[4] = {T(qw_), T(qx_), T(qy_), T(qz_)}; + + // Rotate and translate. + T projection[3]; + ceres::UnitQuaternionRotatePoint(qvec, point3D, projection); + projection[0] += T(tx_); + projection[1] += T(ty_); + projection[2] += T(tz_); + + // Project to image plane. + projection[0] /= projection[2]; + projection[1] /= projection[2]; + + // Distort and transform to pixel space. + CameraModel::WorldToImage(camera_params, projection[0], projection[1], + &residuals[0], &residuals[1]); + + // Re-projection error. + residuals[0] -= T(observed_x_); + residuals[1] -= T(observed_y_); + + return true; + } + + private: + const double qw_; + const double qx_; + const double qy_; + const double qz_; + const double tx_; + const double ty_; + const double tz_; + const double observed_x_; + const double observed_y_; +}; + +// Rig bundle adjustment cost function for variable camera pose and calibration +// and point parameters. Different from the standard bundle adjustment function, +// this cost function is suitable for camera rigs with consistent relative poses +// of the cameras within the rig. The cost function first projects points into +// the local system of the camera rig and then into the local system of the +// camera within the rig. +template +class RigBundleAdjustmentCostFunction { + public: + explicit RigBundleAdjustmentCostFunction(const Eigen::Vector2d& point2D) + : observed_x_(point2D(0)), observed_y_(point2D(1)) {} + + static ceres::CostFunction* Create(const Eigen::Vector2d& point2D) { + return (new ceres::AutoDiffCostFunction< + RigBundleAdjustmentCostFunction, 2, 4, 3, 4, 3, 3, + CameraModel::kNumParams>( + new RigBundleAdjustmentCostFunction(point2D))); + } + + template + bool operator()(const T* const rig_qvec, const T* const rig_tvec, + const T* const rel_qvec, const T* const rel_tvec, + const T* const point3D, const T* const camera_params, + T* residuals) const { + // Concatenate rotations. + T qvec[4]; + ceres::QuaternionProduct(rel_qvec, rig_qvec, qvec); + + // Concatenate translations. + T tvec[3]; + ceres::UnitQuaternionRotatePoint(rel_qvec, rig_tvec, tvec); + tvec[0] += rel_tvec[0]; + tvec[1] += rel_tvec[1]; + tvec[2] += rel_tvec[2]; + + // Rotate and translate. + T projection[3]; + ceres::UnitQuaternionRotatePoint(qvec, point3D, projection); + projection[0] += tvec[0]; + projection[1] += tvec[1]; + projection[2] += tvec[2]; + + // Project to image plane. + projection[0] /= projection[2]; + projection[1] /= projection[2]; + + // Distort and transform to pixel space. + CameraModel::WorldToImage(camera_params, projection[0], projection[1], + &residuals[0], &residuals[1]); + + // Re-projection error. + residuals[0] -= T(observed_x_); + residuals[1] -= T(observed_y_); + + return true; + } + + private: + const double observed_x_; + const double observed_y_; +}; + +// Cost function for refining two-view geometry based on the Sampson-Error. +// +// First pose is assumed to be located at the origin with 0 rotation. Second +// pose is assumed to be on the unit sphere around the first pose, i.e. the +// pose of the second camera is parameterized by a 3D rotation and a +// 3D translation with unit norm. `tvec` is therefore over-parameterized as is +// and should be down-projected using `SphereManifold`. +class RelativePoseCostFunction { + public: + RelativePoseCostFunction(const Eigen::Vector2d& x1, const Eigen::Vector2d& x2) + : x1_(x1(0)), y1_(x1(1)), x2_(x2(0)), y2_(x2(1)) {} + + static ceres::CostFunction* Create(const Eigen::Vector2d& x1, + const Eigen::Vector2d& x2) { + return (new ceres::AutoDiffCostFunction( + new RelativePoseCostFunction(x1, x2))); + } + + template + bool operator()(const T* const qvec, const T* const tvec, + T* residuals) const { + Eigen::Matrix R; + ceres::QuaternionToRotation(qvec, R.data()); + + // Matrix representation of the cross product t x R. + Eigen::Matrix t_x; + t_x << T(0), -tvec[2], tvec[1], tvec[2], T(0), -tvec[0], -tvec[1], tvec[0], + T(0); + + // Essential matrix. + const Eigen::Matrix E = t_x * R; + + // Homogeneous image coordinates. + const Eigen::Matrix x1_h(T(x1_), T(y1_), T(1)); + const Eigen::Matrix x2_h(T(x2_), T(y2_), T(1)); + + // Squared sampson error. + const Eigen::Matrix Ex1 = E * x1_h; + const Eigen::Matrix Etx2 = E.transpose() * x2_h; + const T x2tEx1 = x2_h.transpose() * Ex1; + residuals[0] = x2tEx1 * x2tEx1 / + (Ex1(0) * Ex1(0) + Ex1(1) * Ex1(1) + Etx2(0) * Etx2(0) + + Etx2(1) * Etx2(1)); + + return true; + } + + private: + const double x1_; + const double y1_; + const double x2_; + const double y2_; +}; + +inline void SetQuaternionManifold(ceres::Problem* problem, double* qvec) { +#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 1 + problem->SetManifold(qvec, new ceres::QuaternionManifold); +#else + problem->SetParameterization(qvec, new ceres::QuaternionParameterization); +#endif +} + +inline void SetSubsetManifold(int size, const std::vector& constant_params, + ceres::Problem* problem, double* params) { +#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 1 + problem->SetManifold(params, + new ceres::SubsetManifold(size, constant_params)); +#else + problem->SetParameterization( + params, new ceres::SubsetParameterization(size, constant_params)); +#endif +} + +template +inline void SetSphereManifold(ceres::Problem* problem, double* params) { +#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 1 + problem->SetManifold(params, new ceres::SphereManifold); +#else + problem->SetParameterization( + params, new ceres::HomogeneousVectorParameterization(size)); +#endif +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_COST_FUNCTIONS_H_ diff --git a/colmap/include/colmap/base/database.h b/colmap/include/colmap/base/database.h new file mode 100644 index 0000000000000000000000000000000000000000..5dc810982b2703a3e4ea65fef8cb27ded33490c7 --- /dev/null +++ b/colmap/include/colmap/base/database.h @@ -0,0 +1,394 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_DATABASE_H_ +#define COLMAP_SRC_BASE_DATABASE_H_ + +#include +#include +#include + +#include +#include + +#include "base/camera.h" +#include "base/image.h" +#include "estimators/two_view_geometry.h" +#include "feature/types.h" +#include "util/types.h" + +namespace colmap { + +// Database class to read and write images, features, cameras, matches, etc. +// from a SQLite database. The class is not thread-safe and must not be accessed +// concurrently. The class is optimized for single-thread speed and for optimal +// performance, wrap multiple method calls inside a leading `BeginTransaction` +// and trailing `EndTransaction`. +class Database { + public: + const static int kSchemaVersion = 1; + + // The maximum number of images, that can be stored in the database. + // This limitation arises due to the fact, that we generate unique IDs for + // image pairs manually. Note: do not change this to + // another type than `size_t`. + const static size_t kMaxNumImages; + + Database(); + explicit Database(const std::string& path); + ~Database(); + + // Open and close database. The same database should not be opened + // concurrently in multiple threads or processes. + void Open(const std::string& path); + void Close(); + + // Check if entry already exists in database. For image pairs, the order of + // `image_id1` and `image_id2` does not matter. + bool ExistsCamera(const camera_t camera_id) const; + bool ExistsImage(const image_t image_id) const; + bool ExistsImageWithName(std::string name) const; + bool ExistsKeypoints(const image_t image_id) const; + bool ExistsDescriptors(const image_t image_id) const; + bool ExistsMatches(const image_t image_id1, const image_t image_id2) const; + bool ExistsInlierMatches(const image_t image_id1, + const image_t image_id2) const; + + // Number of rows in `cameras` table. + size_t NumCameras() const; + + // Number of rows in `images` table. + size_t NumImages() const; + + // Sum of `rows` column in `keypoints` table, i.e. number of total keypoints. + size_t NumKeypoints() const; + + // The number of keypoints for the image with most features. + size_t MaxNumKeypoints() const; + + // Number of descriptors for specific image. + size_t NumKeypointsForImage(const image_t image_id) const; + + // Sum of `rows` column in `descriptors` table, + // i.e. number of total descriptors. + size_t NumDescriptors() const; + + // The number of descriptors for the image with most features. + size_t MaxNumDescriptors() const; + + // Number of descriptors for specific image. + size_t NumDescriptorsForImage(const image_t image_id) const; + + // Sum of `rows` column in `matches` table, i.e. number of total matches. + size_t NumMatches() const; + + // Sum of `rows` column in `two_view_geometries` table, + // i.e. number of total inlier matches. + size_t NumInlierMatches() const; + + // Number of rows in `matches` table. + size_t NumMatchedImagePairs() const; + + // Number of rows in `two_view_geometries` table. + size_t NumVerifiedImagePairs() const; + + // Each image pair is assigned an unique ID in the `matches` and + // `two_view_geometries` table. We intentionally avoid to store the pairs in a + // separate table by using e.g. AUTOINCREMENT, since the overhead of querying + // the unique pair ID is significant. + inline static image_pair_t ImagePairToPairId(const image_t image_id1, + const image_t image_id2); + + inline static void PairIdToImagePair(const image_pair_t pair_id, + image_t* image_id1, image_t* image_id2); + + // Return true if image pairs should be swapped. Used to enforce a specific + // image order to generate unique image pair identifiers independent of the + // order in which the image identifiers are used. + inline static bool SwapImagePair(const image_t image_id1, + const image_t image_id2); + + // Read an existing entry in the database. The user is responsible for making + // sure that the entry actually exists. For image pairs, the order of + // `image_id1` and `image_id2` does not matter. + Camera ReadCamera(const camera_t camera_id) const; + std::vector ReadAllCameras() const; + + Image ReadImage(const image_t image_id) const; + Image ReadImageWithName(const std::string& name) const; + std::vector ReadAllImages() const; + + FeatureKeypoints ReadKeypoints(const image_t image_id) const; + FeatureDescriptors ReadDescriptors(const image_t image_id) const; + + FeatureMatches ReadMatches(const image_t image_id1, + const image_t image_id2) const; + std::vector> ReadAllMatches() const; + + TwoViewGeometry ReadTwoViewGeometry(const image_t image_id1, + const image_t image_id2) const; + void ReadTwoViewGeometries( + std::vector* image_pair_ids, + std::vector* two_view_geometries) const; + + // Read all image pairs that have an entry in the `NumVerifiedImagePairs` + // table with at least one inlier match and their number of inlier matches. + void ReadTwoViewGeometryNumInliers( + std::vector>* image_pairs, + std::vector* num_inliers) const; + + // Add new camera and return its database identifier. If `use_camera_id` + // is false a new identifier is automatically generated. + camera_t WriteCamera(const Camera& camera, + const bool use_camera_id = false) const; + + // Add new image and return its database identifier. If `use_image_id` + // is false a new identifier is automatically generated. + image_t WriteImage(const Image& image, const bool use_image_id = false) const; + + // Write a new entry in the database. The user is responsible for making sure + // that the entry does not yet exist. For image pairs, the order of + // `image_id1` and `image_id2` does not matter. + void WriteKeypoints(const image_t image_id, + const FeatureKeypoints& keypoints) const; + void WriteDescriptors(const image_t image_id, + const FeatureDescriptors& descriptors) const; + void WriteMatches(const image_t image_id1, const image_t image_id2, + const FeatureMatches& matches) const; + void WriteTwoViewGeometry(const image_t image_id1, const image_t image_id2, + const TwoViewGeometry& two_view_geometry) const; + + // Update an existing camera in the database. The user is responsible for + // making sure that the entry already exists. + void UpdateCamera(const Camera& camera) const; + + // Update an existing image in the database. The user is responsible for + // making sure that the entry already exists. + void UpdateImage(const Image& image) const; + + // Delete matches of an image pair. + void DeleteMatches(const image_t image_id1, const image_t image_id2) const; + + // Delete inlier matches of an image pair. + void DeleteInlierMatches(const image_t image_id1, + const image_t image_id2) const; + + // Clear all database tables + void ClearAllTables() const; + + // Clear the entire cameras table + void ClearCameras() const; + + // Clear the entire images, keypoints, and descriptors tables + void ClearImages() const; + + // Clear the entire descriptors table + void ClearDescriptors() const; + + // Clear the entire keypoints table + void ClearKeypoints() const; + + // Clear the entire matches table. + void ClearMatches() const; + + // Clear the entire inlier matches table. + void ClearTwoViewGeometries() const; + + // Merge two databases into a single, new database. + static void Merge(const Database& database1, const Database& database2, + Database* merged_database); + + private: + friend class DatabaseTransaction; + + // Combine multiple queries into one transaction by wrapping a code section + // into a `BeginTransaction` and `EndTransaction`. You can create a scoped + // transaction with `DatabaseTransaction` that ends when the transaction + // object is destructed. Combining queries results in faster transaction time + // due to reduced locking of the database etc. + void BeginTransaction() const; + void EndTransaction() const; + + // Prepare SQL statements once at construction of the database, and reuse + // the statements for multiple queries by resetting their states. + void PrepareSQLStatements(); + void FinalizeSQLStatements(); + + // Create database tables, if not existing, called when opening a database. + void CreateTables() const; + void CreateCameraTable() const; + void CreateImageTable() const; + void CreateKeypointsTable() const; + void CreateDescriptorsTable() const; + void CreateMatchesTable() const; + void CreateTwoViewGeometriesTable() const; + + void UpdateSchema() const; + + bool ExistsTable(const std::string& table_name) const; + bool ExistsColumn(const std::string& table_name, + const std::string& column_name) const; + + bool ExistsRowId(sqlite3_stmt* sql_stmt, const sqlite3_int64 row_id) const; + bool ExistsRowString(sqlite3_stmt* sql_stmt, + const std::string& row_entry) const; + + size_t CountRows(const std::string& table) const; + size_t CountRowsForEntry(sqlite3_stmt* sql_stmt, + const sqlite3_int64 row_id) const; + size_t SumColumn(const std::string& column, const std::string& table) const; + size_t MaxColumn(const std::string& column, const std::string& table) const; + + sqlite3* database_ = nullptr; + + // Check if elements got removed from the database to only apply + // the VACUUM command in such case + mutable bool database_cleared_ = false; + + // Ensure that only one database object at a time updates the schema of a + // database. Since the schema is updated every time a database is opened, this + // is to ensure that there are no race conditions ("database locked" error + // messages) when the user actually only intends to read from the database, + // which requires to open it. + static std::mutex update_schema_mutex_; + + // Used to ensure that only one transaction is active at the same time. + std::mutex transaction_mutex_; + + // A collection of all `sqlite3_stmt` objects for deletion in the destructor. + std::vector sql_stmts_; + + // num_* + sqlite3_stmt* sql_stmt_num_keypoints_ = nullptr; + sqlite3_stmt* sql_stmt_num_descriptors_ = nullptr; + + // exists_* + sqlite3_stmt* sql_stmt_exists_camera_ = nullptr; + sqlite3_stmt* sql_stmt_exists_image_id_ = nullptr; + sqlite3_stmt* sql_stmt_exists_image_name_ = nullptr; + sqlite3_stmt* sql_stmt_exists_keypoints_ = nullptr; + sqlite3_stmt* sql_stmt_exists_descriptors_ = nullptr; + sqlite3_stmt* sql_stmt_exists_matches_ = nullptr; + sqlite3_stmt* sql_stmt_exists_two_view_geometry_ = nullptr; + + // add_* + sqlite3_stmt* sql_stmt_add_camera_ = nullptr; + sqlite3_stmt* sql_stmt_add_image_ = nullptr; + + // update_* + sqlite3_stmt* sql_stmt_update_camera_ = nullptr; + sqlite3_stmt* sql_stmt_update_image_ = nullptr; + + // read_* + sqlite3_stmt* sql_stmt_read_camera_ = nullptr; + sqlite3_stmt* sql_stmt_read_cameras_ = nullptr; + sqlite3_stmt* sql_stmt_read_image_id_ = nullptr; + sqlite3_stmt* sql_stmt_read_image_name_ = nullptr; + sqlite3_stmt* sql_stmt_read_images_ = nullptr; + sqlite3_stmt* sql_stmt_read_keypoints_ = nullptr; + sqlite3_stmt* sql_stmt_read_descriptors_ = nullptr; + sqlite3_stmt* sql_stmt_read_matches_ = nullptr; + sqlite3_stmt* sql_stmt_read_matches_all_ = nullptr; + sqlite3_stmt* sql_stmt_read_two_view_geometry_ = nullptr; + sqlite3_stmt* sql_stmt_read_two_view_geometries_ = nullptr; + sqlite3_stmt* sql_stmt_read_two_view_geometry_num_inliers_ = nullptr; + + // write_* + sqlite3_stmt* sql_stmt_write_keypoints_ = nullptr; + sqlite3_stmt* sql_stmt_write_descriptors_ = nullptr; + sqlite3_stmt* sql_stmt_write_matches_ = nullptr; + sqlite3_stmt* sql_stmt_write_two_view_geometry_ = nullptr; + + // delete_* + sqlite3_stmt* sql_stmt_delete_matches_ = nullptr; + sqlite3_stmt* sql_stmt_delete_two_view_geometry_ = nullptr; + + // clear_* + sqlite3_stmt* sql_stmt_clear_cameras_ = nullptr; + sqlite3_stmt* sql_stmt_clear_images_ = nullptr; + sqlite3_stmt* sql_stmt_clear_descriptors_ = nullptr; + sqlite3_stmt* sql_stmt_clear_keypoints_ = nullptr; + sqlite3_stmt* sql_stmt_clear_matches_ = nullptr; + sqlite3_stmt* sql_stmt_clear_two_view_geometries_ = nullptr; +}; + +// This class automatically manages the scope of a database transaction by +// calling `BeginTransaction` and `EndTransaction` during construction and +// destruction, respectively. +class DatabaseTransaction { + public: + explicit DatabaseTransaction(Database* database); + ~DatabaseTransaction(); + + private: + NON_COPYABLE(DatabaseTransaction) + NON_MOVABLE(DatabaseTransaction) + Database* database_; + std::unique_lock database_lock_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +image_pair_t Database::ImagePairToPairId(const image_t image_id1, + const image_t image_id2) { + CHECK_GE(image_id1, 0); + CHECK_GE(image_id2, 0); + CHECK_LT(image_id1, kMaxNumImages); + CHECK_LT(image_id2, kMaxNumImages); + if (SwapImagePair(image_id1, image_id2)) { + return static_cast(kMaxNumImages) * image_id2 + image_id1; + } else { + return static_cast(kMaxNumImages) * image_id1 + image_id2; + } +} + +void Database::PairIdToImagePair(const image_pair_t pair_id, image_t* image_id1, + image_t* image_id2) { + *image_id2 = static_cast(pair_id % kMaxNumImages); + *image_id1 = static_cast((pair_id - *image_id2) / kMaxNumImages); + CHECK_GE(*image_id1, 0); + CHECK_GE(*image_id2, 0); + CHECK_LT(*image_id1, kMaxNumImages); + CHECK_LT(*image_id2, kMaxNumImages); +} + +// Return true if image pairs should be swapped. Used to enforce a specific +// image order to generate unique image pair identifiers independent of the +// order in which the image identifiers are used. +bool Database::SwapImagePair(const image_t image_id1, const image_t image_id2) { + return image_id1 > image_id2; +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_DATABASE_H_ diff --git a/colmap/include/colmap/base/database_cache.h b/colmap/include/colmap/base/database_cache.h new file mode 100644 index 0000000000000000000000000000000000000000..db50150c55be3a4c905995157d8323334fed9c35 --- /dev/null +++ b/colmap/include/colmap/base/database_cache.h @@ -0,0 +1,151 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_DATABASE_CACHE_H_ +#define COLMAP_SRC_BASE_DATABASE_CACHE_H_ + +#include +#include +#include +#include + +#include + +#include "base/camera.h" +#include "base/camera_models.h" +#include "base/correspondence_graph.h" +#include "base/database.h" +#include "base/image.h" +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// A class that caches the contents of the database in memory, used to quickly +// create new reconstruction instances when multiple models are reconstructed. +class DatabaseCache { + public: + DatabaseCache(); + + // Get number of objects. + inline size_t NumCameras() const; + inline size_t NumImages() const; + + // Get specific objects. + inline class Camera& Camera(const camera_t camera_id); + inline const class Camera& Camera(const camera_t camera_id) const; + inline class Image& Image(const image_t image_id); + inline const class Image& Image(const image_t image_id) const; + + // Get all objects. + inline const EIGEN_STL_UMAP(camera_t, class Camera) & Cameras() const; + inline const EIGEN_STL_UMAP(image_t, class Image) & Images() const; + + // Check whether specific object exists. + inline bool ExistsCamera(const camera_t camera_id) const; + inline bool ExistsImage(const image_t image_id) const; + + // Get reference to correspondence graph. + inline const class CorrespondenceGraph& CorrespondenceGraph() const; + + // Manually add data to cache. + void AddCamera(class Camera camera); + void AddImage(class Image image); + + // Load cameras, images, features, and matches from database. + // + // @param database Source database from which to load data. + // @param min_num_matches Only load image pairs with a minimum number + // of matches. + // @param ignore_watermarks Whether to ignore watermark image pairs. + // @param image_names Whether to use only load the data for a subset + // of the images. All images are used if empty. + void Load(const Database& database, const size_t min_num_matches, + const bool ignore_watermarks, + const std::unordered_set& image_names); + + // Find specific image by name. Note that this uses linear search. + const class Image* FindImageWithName(const std::string& name) const; + + private: + class CorrespondenceGraph correspondence_graph_; + + EIGEN_STL_UMAP(camera_t, class Camera) cameras_; + EIGEN_STL_UMAP(image_t, class Image) images_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +size_t DatabaseCache::NumCameras() const { return cameras_.size(); } +size_t DatabaseCache::NumImages() const { return images_.size(); } + +class Camera& DatabaseCache::Camera(const camera_t camera_id) { + return cameras_.at(camera_id); +} + +const class Camera& DatabaseCache::Camera(const camera_t camera_id) const { + return cameras_.at(camera_id); +} + +class Image& DatabaseCache::Image(const image_t image_id) { + return images_.at(image_id); +} + +const class Image& DatabaseCache::Image(const image_t image_id) const { + return images_.at(image_id); +} + +const EIGEN_STL_UMAP(camera_t, class Camera) & DatabaseCache::Cameras() const { + return cameras_; +} + +const EIGEN_STL_UMAP(image_t, class Image) & DatabaseCache::Images() const { + return images_; +} + +bool DatabaseCache::ExistsCamera(const camera_t camera_id) const { + return cameras_.find(camera_id) != cameras_.end(); +} + +bool DatabaseCache::ExistsImage(const image_t image_id) const { + return images_.find(image_id) != images_.end(); +} + +inline const class CorrespondenceGraph& DatabaseCache::CorrespondenceGraph() + const { + return correspondence_graph_; +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_DATABASE_CACHE_H_ diff --git a/colmap/include/colmap/base/essential_matrix.h b/colmap/include/colmap/base/essential_matrix.h new file mode 100644 index 0000000000000000000000000000000000000000..c221259473172470fdfbf086ab73abb5ff91bee7 --- /dev/null +++ b/colmap/include/colmap/base/essential_matrix.h @@ -0,0 +1,160 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_ESSENTIAL_MATRIX_H_ +#define COLMAP_SRC_BASE_ESSENTIAL_MATRIX_H_ + +#include + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Decompose an essential matrix into the possible rotations and translations. +// +// The first pose is assumed to be P = [I | 0] and the set of four other +// possible second poses are defined as: {[R1 | t], [R2 | t], +// [R1 | -t], [R2 | -t]} +// +// @param E 3x3 essential matrix. +// @param R1 First possible 3x3 rotation matrix. +// @param R2 Second possible 3x3 rotation matrix. +// @param t 3x1 possible translation vector (also -t possible). +void DecomposeEssentialMatrix(const Eigen::Matrix3d& E, Eigen::Matrix3d* R1, + Eigen::Matrix3d* R2, Eigen::Vector3d* t); + +// Recover the most probable pose from the given essential matrix. +// +// The pose of the first image is assumed to be P = [I | 0]. +// +// @param E 3x3 essential matrix. +// @param points1 First set of corresponding points. +// @param points2 Second set of corresponding points. +// @param inlier_mask Only points with `true` in the inlier mask are +// considered in the cheirality test. Size of the +// inlier mask must match the number of points N. +// @param R Most probable 3x3 rotation matrix. +// @param t Most probable 3x1 translation vector. +// @param points3D Triangulated 3D points infront of camera. +void PoseFromEssentialMatrix(const Eigen::Matrix3d& E, + const std::vector& points1, + const std::vector& points2, + Eigen::Matrix3d* R, Eigen::Vector3d* t, + std::vector* points3D); + +// Compose essential matrix from relative camera poses. +// +// Assumes that first camera pose has projection matrix P = [I | 0], and +// pose of second camera is given as transformation from world to camera system. +// +// @param R 3x3 rotation matrix. +// @param t 3x1 translation vector. +// +// @return 3x3 essential matrix. +Eigen::Matrix3d EssentialMatrixFromPose(const Eigen::Matrix3d& R, + const Eigen::Vector3d& t); + +// Compose essential matrix from two absolute camera poses. +// +// @param proj_matrix1 3x4 projection matrix. +// @param proj_matrix2 3x4 projection matrix. +// +// @return 3x3 essential matrix. +Eigen::Matrix3d EssentialMatrixFromAbsolutePoses( + const Eigen::Matrix3x4d& proj_matrix1, + const Eigen::Matrix3x4d& proj_matrix2); + +// Find optimal image points, such that: +// +// optimal_point1^t * E * optimal_point2 = 0 +// +// as described in: +// +// Lindstrom, P., "Triangulation made easy", +// Computer Vision and Pattern Recognition (CVPR), +// 2010 IEEE Conference on , vol., no., pp.1554,1561, 13-18 June 2010 +// +// @param E Essential or fundamental matrix. +// @param point1 Corresponding 2D point in first image. +// @param point2 Corresponding 2D point in second image. +// @param optimal_point1 Estimated optimal image point in the first image. +// @param optimal_point2 Estimated optimal image point in the second image. +void FindOptimalImageObservations(const Eigen::Matrix3d& E, + const Eigen::Vector2d& point1, + const Eigen::Vector2d& point2, + Eigen::Vector2d* optimal_point1, + Eigen::Vector2d* optimal_point2); + +// Compute the location of the epipole in homogeneous coordinates. +// +// @param E 3x3 essential matrix. +// @param left_image If true, epipole in left image is computed, +// else in right image. +// +// @return Epipole in homogeneous coordinates. +Eigen::Vector3d EpipoleFromEssentialMatrix(const Eigen::Matrix3d& E, + const bool left_image); + +// Invert the essential matrix, i.e. if the essential matrix E describes the +// transformation from camera A to B, the inverted essential matrix E' describes +// the transformation from camera B to A. +// +// @param E 3x3 essential matrix. +// +// @return Inverted essential matrix. +Eigen::Matrix3d InvertEssentialMatrix(const Eigen::Matrix3d& matrix); + +// Refine essential matrix. +// +// Decomposes the essential matrix into rotation and translation components +// and refines the relative pose using the function `RefineRelativePose`. +// +// @param E 3x3 essential matrix. +// @param points1 First set of corresponding points. +// @param points2 Second set of corresponding points. +// @param inlier_mask Inlier mask for corresponding points. +// @param options Solver options. +// +// @return Flag indicating if solution is usable. +bool RefineEssentialMatrix(const ceres::Solver::Options& options, + const std::vector& points1, + const std::vector& points2, + const std::vector& inlier_mask, + Eigen::Matrix3d* E); + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_ESSENTIAL_MATRIX_H_ diff --git a/colmap/include/colmap/base/gps.h b/colmap/include/colmap/base/gps.h new file mode 100644 index 0000000000000000000000000000000000000000..d40ee91ba27a0ccb05eaf6e914db95416ba30187 --- /dev/null +++ b/colmap/include/colmap/base/gps.h @@ -0,0 +1,89 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_GPS_H_ +#define COLMAP_SRC_BASE_GPS_H_ + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Transform ellipsoidal GPS coordinates to Cartesian GPS coordinate +// representation and vice versa. +class GPSTransform { + public: + enum ELLIPSOID { GRS80, WGS84 }; + + explicit GPSTransform(const int ellipsoid = GRS80); + + std::vector EllToXYZ( + const std::vector& ell) const; + + std::vector XYZToEll( + const std::vector& xyz) const; + + // Convert GPS (lat / lon / alt) to ENU coords. with lat0 and lon0 + // defining the origin of the ENU frame + std::vector EllToENU(const std::vector& ell, + const double lat0, + const double lon0) const; + + std::vector XYZToENU(const std::vector& xyz, + const double lat0, + const double lon0) const; + + std::vector ENUToEll(const std::vector& enu, + const double lat0, const double lon0, + const double alt0) const; + + std::vector ENUToXYZ(const std::vector& enu, + const double lat0, const double lon0, + const double alt0) const; + + private: + // Semimajor axis. + double a_; + // Semiminor axis. + double b_; + // Flattening. + double f_; + // Numerical eccentricity. + double e2_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_GPS_H_ diff --git a/colmap/include/colmap/base/graph_cut.h b/colmap/include/colmap/base/graph_cut.h new file mode 100644 index 0000000000000000000000000000000000000000..cb0cf3fe27e29667b9ec26d3d885e9e586e6b949 --- /dev/null +++ b/colmap/include/colmap/base/graph_cut.h @@ -0,0 +1,209 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_GRAPH_CUT_H_ +#define COLMAP_SRC_BASE_GRAPH_CUT_H_ + +#include +#include + +#include +#include +#include +#include + +#include "util/logging.h" + +namespace colmap { + +// Compute the min-cut of a undirected graph using the Stoer Wagner algorithm. +void ComputeMinGraphCutStoerWagner( + const std::vector>& edges, + const std::vector& weights, int* cut_weight, + std::vector* cut_labels); + +// Compute the normalized min-cut of an undirected graph using Metis. +// Partitions the graph into clusters and returns the cluster labels per vertex. +std::unordered_map ComputeNormalizedMinGraphCut( + const std::vector>& edges, + const std::vector& weights, const int num_parts); + +// Compute the minimum graph cut of a directed S-T graph using the +// Boykov-Kolmogorov max-flow min-cut algorithm, as descibed in: +// "An Experimental Comparison of Min-Cut/Max-Flow Algorithms for Energy +// Minimization in Vision". Yuri Boykov and Vladimir Kolmogorov. PAMI, 2004. +template +class MinSTGraphCut { + public: + typedef boost::adjacency_list_traits + graph_traits_t; + typedef graph_traits_t::edge_descriptor edge_descriptor_t; + typedef graph_traits_t::vertices_size_type vertices_size_t; + + struct Edge { + value_t capacity; + value_t residual; + edge_descriptor_t reverse; + }; + + typedef boost::adjacency_list + graph_t; + + MinSTGraphCut(const size_t num_nodes); + + // Count the number of nodes and edges in the graph. + size_t NumNodes() const; + size_t NumEdges() const; + + // Add node to the graph. + void AddNode(const node_t node_idx, const value_t source_capacity, + const value_t sink_capacity); + + // Add edge to the graph. + void AddEdge(const node_t node_idx1, const node_t node_idx2, + const value_t capacity, const value_t reverse_capacity); + + // Compute the min-cut using the max-flow algorithm. Returns the flow. + value_t Compute(); + + // Check whether node is connected to source or sink after computing the cut. + bool IsConnectedToSource(const node_t node_idx) const; + bool IsConnectedToSink(const node_t node_idx) const; + + private: + const node_t S_node_; + const node_t T_node_; + graph_t graph_; + std::vector colors_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +MinSTGraphCut::MinSTGraphCut(const size_t num_nodes) + : S_node_(num_nodes), T_node_(num_nodes + 1), graph_(num_nodes + 2) {} + +template +size_t MinSTGraphCut::NumNodes() const { + return boost::num_vertices(graph_) - 2; +} + +template +size_t MinSTGraphCut::NumEdges() const { + return boost::num_edges(graph_); +} + +template +void MinSTGraphCut::AddNode(const node_t node_idx, + const value_t source_capacity, + const value_t sink_capacity) { + CHECK_GE(node_idx, 0); + CHECK_LE(node_idx, boost::num_vertices(graph_)); + CHECK_GE(source_capacity, 0); + CHECK_GE(sink_capacity, 0); + + if (source_capacity > 0) { + const edge_descriptor_t edge = + boost::add_edge(S_node_, node_idx, graph_).first; + const edge_descriptor_t edge_reverse = + boost::add_edge(node_idx, S_node_, graph_).first; + graph_[edge].capacity = source_capacity; + graph_[edge].reverse = edge_reverse; + graph_[edge_reverse].reverse = edge; + } + + if (sink_capacity > 0) { + const edge_descriptor_t edge = + boost::add_edge(node_idx, T_node_, graph_).first; + const edge_descriptor_t edge_reverse = + boost::add_edge(T_node_, node_idx, graph_).first; + graph_[edge].capacity = sink_capacity; + graph_[edge].reverse = edge_reverse; + graph_[edge_reverse].reverse = edge; + } +} + +template +void MinSTGraphCut::AddEdge(const node_t node_idx1, + const node_t node_idx2, + const value_t capacity, + const value_t reverse_capacity) { + CHECK_GE(node_idx1, 0); + CHECK_LE(node_idx1, boost::num_vertices(graph_)); + CHECK_GE(node_idx2, 0); + CHECK_LE(node_idx2, boost::num_vertices(graph_)); + CHECK_GE(capacity, 0); + CHECK_GE(reverse_capacity, 0); + + const edge_descriptor_t edge = + boost::add_edge(node_idx1, node_idx2, graph_).first; + const edge_descriptor_t edge_reverse = + boost::add_edge(node_idx2, node_idx1, graph_).first; + graph_[edge].capacity = capacity; + graph_[edge_reverse].capacity = reverse_capacity; + graph_[edge].reverse = edge_reverse; + graph_[edge_reverse].reverse = edge; +} + +template +value_t MinSTGraphCut::Compute() { + const vertices_size_t num_vertices = boost::num_vertices(graph_); + + colors_.resize(num_vertices); + std::vector predecessors(num_vertices); + std::vector distances(num_vertices); + + return boost::boykov_kolmogorov_max_flow( + graph_, boost::get(&Edge::capacity, graph_), + boost::get(&Edge::residual, graph_), boost::get(&Edge::reverse, graph_), + predecessors.data(), colors_.data(), distances.data(), + boost::get(boost::vertex_index, graph_), S_node_, T_node_); +} + +template +bool MinSTGraphCut::IsConnectedToSource( + const node_t node_idx) const { + return colors_.at(node_idx) != boost::white_color; +} + +template +bool MinSTGraphCut::IsConnectedToSink( + const node_t node_idx) const { + return colors_.at(node_idx) == boost::white_color; +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_GRAPH_CUT_H_ diff --git a/colmap/include/colmap/base/homography_matrix.h b/colmap/include/colmap/base/homography_matrix.h new file mode 100644 index 0000000000000000000000000000000000000000..b554240142db739ccfde903d7d3d5cc2ed42ba74 --- /dev/null +++ b/colmap/include/colmap/base/homography_matrix.h @@ -0,0 +1,111 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_HOMOGRAPHY_MATRIX_UTILS_H_ +#define COLMAP_SRC_BASE_HOMOGRAPHY_MATRIX_UTILS_H_ + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Decompose an homography matrix into the possible rotations, translations, +// and plane normal vectors, according to: +// +// Malis, Ezio, and Manuel Vargas. "Deeper understanding of the homography +// decomposition for vision-based control." (2007): 90. +// +// The first pose is assumed to be P = [I | 0]. Note that the homography is +// plane-induced if `R.size() == t.size() == n.size() == 4`. If `R.size() == +// t.size() == n.size() == 1` the homography is pure-rotational. +// +// @param H 3x3 homography matrix. +// @param K 3x3 calibration matrix. +// @param R Possible 3x3 rotation matrices. +// @param t Possible translation vectors. +// @param n Possible normal vectors. +void DecomposeHomographyMatrix(const Eigen::Matrix3d& H, + const Eigen::Matrix3d& K1, + const Eigen::Matrix3d& K2, + std::vector* R, + std::vector* t, + std::vector* n); + +// Recover the most probable pose from the given homography matrix. +// +// The pose of the first image is assumed to be P = [I | 0]. +// +// @param H 3x3 homography matrix. +// @param K1 3x3 calibration matrix of first camera. +// @param K2 3x3 calibration matrix of second camera. +// @param points1 First set of corresponding points. +// @param points2 Second set of corresponding points. +// @param inlier_mask Only points with `true` in the inlier mask are +// considered in the cheirality test. Size of the +// inlier mask must match the number of points N. +// @param R Most probable 3x3 rotation matrix. +// @param t Most probable 3x1 translation vector. +// @param n Most probable 3x1 normal vector. +// @param points3D Triangulated 3D points infront of camera +// (only if homography is not pure-rotational). +void PoseFromHomographyMatrix(const Eigen::Matrix3d& H, + const Eigen::Matrix3d& K1, + const Eigen::Matrix3d& K2, + const std::vector& points1, + const std::vector& points2, + Eigen::Matrix3d* R, Eigen::Vector3d* t, + Eigen::Vector3d* n, + std::vector* points3D); + +// Compose homography matrix from relative pose. +// +// @param K1 3x3 calibration matrix of first camera. +// @param K2 3x3 calibration matrix of second camera. +// @param R Most probable 3x3 rotation matrix. +// @param t Most probable 3x1 translation vector. +// @param n Most probable 3x1 normal vector. +// @param d Orthogonal distance from plane. +// +// @return 3x3 homography matrix. +Eigen::Matrix3d HomographyMatrixFromPose(const Eigen::Matrix3d& K1, + const Eigen::Matrix3d& K2, + const Eigen::Matrix3d& R, + const Eigen::Vector3d& t, + const Eigen::Vector3d& n, + const double d); + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_HOMOGRAPHY_MATRIX_UTILS_H_ diff --git a/colmap/include/colmap/base/image.h b/colmap/include/colmap/base/image.h new file mode 100644 index 0000000000000000000000000000000000000000..b67c0063ed29b7ff3623b848dbdee9ac1784df37 --- /dev/null +++ b/colmap/include/colmap/base/image.h @@ -0,0 +1,364 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_IMAGE_H_ +#define COLMAP_SRC_BASE_IMAGE_H_ + +#include +#include + +#include + +#include "base/camera.h" +#include "base/point2d.h" +#include "base/visibility_pyramid.h" +#include "util/alignment.h" +#include "util/logging.h" +#include "util/math.h" +#include "util/types.h" + +namespace colmap { + +// Class that holds information about an image. An image is the product of one +// camera shot at a certain location (parameterized as the pose). An image may +// share a camera with multiple other images, if its intrinsics are the same. +class Image { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Image(); + + // Setup / tear down the image and necessary internal data structures before + // and after being used in reconstruction. + void SetUp(const Camera& camera); + void TearDown(); + + // Access the unique identifier of the image. + inline image_t ImageId() const; + inline void SetImageId(const image_t image_id); + + // Access the name of the image. + inline const std::string& Name() const; + inline std::string& Name(); + inline void SetName(const std::string& name); + + // Access the unique identifier of the camera. Note that multiple images + // might share the same camera. + inline camera_t CameraId() const; + inline void SetCameraId(const camera_t camera_id); + // Check whether identifier of camera has been set. + inline bool HasCamera() const; + + // Check if image is registered. + inline bool IsRegistered() const; + inline void SetRegistered(const bool registered); + + // Get the number of image points. + inline point2D_t NumPoints2D() const; + + // Get the number of triangulations, i.e. the number of points that + // are part of a 3D point track. + inline point2D_t NumPoints3D() const; + + // Get the number of observations, i.e. the number of image points that + // have at least one correspondence to another image. + inline point2D_t NumObservations() const; + inline void SetNumObservations(const point2D_t num_observations); + + // Get the number of correspondences for all image points. + inline point2D_t NumCorrespondences() const; + inline void SetNumCorrespondences(const point2D_t num_observations); + + // Get the number of observations that see a triangulated point, i.e. the + // number of image points that have at least one correspondence to a + // triangulated point in another image. + inline point2D_t NumVisiblePoints3D() const; + + // Get the score of triangulated observations. In contrast to + // `NumVisiblePoints3D`, this score also captures the distribution + // of triangulated observations in the image. This is useful to select + // the next best image in incremental reconstruction, because a more + // uniform distribution of observations results in more robust registration. + inline size_t Point3DVisibilityScore() const; + + // Access quaternion vector as (qw, qx, qy, qz) specifying the rotation of the + // pose which is defined as the transformation from world to image space. + inline const Eigen::Vector4d& Qvec() const; + inline Eigen::Vector4d& Qvec(); + inline double Qvec(const size_t idx) const; + inline double& Qvec(const size_t idx); + inline void SetQvec(const Eigen::Vector4d& qvec); + + // Quaternion prior, e.g. given by EXIF gyroscope tag. + inline const Eigen::Vector4d& QvecPrior() const; + inline Eigen::Vector4d& QvecPrior(); + inline double QvecPrior(const size_t idx) const; + inline double& QvecPrior(const size_t idx); + inline bool HasQvecPrior() const; + inline void SetQvecPrior(const Eigen::Vector4d& qvec); + + // Access translation vector as (tx, ty, tz) specifying the translation of the + // pose which is defined as the transformation from world to image space. + inline const Eigen::Vector3d& Tvec() const; + inline Eigen::Vector3d& Tvec(); + inline double Tvec(const size_t idx) const; + inline double& Tvec(const size_t idx); + inline void SetTvec(const Eigen::Vector3d& tvec); + + // Translation prior, e.g. given by EXIF GPS tag. + inline const Eigen::Vector3d& TvecPrior() const; + inline Eigen::Vector3d& TvecPrior(); + inline double TvecPrior(const size_t idx) const; + inline double& TvecPrior(const size_t idx); + inline bool HasTvecPrior() const; + inline void SetTvecPrior(const Eigen::Vector3d& tvec); + + // Access the coordinates of image points. + inline const class Point2D& Point2D(const point2D_t point2D_idx) const; + inline class Point2D& Point2D(const point2D_t point2D_idx); + inline const std::vector& Points2D() const; + void SetPoints2D(const std::vector& points); + void SetPoints2D(const std::vector& points); + + // Set the point as triangulated, i.e. it is part of a 3D point track. + void SetPoint3DForPoint2D(const point2D_t point2D_idx, + const point3D_t point3D_id); + + // Set the point as not triangulated, i.e. it is not part of a 3D point track. + void ResetPoint3DForPoint2D(const point2D_t point2D_idx); + + // Check whether an image point has a correspondence to an image point in + // another image that has a 3D point. + inline bool IsPoint3DVisible(const point2D_t point2D_idx) const; + + // Check whether one of the image points is part of the 3D point track. + bool HasPoint3D(const point3D_t point3D_id) const; + + // Indicate that another image has a point that is triangulated and has + // a correspondence to this image point. Note that this must only be called + // after calling `SetUp`. + void IncrementCorrespondenceHasPoint3D(const point2D_t point2D_idx); + + // Indicate that another image has a point that is not triangulated any more + // and has a correspondence to this image point. This assumes that + // `IncrementCorrespondenceHasPoint3D` was called for the same image point + // and correspondence before. Note that this must only be called + // after calling `SetUp`. + void DecrementCorrespondenceHasPoint3D(const point2D_t point2D_idx); + + // Normalize the quaternion vector. + void NormalizeQvec(); + + // Compose the projection matrix from world to image space. + Eigen::Matrix3x4d ProjectionMatrix() const; + + // Compose the inverse projection matrix from image to world space + Eigen::Matrix3x4d InverseProjectionMatrix() const; + + // Compose rotation matrix from quaternion vector. + Eigen::Matrix3d RotationMatrix() const; + + // Extract the projection center in world space. + Eigen::Vector3d ProjectionCenter() const; + + // Extract the viewing direction of the image. + Eigen::Vector3d ViewingDirection() const; + + // The number of levels in the 3D point multi-resolution visibility pyramid. + static const int kNumPoint3DVisibilityPyramidLevels; + + private: + // Identifier of the image, if not specified `kInvalidImageId`. + image_t image_id_; + + // The name of the image, i.e. the relative path. + std::string name_; + + // The identifier of the associated camera. Note that multiple images might + // share the same camera. If not specified `kInvalidCameraId`. + camera_t camera_id_; + + // Whether the image is successfully registered in the reconstruction. + bool registered_; + + // The number of 3D points the image observes, i.e. the sum of its `points2D` + // where `point3D_id != kInvalidPoint3DId`. + point2D_t num_points3D_; + + // The number of image points that have at least one correspondence to + // another image. + point2D_t num_observations_; + + // The sum of correspondences per image point. + point2D_t num_correspondences_; + + // The number of 2D points, which have at least one corresponding 2D point in + // another image that is part of a 3D point track, i.e. the sum of `points2D` + // where `num_tris > 0`. + point2D_t num_visible_points3D_; + + // The pose of the image, defined as the transformation from world to image. + Eigen::Vector4d qvec_; + Eigen::Vector3d tvec_; + + // The pose prior of the image, e.g. extracted from EXIF tags. + Eigen::Vector4d qvec_prior_; + Eigen::Vector3d tvec_prior_; + + // All image points, including points that are not part of a 3D point track. + std::vector points2D_; + + // Per image point, the number of correspondences that have a 3D point. + std::vector num_correspondences_have_point3D_; + + // Data structure to compute the distribution of triangulated correspondences + // in the image. Note that this structure is only usable after `SetUp`. + VisibilityPyramid point3D_visibility_pyramid_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +image_t Image::ImageId() const { return image_id_; } + +void Image::SetImageId(const image_t image_id) { image_id_ = image_id; } + +const std::string& Image::Name() const { return name_; } + +std::string& Image::Name() { return name_; } + +void Image::SetName(const std::string& name) { name_ = name; } + +inline camera_t Image::CameraId() const { return camera_id_; } + +inline void Image::SetCameraId(const camera_t camera_id) { + CHECK_NE(camera_id, kInvalidCameraId); + camera_id_ = camera_id; +} + +inline bool Image::HasCamera() const { return camera_id_ != kInvalidCameraId; } + +bool Image::IsRegistered() const { return registered_; } + +void Image::SetRegistered(const bool registered) { registered_ = registered; } + +point2D_t Image::NumPoints2D() const { + return static_cast(points2D_.size()); +} + +point2D_t Image::NumPoints3D() const { return num_points3D_; } + +point2D_t Image::NumObservations() const { return num_observations_; } + +void Image::SetNumObservations(const point2D_t num_observations) { + num_observations_ = num_observations; +} + +point2D_t Image::NumCorrespondences() const { return num_correspondences_; } + +void Image::SetNumCorrespondences(const point2D_t num_correspondences) { + num_correspondences_ = num_correspondences; +} + +point2D_t Image::NumVisiblePoints3D() const { return num_visible_points3D_; } + +size_t Image::Point3DVisibilityScore() const { + return point3D_visibility_pyramid_.Score(); +} + +const Eigen::Vector4d& Image::Qvec() const { return qvec_; } + +Eigen::Vector4d& Image::Qvec() { return qvec_; } + +inline double Image::Qvec(const size_t idx) const { return qvec_(idx); } + +inline double& Image::Qvec(const size_t idx) { return qvec_(idx); } + +void Image::SetQvec(const Eigen::Vector4d& qvec) { qvec_ = qvec; } + +const Eigen::Vector4d& Image::QvecPrior() const { return qvec_prior_; } + +Eigen::Vector4d& Image::QvecPrior() { return qvec_prior_; } + +inline double Image::QvecPrior(const size_t idx) const { + return qvec_prior_(idx); +} + +inline double& Image::QvecPrior(const size_t idx) { return qvec_prior_(idx); } + +inline bool Image::HasQvecPrior() const { return !IsNaN(qvec_prior_.sum()); } + +void Image::SetQvecPrior(const Eigen::Vector4d& qvec) { qvec_prior_ = qvec; } + +const Eigen::Vector3d& Image::Tvec() const { return tvec_; } + +Eigen::Vector3d& Image::Tvec() { return tvec_; } + +inline double Image::Tvec(const size_t idx) const { return tvec_(idx); } + +inline double& Image::Tvec(const size_t idx) { return tvec_(idx); } + +void Image::SetTvec(const Eigen::Vector3d& tvec) { tvec_ = tvec; } + +const Eigen::Vector3d& Image::TvecPrior() const { return tvec_prior_; } + +Eigen::Vector3d& Image::TvecPrior() { return tvec_prior_; } + +inline double Image::TvecPrior(const size_t idx) const { + return tvec_prior_(idx); +} + +inline double& Image::TvecPrior(const size_t idx) { return tvec_prior_(idx); } + +inline bool Image::HasTvecPrior() const { return !IsNaN(tvec_prior_.sum()); } + +void Image::SetTvecPrior(const Eigen::Vector3d& tvec) { tvec_prior_ = tvec; } + +const class Point2D& Image::Point2D(const point2D_t point2D_idx) const { + return points2D_.at(point2D_idx); +} + +class Point2D& Image::Point2D(const point2D_t point2D_idx) { + return points2D_.at(point2D_idx); +} + +const std::vector& Image::Points2D() const { return points2D_; } + +bool Image::IsPoint3DVisible(const point2D_t point2D_idx) const { + return num_correspondences_have_point3D_.at(point2D_idx) > 0; +} + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::Image) + +#endif // COLMAP_SRC_BASE_IMAGE_H_ diff --git a/colmap/include/colmap/base/image_reader.h b/colmap/include/colmap/base/image_reader.h new file mode 100644 index 0000000000000000000000000000000000000000..4329ab342ddeecbc652bdab196e7cbf5e7b71d1a --- /dev/null +++ b/colmap/include/colmap/base/image_reader.h @@ -0,0 +1,133 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_IMAGE_READER_H_ +#define COLMAP_SRC_BASE_IMAGE_READER_H_ + +#include + +#include "base/database.h" +#include "util/bitmap.h" +#include "util/threading.h" + +namespace colmap { + +struct ImageReaderOptions { + // Path to database in which to store the extracted data. + std::string database_path = ""; + + // Root path to folder which contains the images. + std::string image_path = ""; + + // Optional root path to folder which contains image masks. For a given image, + // the corresponding mask must have the same sub-path below this root as the + // image has below image_path. The filename must be equal, aside from the + // added extension .png. For example, for an image image_path/abc/012.jpg, the + // mask would be mask_path/abc/012.jpg.png. No features will be extracted in + // regions where the mask image is black (pixel intensity value 0 in + // grayscale). + std::string mask_path = ""; + + // Optional list of images to read. The list must contain the relative path + // of the images with respect to the image_path. + std::vector image_list; + + // Name of the camera model. + std::string camera_model = "SIMPLE_RADIAL"; + + // Whether to use the same camera for all images. + bool single_camera = false; + + // Whether to use the same camera for all images in the same sub-folder. + bool single_camera_per_folder = false; + + // Whether to use a different camera for each image. + bool single_camera_per_image = false; + + // Whether to explicitly use an existing camera for all images. Note that in + // this case the specified camera model and parameters are ignored. + int existing_camera_id = kInvalidCameraId; + + // Manual specification of camera parameters. If empty, camera parameters + // will be extracted from EXIF, i.e. principal point and focal length. + std::string camera_params = ""; + + // If camera parameters are not specified manually and the image does not + // have focal length EXIF information, the focal length is set to the + // value `default_focal_length_factor * max(width, height)`. + double default_focal_length_factor = 1.2; + + // Optional path to an image file specifying a mask for all images. No + // features will be extracted in regions where the mask is black (pixel + // intensity value 0 in grayscale). + std::string camera_mask_path = ""; + + bool Check() const; +}; + +// Recursively iterate over the images in a directory. Skips an image if it +// already exists in the database. Extracts the camera intrinsics from EXIF and +// writes the camera information to the database. +class ImageReader { + public: + enum class Status { + FAILURE, + SUCCESS, + IMAGE_EXISTS, + BITMAP_ERROR, + CAMERA_SINGLE_DIM_ERROR, + CAMERA_EXIST_DIM_ERROR, + CAMERA_PARAM_ERROR + }; + + ImageReader(const ImageReaderOptions& options, Database* database); + + Status Next(Camera* camera, Image* image, Bitmap* bitmap, Bitmap* mask); + size_t NextIndex() const; + size_t NumImages() const; + + private: + // Image reader options. + ImageReaderOptions options_; + Database* database_; + // Index of previously processed image. + size_t image_index_; + // Previously processed camera. + Camera prev_camera_; + std::unordered_map camera_model_to_id_; + // Names of image sub-folders. + std::string prev_image_folder_; + std::unordered_set image_folders_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_IMAGE_READER_H_ diff --git a/colmap/include/colmap/base/line.h b/colmap/include/colmap/base/line.h new file mode 100644 index 0000000000000000000000000000000000000000..3fdf3b9cce04f6c4fef8eb5e004bd6e6d9e072a5 --- /dev/null +++ b/colmap/include/colmap/base/line.h @@ -0,0 +1,66 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_LINE_H_ +#define COLMAP_SRC_BASE_LINE_H_ + +#include + +#include "util/alignment.h" +#include "util/bitmap.h" + +namespace colmap { + +struct LineSegment { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::Vector2d start; + Eigen::Vector2d end; +}; + +enum class LineSegmentOrientation { + HORIZONTAL = 1, + VERTICAL = -1, + UNDEFINED = 0, +}; + +// Detect line segments in the given bitmap image. +std::vector DetectLineSegments(const Bitmap& bitmap, + const double min_length = 3); + +// Classify line segments into horizontal/vertical. +std::vector ClassifyLineSegmentOrientations( + const std::vector& segments, const double tolerance = 0.25); + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::LineSegment) + +#endif // COLMAP_SRC_BASE_LINE_H_ diff --git a/colmap/include/colmap/base/point2d.h b/colmap/include/colmap/base/point2d.h new file mode 100644 index 0000000000000000000000000000000000000000..b7a6c041247cfadb5caa03f63a64dad73d7bb448 --- /dev/null +++ b/colmap/include/colmap/base/point2d.h @@ -0,0 +1,98 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_POINT2D_H_ +#define COLMAP_SRC_BASE_POINT2D_H_ + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// 2D point class corresponds to a feature in an image. It may or may not have a +// corresponding 3D point if it is part of a triangulated track. +class Point2D { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Point2D(); + + // The coordinate in image space in pixels. + inline const Eigen::Vector2d& XY() const; + inline Eigen::Vector2d& XY(); + inline double X() const; + inline double Y() const; + inline void SetXY(const Eigen::Vector2d& xy); + + // The identifier of the observed 3D point. If the image point does not + // observe a 3D point, the identifier is `kInvalidPoint3Did`. + inline point3D_t Point3DId() const; + inline bool HasPoint3D() const; + inline void SetPoint3DId(const point3D_t point3D_id); + + private: + // The image coordinates in pixels, starting at upper left corner with 0. + Eigen::Vector2d xy_; + + // The identifier of the 3D point. If the 2D point is not part of a 3D point + // track the identifier is `kInvalidPoint3DId` and `HasPoint3D() = false`. + point3D_t point3D_id_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +const Eigen::Vector2d& Point2D::XY() const { return xy_; } + +Eigen::Vector2d& Point2D::XY() { return xy_; } + +double Point2D::X() const { return xy_.x(); } + +double Point2D::Y() const { return xy_.y(); } + +void Point2D::SetXY(const Eigen::Vector2d& xy) { xy_ = xy; } + +point3D_t Point2D::Point3DId() const { return point3D_id_; } + +bool Point2D::HasPoint3D() const { return point3D_id_ != kInvalidPoint3DId; } + +void Point2D::SetPoint3DId(const point3D_t point3D_id) { + point3D_id_ = point3D_id; +} + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::Point2D) + +#endif // COLMAP_SRC_BASE_POINT2D_H_ diff --git a/colmap/include/colmap/base/point3d.h b/colmap/include/colmap/base/point3d.h new file mode 100644 index 0000000000000000000000000000000000000000..4cc31e858382144ddc4f66c01fe2aa9a0ff611ff --- /dev/null +++ b/colmap/include/colmap/base/point3d.h @@ -0,0 +1,140 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_POINT3D_H_ +#define COLMAP_SRC_BASE_POINT3D_H_ + +#include + +#include + +#include "base/track.h" +#include "util/logging.h" +#include "util/types.h" + +namespace colmap { + +// 3D point class that holds information about triangulated 2D points. +class Point3D { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Point3D(); + + // The point coordinate in world space. + inline const Eigen::Vector3d& XYZ() const; + inline Eigen::Vector3d& XYZ(); + inline double XYZ(const size_t idx) const; + inline double& XYZ(const size_t idx); + inline double X() const; + inline double Y() const; + inline double Z() const; + inline void SetXYZ(const Eigen::Vector3d& xyz); + + // The RGB color of the point. + inline const Eigen::Vector3ub& Color() const; + inline Eigen::Vector3ub& Color(); + inline uint8_t Color(const size_t idx) const; + inline uint8_t& Color(const size_t idx); + inline void SetColor(const Eigen::Vector3ub& color); + + // The mean reprojection error in image space. + inline double Error() const; + inline bool HasError() const; + inline void SetError(const double error); + + inline const class Track& Track() const; + inline class Track& Track(); + inline void SetTrack(class Track track); + + private: + // The 3D position of the point. + Eigen::Vector3d xyz_; + + // The color of the point in the range [0, 255]. + Eigen::Vector3ub color_; + + // The mean reprojection error in pixels. + double error_; + + // The track of the point as a list of image observations. + class Track track_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +const Eigen::Vector3d& Point3D::XYZ() const { return xyz_; } + +Eigen::Vector3d& Point3D::XYZ() { return xyz_; } + +double Point3D::XYZ(const size_t idx) const { return xyz_(idx); } + +double& Point3D::XYZ(const size_t idx) { return xyz_(idx); } + +double Point3D::X() const { return xyz_.x(); } + +double Point3D::Y() const { return xyz_.y(); } + +double Point3D::Z() const { return xyz_.z(); } + +void Point3D::SetXYZ(const Eigen::Vector3d& xyz) { xyz_ = xyz; } + +const Eigen::Vector3ub& Point3D::Color() const { return color_; } + +Eigen::Vector3ub& Point3D::Color() { return color_; } + +uint8_t Point3D::Color(const size_t idx) const { return color_(idx); } + +uint8_t& Point3D::Color(const size_t idx) { return color_(idx); } + +void Point3D::SetColor(const Eigen::Vector3ub& color) { color_ = color; } + +double Point3D::Error() const { return error_; } + +bool Point3D::HasError() const { return error_ != -1.0; } + +void Point3D::SetError(const double error) { error_ = error; } + +const class Track& Point3D::Track() const { return track_; } + +class Track& Point3D::Track() { + return track_; +} + +void Point3D::SetTrack(class Track track) { track_ = std::move(track); } + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::Point3D) + +#endif // COLMAP_SRC_BASE_POINT3D_H_ diff --git a/colmap/include/colmap/base/polynomial.h b/colmap/include/colmap/base/polynomial.h new file mode 100644 index 0000000000000000000000000000000000000000..8f002f62b8de4ce499124ed1d87c75872c0d368a --- /dev/null +++ b/colmap/include/colmap/base/polynomial.h @@ -0,0 +1,101 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_POLYNOMIAL_H_ +#define COLMAP_SRC_BASE_POLYNOMIAL_H_ + +#include + +namespace colmap { + +// All polynomials are assumed to be the form: +// +// sum_{i=0}^N polynomial(i) x^{N-i}. +// +// and are given by a vector of coefficients of size N + 1. +// +// The implementation is based on COLMAP's old polynomial functionality and is +// inspired by Ceres-Solver's/Theia's implementation to support complex +// polynomials. The companion matrix implementation is based on NumPy. + +// Evaluate the polynomial for the given coefficients at x using the Horner +// scheme. This function is templated such that the polynomial may be evaluated +// at real and/or imaginary points. +template +T EvaluatePolynomial(const Eigen::VectorXd& coeffs, const T& x); + +// Find the root of polynomials of the form: a * x + b = 0. +// The real and/or imaginary variable may be NULL if the output is not needed. +bool FindLinearPolynomialRoots(const Eigen::VectorXd& coeffs, + Eigen::VectorXd* real, Eigen::VectorXd* imag); + +// Find the roots of polynomials of the form: a * x^2 + b * x + c = 0. +// The real and/or imaginary variable may be NULL if the output is not needed. +bool FindQuadraticPolynomialRoots(const Eigen::VectorXd& coeffs, + Eigen::VectorXd* real, Eigen::VectorXd* imag); + +// Find the roots of a polynomial using the Durand-Kerner method, based on: +// +// https://en.wikipedia.org/wiki/Durand%E2%80%93Kerner_method +// +// The Durand-Kerner is comparatively fast but often unstable/inaccurate. +// The real and/or imaginary variable may be NULL if the output is not needed. +bool FindPolynomialRootsDurandKerner(const Eigen::VectorXd& coeffs, + Eigen::VectorXd* real, + Eigen::VectorXd* imag); + +// Find the roots of a polynomial using the companion matrix method, based on: +// +// R. A. Horn & C. R. Johnson, Matrix Analysis. Cambridge, +// UK: Cambridge University Press, 1999, pp. 146-7. +// +// Compared to Durand-Kerner, this method is slower but more stable/accurate. +// The real and/or imaginary variable may be NULL if the output is not needed. +bool FindPolynomialRootsCompanionMatrix(const Eigen::VectorXd& coeffs, + Eigen::VectorXd* real, + Eigen::VectorXd* imag); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +T EvaluatePolynomial(const Eigen::VectorXd& coeffs, const T& x) { + T value = 0.0; + for (Eigen::VectorXd::Index i = 0; i < coeffs.size(); ++i) { + value = value * x + coeffs(i); + } + return value; +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_POLYNOMIAL_H_ diff --git a/colmap/include/colmap/base/pose.h b/colmap/include/colmap/base/pose.h new file mode 100644 index 0000000000000000000000000000000000000000..7eca7721ae50041abf079fdb03f97a86e0aa231f --- /dev/null +++ b/colmap/include/colmap/base/pose.h @@ -0,0 +1,230 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_POSE_H_ +#define COLMAP_SRC_BASE_POSE_H_ + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Compose the skew symmetric cross product matrix from a vector. +Eigen::Matrix3d CrossProductMatrix(const Eigen::Vector3d& vector); + +// Convert 3D rotation matrix to Euler angles. +// +// The convention `R = Rx * Ry * Rz` is used, +// using a right-handed coordinate system. +// +// @param R 3x3 rotation matrix. +// @param rx, ry, rz Euler angles in radians. +void RotationMatrixToEulerAngles(const Eigen::Matrix3d& R, double* rx, + double* ry, double* rz); + +// Convert Euler angles to 3D rotation matrix. +// +// The convention `R = Rz * Ry * Rx` is used, +// using a right-handed coordinate system. +// +// @param rx, ry, rz Euler angles in radians. +// +// @return 3x3 rotation matrix. +Eigen::Matrix3d EulerAnglesToRotationMatrix(const double rx, const double ry, + const double rz); + +// Convert 3D rotation matrix to Quaternion representation. +// +// @param rot_mat 3x3 rotation matrix. +// +// @return Unit Quaternion rotation coefficients (w, x, y, z). +Eigen::Vector4d RotationMatrixToQuaternion(const Eigen::Matrix3d& rot_mat); + +// Convert Quaternion representation to 3D rotation matrix. +// +// @param qvec Unit Quaternion rotation coefficients (w, x, y, z). +// +// @return 3x3 rotation matrix. +Eigen::Matrix3d QuaternionToRotationMatrix(const Eigen::Vector4d& qvec); + +// Compose the Quaternion vector corresponding to a identity transformation. +inline Eigen::Vector4d ComposeIdentityQuaternion(); + +// Normalize Quaternion vector. +// +// @param qvec Quaternion rotation coefficients (w, x, y, z). +// +// @return Unit Quaternion rotation coefficients (w, x, y, z). +Eigen::Vector4d NormalizeQuaternion(const Eigen::Vector4d& qvec); + +// Invert Quaternion vector to return Quaternion of inverse rotation. +// +// @param qvec Quaternion rotation coefficients (w, x, y, z). +// +// @return Inverse Quaternion rotation coefficients (w, x, y, z). +Eigen::Vector4d InvertQuaternion(const Eigen::Vector4d& qvec); + +// Concatenate Quaternion rotations such that the rotation of `qvec1` is applied +// before the rotation of `qvec2`. +// +// @param qvec1 Quaternion rotation coefficients (w, x, y, z). +// @param qvec2 Quaternion rotation coefficients (w, x, y, z). +// +// @return Concatenated Quaternion coefficients (w, x, y, z). +Eigen::Vector4d ConcatenateQuaternions(const Eigen::Vector4d& qvec1, + const Eigen::Vector4d& qvec2); + +// Transform point by quaternion rotation. +// +// @param qvec Quaternion rotation coefficients (w, x, y, z). +// @param point Point to rotate. +// +// @return Rotated point. +Eigen::Vector3d QuaternionRotatePoint(const Eigen::Vector4d& qvec, + const Eigen::Vector3d& point); + +// Compute the weighted average of multiple Quaternions according to: +// +// Markley, F. Landis, et al. "Averaging quaternions." +// Journal of Guidance, Control, and Dynamics 30.4 (2007): 1193-1197. +// +// @param qvecs The Quaternions to be averaged. +// @param weights Non-negative weights. +// +// @return The average Quaternion. +Eigen::Vector4d AverageQuaternions(const std::vector& qvecs, + const std::vector& weights); + +// Compose rotation matrix that rotates unit vector 1 to unit vector 2. +// Note that when vector 1 points into the opposite direction of vector 2, +// the function returns an identity rotation. +Eigen::Matrix3d RotationFromUnitVectors(const Eigen::Vector3d& vec1, + const Eigen::Vector3d& vec2); + +// Extract camera projection center from projection matrix, i.e. the projection +// center in world coordinates `-R^T t`. +Eigen::Vector3d ProjectionCenterFromMatrix( + const Eigen::Matrix3x4d& proj_matrix); + +// Extract camera projection center from projection parameters. +// +// @param qvec Unit Quaternion rotation coefficients (w, x, y, z). +// @param tvec 3x1 translation vector. +// +// @return 3x1 camera projection center. +Eigen::Vector3d ProjectionCenterFromPose(const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec); + +// Compute the relative transformation from pose 1 to 2. +// +// @param qvec1, tvec1 First camera pose. +// @param qvec2, tvec2 Second camera pose. +// @param qvec12, tvec12 Relative pose. +void ComputeRelativePose(const Eigen::Vector4d& qvec1, + const Eigen::Vector3d& tvec1, + const Eigen::Vector4d& qvec2, + const Eigen::Vector3d& tvec2, Eigen::Vector4d* qvec12, + Eigen::Vector3d* tvec12); + +// Concatenate the transformations of the two poses. +// +// @param qvec1, tvec1 First camera pose. +// @param qvec2, tvec2 Second camera pose. +// @param qvec12, tvec12 Concatenated pose. +void ConcatenatePoses(const Eigen::Vector4d& qvec1, + const Eigen::Vector3d& tvec1, + const Eigen::Vector4d& qvec2, + const Eigen::Vector3d& tvec2, Eigen::Vector4d* qvec12, + Eigen::Vector3d* tvec12); + +// Invert transformation of the pose. +// @param qvec, tvec Input camera pose. +// @param inv_qvec, inv_tvec Inverse camera pose. +void InvertPose(const Eigen::Vector4d& qvec, const Eigen::Vector3d& tvec, + Eigen::Vector4d* inv_qvec, Eigen::Vector3d* inv_tvec); + +// Linearly interpolate camera pose. +// +// @param qvec1, tvec1 Camera pose at t0 = 0. +// @param qvec2, tvec2 Camera pose at t1 = 1. +// @param t Interpolation time. +// @param qveci, tveci Camera pose at time t. +void InterpolatePose(const Eigen::Vector4d& qvec1, const Eigen::Vector3d& tvec1, + const Eigen::Vector4d& qvec2, const Eigen::Vector3d& tvec2, + const double t, Eigen::Vector4d* qveci, + Eigen::Vector3d* tveci); + +// Calculate baseline vector from first to second pose. +// +// The given rotation and orientation is expected as the +// world to camera transformation. +// +// @param qvec1 Unit Quaternion rotation coefficients (w, x, y, z). +// @param tvec1 3x1 translation vector. +// @param qvec2 Unit Quaternion rotation coefficients (w, x, y, z). +// @param tvec2 3x1 translation vector. +// +// @return Baseline vector from 1 to 2. +Eigen::Vector3d CalculateBaseline(const Eigen::Vector4d& qvec1, + const Eigen::Vector3d& tvec1, + const Eigen::Vector4d& qvec2, + const Eigen::Vector3d& tvec2); + +// Perform cheirality constraint test, i.e., determine which of the triangulated +// correspondences lie in front of of both cameras. The first camera has the +// projection matrix P1 = [I | 0] and the second camera has the projection +// matrix P2 = [R | t]. +// +// @param R 3x3 rotation matrix of second projection matrix. +// @param t 3x1 translation vector of second projection matrix. +// @param points1 First set of corresponding points. +// @param points2 Second set of corresponding points. +// @param points3D Points that lie in front of both cameras. +bool CheckCheirality(const Eigen::Matrix3d& R, const Eigen::Vector3d& t, + const std::vector& points1, + const std::vector& points2, + std::vector* points3D); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +Eigen::Vector4d ComposeIdentityQuaternion() { + return Eigen::Vector4d(1, 0, 0, 0); +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_POSE_H_ diff --git a/colmap/include/colmap/base/projection.h b/colmap/include/colmap/base/projection.h new file mode 100644 index 0000000000000000000000000000000000000000..ef6e5592fbc005aed59d63d46165bab399676dc1 --- /dev/null +++ b/colmap/include/colmap/base/projection.h @@ -0,0 +1,167 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_PROJECTION_H_ +#define COLMAP_SRC_BASE_PROJECTION_H_ + +#include +#include + +#include +#include + +#include "base/camera.h" + +namespace colmap { + +// Compose projection matrix from rotation and translation components. +// +// The projection matrix transforms 3D world to image points. +// +// @param qvec Unit Quaternion rotation coefficients (w, x, y, z). +// @param tvec 3x1 translation vector. +// +// @return 3x4 projection matrix. +Eigen::Matrix3x4d ComposeProjectionMatrix(const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec); + +// Compose projection matrix from rotation matrix and translation components). +// +// The projection matrix transforms 3D world to image points. +// +// @param R 3x3 rotation matrix. +// @param t 3x1 translation vector. +// +// @return 3x4 projection matrix. +Eigen::Matrix3x4d ComposeProjectionMatrix(const Eigen::Matrix3d& R, + const Eigen::Vector3d& T); + +// Invert projection matrix, defined as: +// +// P = [R | t] with R \in SO(3) and t \in R^3 +// +// and the inverse projection matrix as: +// +// P' = [R^T | -R^T t] +// +// @param proj_matrix 3x4 projection matrix. +// +// @return 3x4 inverse projection matrix. +Eigen::Matrix3x4d InvertProjectionMatrix(const Eigen::Matrix3x4d& proj_matrix); + +// Compute the closes rotation matrix with the closest Frobenius norm by setting +// the singular values of the given matrix to 1. +Eigen::Matrix3d ComputeClosestRotationMatrix(const Eigen::Matrix3d& matrix); + +// Decompose projection matrix into intrinsic camera matrix, rotation matrix and +// translation vector. Returns false if decomposition fails. This implementation +// is inspired by the OpenCV implementation with some additional checks. +bool DecomposeProjectionMatrix(const Eigen::Matrix3x4d& proj_matrix, + Eigen::Matrix3d* K, Eigen::Matrix3d* R, + Eigen::Vector3d* T); + +// Project 3D point to image. +// +// @param points3D 3D world point as 3x1 vector. +// @param proj_matrix 3x4 projection matrix. +// @param camera Camera used to project to image plane. +// +// @return Projected image point. +Eigen::Vector2d ProjectPointToImage(const Eigen::Vector3d& point3D, + const Eigen::Matrix3x4d& proj_matrix, + const Camera& camera); + +// Calculate the reprojection error. +// +// The reprojection error is the Euclidean distance between the observation +// in the image and the projection of the 3D point into the image. If the +// 3D point is behind the camera, then this function returns DBL_MAX. +double CalculateSquaredReprojectionError(const Eigen::Vector2d& point2D, + const Eigen::Vector3d& point3D, + const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec, + const Camera& camera); +double CalculateSquaredReprojectionError(const Eigen::Vector2d& point2D, + const Eigen::Vector3d& point3D, + const Eigen::Matrix3x4d& proj_matrix, + const Camera& camera); + +// Calculate the angular error. +// +// The angular error is the angle between the observed viewing ray and the +// actual viewing ray from the camera center to the 3D point. +double CalculateAngularError(const Eigen::Vector2d& point2D, + const Eigen::Vector3d& point3D, + const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec, const Camera& camera); +double CalculateAngularError(const Eigen::Vector2d& point2D, + const Eigen::Vector3d& point3D, + const Eigen::Matrix3x4d& proj_matrix, + const Camera& camera); + +// Calculate angulate error using normalized image points. +// +// The angular error is the angle between the observed viewing ray and the +// actual viewing ray from the camera center to the 3D point. +double CalculateNormalizedAngularError(const Eigen::Vector2d& point2D, + const Eigen::Vector3d& point3D, + const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec); +double CalculateNormalizedAngularError(const Eigen::Vector2d& point2D, + const Eigen::Vector3d& point3D, + const Eigen::Matrix3x4d& proj_matrix); + +// Calculate depth of 3D point with respect to camera. +// +// The depth is defined as the Euclidean distance of a 3D point from the +// camera and is positive if the 3D point is in front and negative if +// behind of the camera. +// +// @param proj_matrix 3x4 projection matrix. +// @param point3D 3D point as 3x1 vector. +// +// @return Depth of 3D point. +double CalculateDepth(const Eigen::Matrix3x4d& proj_matrix, + const Eigen::Vector3d& point3D); + +// Check if 3D point passes cheirality constraint, +// i.e. it lies in front of the camera and not in the image plane. +// +// @param proj_matrix 3x4 projection matrix. +// @param point3D 3D point as 3x1 vector. +// +// @return True if point lies in front of camera. +bool HasPointPositiveDepth(const Eigen::Matrix3x4d& proj_matrix, + const Eigen::Vector3d& point3D); + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_PROJECTION_H_ diff --git a/colmap/include/colmap/base/reconstruction.h b/colmap/include/colmap/base/reconstruction.h new file mode 100644 index 0000000000000000000000000000000000000000..035e5191184d2582dbef07dae558ce033d5bf6f0 --- /dev/null +++ b/colmap/include/colmap/base/reconstruction.h @@ -0,0 +1,651 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_RECONSTRUCTION_H_ +#define COLMAP_SRC_BASE_RECONSTRUCTION_H_ + +#include +#include +#include +#include + +#include + +#include "base/camera.h" +#include "base/database.h" +#include "base/image.h" +#include "base/point2d.h" +#include "base/point3d.h" +#include "base/similarity_transform.h" +#include "base/track.h" +#include "estimators/similarity_transform.h" +#include "optim/loransac.h" +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +struct PlyPoint; +struct RANSACOptions; +class DatabaseCache; +class CorrespondenceGraph; + +// Reconstruction class holds all information about a single reconstructed +// model. It is used by the mapping and bundle adjustment classes and can be +// written to and read from disk. +class Reconstruction { + public: + struct ImagePairStat { + // The number of triangulated correspondences between two images. + size_t num_tri_corrs = 0; + // The number of total correspondences/matches between two images. + size_t num_total_corrs = 0; + }; + + Reconstruction(); + + // Get number of objects. + inline size_t NumCameras() const; + inline size_t NumImages() const; + inline size_t NumRegImages() const; + inline size_t NumPoints3D() const; + inline size_t NumImagePairs() const; + + // Get const objects. + inline const class Camera& Camera(const camera_t camera_id) const; + inline const class Image& Image(const image_t image_id) const; + inline const class Point3D& Point3D(const point3D_t point3D_id) const; + inline const ImagePairStat& ImagePair(const image_pair_t pair_id) const; + inline ImagePairStat& ImagePair(const image_t image_id1, + const image_t image_id2); + + // Get mutable objects. + inline class Camera& Camera(const camera_t camera_id); + inline class Image& Image(const image_t image_id); + inline class Point3D& Point3D(const point3D_t point3D_id); + inline ImagePairStat& ImagePair(const image_pair_t pair_id); + inline const ImagePairStat& ImagePair(const image_t image_id1, + const image_t image_id2) const; + + // Get reference to all objects. + inline const EIGEN_STL_UMAP(camera_t, class Camera) & Cameras() const; + inline const EIGEN_STL_UMAP(image_t, class Image) & Images() const; + inline const std::vector& RegImageIds() const; + inline const EIGEN_STL_UMAP(point3D_t, class Point3D) & Points3D() const; + inline const std::unordered_map& ImagePairs() + const; + + // Identifiers of all 3D points. + std::unordered_set Point3DIds() const; + + // Check whether specific object exists. + inline bool ExistsCamera(const camera_t camera_id) const; + inline bool ExistsImage(const image_t image_id) const; + inline bool ExistsPoint3D(const point3D_t point3D_id) const; + inline bool ExistsImagePair(const image_pair_t pair_id) const; + + // Load data from given `DatabaseCache`. + void Load(const DatabaseCache& database_cache); + + // Setup all relevant data structures before reconstruction. Note the + // correspondence graph object must live until `TearDown` is called. + void SetUp(const CorrespondenceGraph* correspondence_graph); + + // Finalize the Reconstruction after the reconstruction has finished. + // + // Once a scene has been finalized, it cannot be used for reconstruction. + // + // This removes all not yet registered images and unused cameras, in order to + // save memory. + void TearDown(); + + // Add new camera. There is only one camera per image, while multiple images + // might be taken by the same camera. + void AddCamera(class Camera camera); + + // Add new image. + void AddImage(class Image image); + + // Add new 3D object, and return its unique ID. + point3D_t AddPoint3D( + const Eigen::Vector3d& xyz, Track track, + const Eigen::Vector3ub& color = Eigen::Vector3ub::Zero()); + + // Add observation to existing 3D point. + void AddObservation(const point3D_t point3D_id, const TrackElement& track_el); + + // Merge two 3D points and return new identifier of new 3D point. + // The location of the merged 3D point is a weighted average of the two + // original 3D point's locations according to their track lengths. + point3D_t MergePoints3D(const point3D_t point3D_id1, + const point3D_t point3D_id2); + + // Delete a 3D point, and all its references in the observed images. + void DeletePoint3D(const point3D_t point3D_id); + + // Delete one observation from an image and the corresponding 3D point. + // Note that this deletes the entire 3D point, if the track has two elements + // prior to calling this method. + void DeleteObservation(const image_t image_id, const point2D_t point2D_idx); + + // Delete all 2D points of all images and all 3D points. + void DeleteAllPoints2DAndPoints3D(); + + // Register an existing image. + void RegisterImage(const image_t image_id); + + // De-register an existing image, and all its references. + void DeRegisterImage(const image_t image_id); + + // Check if image is registered. + inline bool IsImageRegistered(const image_t image_id) const; + + // Normalize scene by scaling and translation to avoid degenerate + // visualization after bundle adjustment and to improve numerical + // stability of algorithms. + // + // Translates scene such that the mean of the camera centers or point + // locations are at the origin of the coordinate system. + // + // Scales scene such that the minimum and maximum camera centers are at the + // given `extent`, whereas `p0` and `p1` determine the minimum and + // maximum percentiles of the camera centers considered. + void Normalize(const double extent = 10.0, const double p0 = 0.1, + const double p1 = 0.9, const bool use_images = true); + + // Compute the centroid of the 3D points + Eigen::Vector3d ComputeCentroid(const double p0 = 0.1, + const double p1 = 0.9) const; + + // Compute the bounding box corners of the 3D points + std::pair ComputeBoundingBox( + const double p0 = 0.0, const double p1 = 1.0) const; + + // Apply the 3D similarity transformation to all images and points. + void Transform(const SimilarityTransform3& tform); + + // Creates a cropped reconstruction using the input bounds as corner points + // of the bounding box containing the included 3D points of the new + // reconstruction. Only the cameras and images of the included points are + // registered. + Reconstruction Crop( + const std::pair& bbox) const; + + // Merge the given reconstruction into this reconstruction by registering the + // images registered in the given but not in this reconstruction and by + // merging the two clouds and their tracks. The coordinate frames of the two + // reconstructions are aligned using the projection centers of common + // registered images. Return true if the two reconstructions could be merged. + bool Merge(const Reconstruction& reconstruction, + const double max_reproj_error); + + // Align the given reconstruction with a set of pre-defined camera positions. + // Assuming that locations[i] gives the 3D coordinates of the center + // of projection of the image with name image_names[i]. + template + bool Align(const std::vector& image_names, + const std::vector& locations, + const int min_common_images, + SimilarityTransform3* tform = nullptr); + + // Robust alignment using RANSAC. + template + bool AlignRobust(const std::vector& image_names, + const std::vector& locations, + const int min_common_images, + const RANSACOptions& ransac_options, + SimilarityTransform3* tform = nullptr); + + // Find specific image by name. Note that this uses linear search. + const class Image* FindImageWithName(const std::string& name) const; + + // Find images that are both present in this and the given reconstruction. + std::vector FindCommonRegImageIds( + const Reconstruction& reconstruction) const; + + // Update the image identifiers to match the ones in the database by matching + // the names of the images. + void TranscribeImageIdsToDatabase(const Database& database); + + // Filter 3D points with large reprojection error, negative depth, or + // insufficient triangulation angle. + // + // @param max_reproj_error The maximum reprojection error. + // @param min_tri_angle The minimum triangulation angle. + // @param point3D_ids The points to be filtered. + // + // @return The number of filtered observations. + size_t FilterPoints3D(const double max_reproj_error, + const double min_tri_angle, + const std::unordered_set& point3D_ids); + size_t FilterPoints3DInImages(const double max_reproj_error, + const double min_tri_angle, + const std::unordered_set& image_ids); + size_t FilterAllPoints3D(const double max_reproj_error, + const double min_tri_angle); + + // Filter observations that have negative depth. + // + // @return The number of filtered observations. + size_t FilterObservationsWithNegativeDepth(); + + // Filter images without observations or bogus camera parameters. + // + // @return The identifiers of the filtered images. + std::vector FilterImages(const double min_focal_length_ratio, + const double max_focal_length_ratio, + const double max_extra_param); + + // Compute statistics for scene. + size_t ComputeNumObservations() const; + double ComputeMeanTrackLength() const; + double ComputeMeanObservationsPerRegImage() const; + double ComputeMeanReprojectionError() const; + + // Read data from text or binary file. Prefer binary data if it exists. + void Read(const std::string& path); + void Write(const std::string& path) const; + + // Read data from binary/text file. + void ReadText(const std::string& path); + void ReadBinary(const std::string& path); + + // Write data from binary/text file. + void WriteText(const std::string& path) const; + void WriteBinary(const std::string& path) const; + + // Convert 3D points in reconstruction to PLY point cloud. + std::vector ConvertToPLY() const; + + // Import from other data formats. Note that these import functions are + // only intended for visualization of data and usable for reconstruction. + void ImportPLY(const std::string& path); + void ImportPLY(const std::vector& ply_points); + + // Export to other data formats. + + // Exports in NVM format http://ccwu.me/vsfm/doc.html#nvm. Only supports + // SIMPLE_RADIAL camera model when exporting distortion parameters. When + // skip_distortion == true it supports all camera models with the caveat that + // it's using the mean focal length which will be inaccurate for camera models + // with two focal lengths and distortion. + bool ExportNVM(const std::string& path, bool skip_distortion = false) const; + + // Exports in CAM format which is a simple text file that contains pose + // information and camera intrinsics for each image and exports one file per + // image; it does not include information on the 3D points. The format is as + // follows (2 lines of text with space separated numbers): + // + // 1.0 + // Note that focal length is relative to the image max(width, height), + // and principal points x and y are relative to width and height respectively. + // + // Only supports SIMPLE_RADIAL and RADIAL camera models when exporting + // distortion parameters. When skip_distortion == true it supports all camera + // models with the caveat that it's using the mean focal length which will be + // inaccurate for camera models with two focal lengths and distortion. + bool ExportCam(const std::string& path, bool skip_distortion = false) const; + + // Exports in Recon3D format which consists of three text files with the + // following format and content: + // 1) imagemap_0.txt: a list of image numeric IDs with one entry per line. + // 2) urd-images.txt: A list of images with one entry per line as: + // + // 3) synth_0.out: Contains information for image poses, camera intrinsics, + // and 3D points as: + // + // + // + // + // Each image entry consists of 5 lines as: + // + // + // + // Note that the focal length is scaled by 1 / max(width, height) + // + // Each point entry consists of 3 lines as: + // + // + // ... + // + // Each track elemenet is a sequence of 5 values as: + // <2D point ID> -1.0 + // Note that the 2D point coordinates are centered around the principal + // point and scaled by 1 / max(width, height). + // + // When skip_distortion == true it supports all camera models with the + // caveat that it's using the mean focal length which will be inaccurate + // for camera models with two focal lengths and distortion. + bool ExportRecon3D(const std::string& path, + bool skip_distortion = false) const; + + // Exports in Bundler format https://www.cs.cornell.edu/~snavely/bundler/. + // Supports SIMPLE_PINHOLE, PINHOLE, SIMPLE_RADIAL and RADIAL camera models + // when exporting distortion parameters. When skip_distortion == true it + // supports all camera models with the caveat that it's using the mean focal + // length which will be inaccurate for camera models with two focal lengths + // and distortion. + bool ExportBundler(const std::string& path, const std::string& list_path, + bool skip_distortion = false) const; + + // Exports 3D points only in PLY format. + void ExportPLY(const std::string& path) const; + + // Exports in VRML format https://en.wikipedia.org/wiki/VRML. + void ExportVRML(const std::string& images_path, + const std::string& points3D_path, const double image_scale, + const Eigen::Vector3d& image_rgb) const; + + // Extract colors for 3D points of given image. Colors will be extracted + // only for 3D points which are completely black. + // + // @param image_id Identifier of the image for which to extract colors. + // @param path Absolute or relative path to root folder of image. + // The image path is determined by concatenating the + // root path and the name of the image. + // + // @return True if image could be read at given path. + bool ExtractColorsForImage(const image_t image_id, const std::string& path); + + // Extract colors for all 3D points by computing the mean color of all images. + // + // @param path Absolute or relative path to root folder of image. + // The image path is determined by concatenating the + // root path and the name of the image. + void ExtractColorsForAllImages(const std::string& path); + + // Create all image sub-directories in the given path. + void CreateImageDirs(const std::string& path) const; + + private: + size_t FilterPoints3DWithSmallTriangulationAngle( + const double min_tri_angle, + const std::unordered_set& point3D_ids); + size_t FilterPoints3DWithLargeReprojectionError( + const double max_reproj_error, + const std::unordered_set& point3D_ids); + + std::tuple + ComputeBoundsAndCentroid(const double p0, const double p1, + const bool use_images) const; + + void ReadCamerasText(const std::string& path); + void ReadImagesText(const std::string& path); + void ReadPoints3DText(const std::string& path); + void ReadCamerasBinary(const std::string& path); + void ReadImagesBinary(const std::string& path); + void ReadPoints3DBinary(const std::string& path); + + void WriteCamerasText(const std::string& path) const; + void WriteImagesText(const std::string& path) const; + void WritePoints3DText(const std::string& path) const; + void WriteCamerasBinary(const std::string& path) const; + void WriteImagesBinary(const std::string& path) const; + void WritePoints3DBinary(const std::string& path) const; + + void SetObservationAsTriangulated(const image_t image_id, + const point2D_t point2D_idx, + const bool is_continued_point3D); + void ResetTriObservations(const image_t image_id, const point2D_t point2D_idx, + const bool is_deleted_point3D); + + const CorrespondenceGraph* correspondence_graph_; + + EIGEN_STL_UMAP(camera_t, class Camera) cameras_; + EIGEN_STL_UMAP(image_t, class Image) images_; + EIGEN_STL_UMAP(point3D_t, class Point3D) points3D_; + + std::unordered_map image_pair_stats_; + + // { image_id, ... } where `images_.at(image_id).registered == true`. + std::vector reg_image_ids_; + + // Total number of added 3D points, used to generate unique identifiers. + point3D_t num_added_points3D_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +size_t Reconstruction::NumCameras() const { return cameras_.size(); } + +size_t Reconstruction::NumImages() const { return images_.size(); } + +size_t Reconstruction::NumRegImages() const { return reg_image_ids_.size(); } + +size_t Reconstruction::NumPoints3D() const { return points3D_.size(); } + +size_t Reconstruction::NumImagePairs() const { + return image_pair_stats_.size(); +} + +const class Camera& Reconstruction::Camera(const camera_t camera_id) const { + return cameras_.at(camera_id); +} + +const class Image& Reconstruction::Image(const image_t image_id) const { + return images_.at(image_id); +} + +const class Point3D& Reconstruction::Point3D(const point3D_t point3D_id) const { + return points3D_.at(point3D_id); +} + +const Reconstruction::ImagePairStat& Reconstruction::ImagePair( + const image_pair_t pair_id) const { + return image_pair_stats_.at(pair_id); +} + +const Reconstruction::ImagePairStat& Reconstruction::ImagePair( + const image_t image_id1, const image_t image_id2) const { + const auto pair_id = Database::ImagePairToPairId(image_id1, image_id2); + return image_pair_stats_.at(pair_id); +} + +class Camera& Reconstruction::Camera(const camera_t camera_id) { + return cameras_.at(camera_id); +} + +class Image& Reconstruction::Image(const image_t image_id) { + return images_.at(image_id); +} + +class Point3D& Reconstruction::Point3D(const point3D_t point3D_id) { + return points3D_.at(point3D_id); +} + +Reconstruction::ImagePairStat& Reconstruction::ImagePair( + const image_pair_t pair_id) { + return image_pair_stats_.at(pair_id); +} + +Reconstruction::ImagePairStat& Reconstruction::ImagePair( + const image_t image_id1, const image_t image_id2) { + const auto pair_id = Database::ImagePairToPairId(image_id1, image_id2); + return image_pair_stats_.at(pair_id); +} + +const EIGEN_STL_UMAP(camera_t, Camera) & Reconstruction::Cameras() const { + return cameras_; +} + +const EIGEN_STL_UMAP(image_t, class Image) & Reconstruction::Images() const { + return images_; +} + +const std::vector& Reconstruction::RegImageIds() const { + return reg_image_ids_; +} + +const EIGEN_STL_UMAP(point3D_t, Point3D) & Reconstruction::Points3D() const { + return points3D_; +} + +const std::unordered_map& +Reconstruction::ImagePairs() const { + return image_pair_stats_; +} + +bool Reconstruction::ExistsCamera(const camera_t camera_id) const { + return cameras_.find(camera_id) != cameras_.end(); +} + +bool Reconstruction::ExistsImage(const image_t image_id) const { + return images_.find(image_id) != images_.end(); +} + +bool Reconstruction::ExistsPoint3D(const point3D_t point3D_id) const { + return points3D_.find(point3D_id) != points3D_.end(); +} + +bool Reconstruction::ExistsImagePair(const image_pair_t pair_id) const { + return image_pair_stats_.find(pair_id) != image_pair_stats_.end(); +} + +bool Reconstruction::IsImageRegistered(const image_t image_id) const { + return Image(image_id).IsRegistered(); +} + +template +bool Reconstruction::Align(const std::vector& image_names, + const std::vector& locations, + const int min_common_images, + SimilarityTransform3* tform) { + CHECK_GE(min_common_images, 3); + CHECK_EQ(image_names.size(), locations.size()); + + // Find out which images are contained in the reconstruction and get the + // positions of their camera centers. + std::unordered_set common_image_ids; + std::vector src; + std::vector dst; + for (size_t i = 0; i < image_names.size(); ++i) { + const class Image* image = FindImageWithName(image_names[i]); + if (image == nullptr) { + continue; + } + + if (!IsImageRegistered(image->ImageId())) { + continue; + } + + // Ignore duplicate images. + if (common_image_ids.count(image->ImageId()) > 0) { + continue; + } + + common_image_ids.insert(image->ImageId()); + src.push_back(image->ProjectionCenter()); + dst.push_back(locations[i]); + } + + // Only compute the alignment if there are enough correspondences. + if (common_image_ids.size() < static_cast(min_common_images)) { + return false; + } + + SimilarityTransform3 transform; + if (!transform.Estimate(src, dst)) { + return false; + } + + Transform(transform); + + if (tform != nullptr) { + *tform = transform; + } + + return true; +} + +template +bool Reconstruction::AlignRobust(const std::vector& image_names, + const std::vector& locations, + const int min_common_images, + const RANSACOptions& ransac_options, + SimilarityTransform3* tform) { + CHECK_GE(min_common_images, 3); + CHECK_EQ(image_names.size(), locations.size()); + + // Find out which images are contained in the reconstruction and get the + // positions of their camera centers. + std::unordered_set common_image_ids; + std::vector src; + std::vector dst; + for (size_t i = 0; i < image_names.size(); ++i) { + const class Image* image = FindImageWithName(image_names[i]); + if (image == nullptr) { + continue; + } + + if (!IsImageRegistered(image->ImageId())) { + continue; + } + + // Ignore duplicate images. + if (common_image_ids.count(image->ImageId()) > 0) { + continue; + } + + common_image_ids.insert(image->ImageId()); + src.push_back(image->ProjectionCenter()); + dst.push_back(locations[i]); + } + + // Only compute the alignment if there are enough correspondences. + if (common_image_ids.size() < static_cast(min_common_images)) { + return false; + } + + LORANSAC, + SimilarityTransformEstimator<3, kEstimateScale>> + ransac(ransac_options); + + const auto report = ransac.Estimate(src, dst); + + if (report.support.num_inliers < static_cast(min_common_images)) { + return false; + } + + SimilarityTransform3 transform = SimilarityTransform3(report.model); + Transform(transform); + + if (tform != nullptr) { + *tform = transform; + } + + return true; +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_RECONSTRUCTION_H_ diff --git a/colmap/include/colmap/base/reconstruction_manager.h b/colmap/include/colmap/base/reconstruction_manager.h new file mode 100644 index 0000000000000000000000000000000000000000..b0415497d2e1420d8d78419b2cc68589b3c6cedb --- /dev/null +++ b/colmap/include/colmap/base/reconstruction_manager.h @@ -0,0 +1,81 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_RECONSTRUCTION_MANAGER_H_ +#define COLMAP_SRC_BASE_RECONSTRUCTION_MANAGER_H_ + +#include "base/reconstruction.h" + +namespace colmap { + +class OptionManager; + +class ReconstructionManager { + public: + ReconstructionManager(); + + // Move constructor and assignment. + ReconstructionManager(ReconstructionManager&& other); + ReconstructionManager& operator=(ReconstructionManager&& other); + + // The number of reconstructions managed. + size_t Size() const; + + // Get a reference to a specific reconstruction. + const Reconstruction& Get(const size_t idx) const; + Reconstruction& Get(const size_t idx); + + // Add a new empty reconstruction and return its index. + size_t Add(); + + // Delete a specific reconstruction. + void Delete(const size_t idx); + + // Delete all reconstructions. + void Clear(); + + // Read and add a new reconstruction and return its index. + size_t Read(const std::string& path); + + // Write all managed reconstructions into sub-folders "0", "1", "2", ... + // If the option manager object is not null, the options are written + // to each respective reconstruction folder as well. + void Write(const std::string& path, const OptionManager* options) const; + + private: + NON_COPYABLE(ReconstructionManager) + + std::vector> reconstructions_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_RECONSTRUCTION_MANAGER_H_ diff --git a/colmap/include/colmap/base/scene_clustering.h b/colmap/include/colmap/base/scene_clustering.h new file mode 100644 index 0000000000000000000000000000000000000000..49fd5596c53c94205aa4492d131c1d6dac192ebd --- /dev/null +++ b/colmap/include/colmap/base/scene_clustering.h @@ -0,0 +1,100 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_SCENE_CLUSTERING_H_ +#define COLMAP_SRC_BASE_SCENE_CLUSTERING_H_ + +#include +#include + +#include "base/database.h" +#include "util/types.h" + +namespace colmap { + +// Scene clustering approach using normalized cuts on the scene graph. The scene +// is hierarchically partitioned into overlapping clusters until a maximum +// number of images is in a leaf node. +class SceneClustering { + public: + struct Options { + // Flag for hierarchical vs flat clustering + bool is_hierarchical = true; + + // The branching factor of the hierarchical clustering. + int branching = 2; + + // The number of overlapping images between child clusters. + int image_overlap = 50; + + // The max related images matches to look for in a flat cluster + int num_image_matches = 20; + + // The maximum number of images in a leaf node cluster, otherwise the + // cluster is further partitioned using the given branching factor. Note + // that a cluster leaf node will have at most `leaf_max_num_images + + // overlap` images to satisfy the overlap constraint. + int leaf_max_num_images = 500; + + bool Check() const; + }; + + struct Cluster { + std::vector image_ids; + std::vector child_clusters; + }; + + SceneClustering(const Options& options); + + void Partition(const std::vector>& image_pairs, + const std::vector& num_inliers); + + const Cluster* GetRootCluster() const; + std::vector GetLeafClusters() const; + + static SceneClustering Create(const Options& options, + const Database& database); + + private: + void PartitionHierarchicalCluster( + const std::vector>& edges, + const std::vector& weights, Cluster* cluster); + + void PartitionFlatCluster(const std::vector>& edges, + const std::vector& weights); + + const Options options_; + std::unique_ptr root_cluster_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_SCENE_CLUSTERING_H_ diff --git a/colmap/include/colmap/base/similarity_transform.h b/colmap/include/colmap/base/similarity_transform.h new file mode 100644 index 0000000000000000000000000000000000000000..09b5738d631ab6949ae5b23543640e5a202f52ee --- /dev/null +++ b/colmap/include/colmap/base/similarity_transform.h @@ -0,0 +1,121 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_ +#define COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_ + +#include + +#include +#include + +#include "estimators/similarity_transform.h" +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +struct RANSACOptions; +class Reconstruction; + +// 3D similarity transformation with 7 degrees of freedom. +class SimilarityTransform3 { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + SimilarityTransform3(); + + explicit SimilarityTransform3(const Eigen::Matrix3x4d& matrix); + + explicit SimilarityTransform3( + const Eigen::Transform& transform); + + SimilarityTransform3(const double scale, const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec); + + void Write(const std::string& path); + + template + bool Estimate(const std::vector& src, + const std::vector& dst); + + SimilarityTransform3 Inverse() const; + + void TransformPoint(Eigen::Vector3d* xyz) const; + void TransformPose(Eigen::Vector4d* qvec, Eigen::Vector3d* tvec) const; + + Eigen::Matrix4d Matrix() const; + double Scale() const; + Eigen::Vector4d Rotation() const; + Eigen::Vector3d Translation() const; + + static SimilarityTransform3 FromFile(const std::string& path); + + private: + Eigen::Transform transform_; +}; + +// Robustly compute alignment between reconstructions by finding images that +// are registered in both reconstructions. The alignment is then estimated +// robustly inside RANSAC from corresponding projection centers. An alignment +// is verified by reprojecting common 3D point observations. +// The min_inlier_observations threshold determines how many observations +// in a common image must reproject within the given threshold. +bool ComputeAlignmentBetweenReconstructions( + const Reconstruction& src_reconstruction, + const Reconstruction& ref_reconstruction, + const double min_inlier_observations, const double max_reproj_error, + Eigen::Matrix3x4d* alignment); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +bool SimilarityTransform3::Estimate(const std::vector& src, + const std::vector& dst) { + const auto results = + SimilarityTransformEstimator<3, kEstimateScale>().Estimate(src, dst); + if (results.empty()) { + return false; + } + + CHECK_EQ(results.size(), 1); + transform_.matrix().topLeftCorner<3, 4>() = results[0]; + + return true; +} + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::SimilarityTransform3) + +#endif // COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_ diff --git a/colmap/include/colmap/base/track.h b/colmap/include/colmap/base/track.h new file mode 100644 index 0000000000000000000000000000000000000000..20611a48392a76ba22dbda3602b893ac93346fb6 --- /dev/null +++ b/colmap/include/colmap/base/track.h @@ -0,0 +1,139 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_TRACK_H_ +#define COLMAP_SRC_BASE_TRACK_H_ + +#include + +#include "util/logging.h" +#include "util/types.h" + +namespace colmap { + +// Track class stores all observations of a 3D point. +struct TrackElement { + TrackElement(); + TrackElement(const image_t image_id, const point2D_t point2D_idx); + // The image in which the track element is observed. + image_t image_id; + // The point in the image that the track element is observed. + point2D_t point2D_idx; +}; + +class Track { + public: + Track(); + + // The number of track elements. + inline size_t Length() const; + + // Access all elements. + inline const std::vector& Elements() const; + inline std::vector& Elements(); + inline void SetElements(std::vector elements); + + // Access specific elements. + inline const TrackElement& Element(const size_t idx) const; + inline TrackElement& Element(const size_t idx); + inline void SetElement(const size_t idx, const TrackElement& element); + + // Append new elements. + inline void AddElement(const TrackElement& element); + inline void AddElement(const image_t image_id, const point2D_t point2D_idx); + inline void AddElements(const std::vector& elements); + + // Delete existing element. + inline void DeleteElement(const size_t idx); + void DeleteElement(const image_t image_id, const point2D_t point2D_idx); + + // Requests that the track capacity be at least enough to contain the + // specified number of elements. + inline void Reserve(const size_t num_elements); + + // Shrink the capacity of track vector to fit its size to save memory. + inline void Compress(); + + private: + std::vector elements_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +size_t Track::Length() const { return elements_.size(); } + +const std::vector& Track::Elements() const { return elements_; } + +std::vector& Track::Elements() { return elements_; } + +void Track::SetElements(std::vector elements) { + elements_ = std::move(elements); +} + +// Access specific elements. +const TrackElement& Track::Element(const size_t idx) const { + return elements_.at(idx); +} + +TrackElement& Track::Element(const size_t idx) { return elements_.at(idx); } + +void Track::SetElement(const size_t idx, const TrackElement& element) { + elements_.at(idx) = element; +} + +void Track::AddElement(const TrackElement& element) { + elements_.push_back(element); +} + +void Track::AddElement(const image_t image_id, const point2D_t point2D_idx) { + elements_.emplace_back(image_id, point2D_idx); +} + +void Track::AddElements(const std::vector& elements) { + elements_.insert(elements_.end(), elements.begin(), elements.end()); +} + +void Track::DeleteElement(const size_t idx) { + CHECK_LT(idx, elements_.size()); + elements_.erase(elements_.begin() + idx); +} + +void Track::Reserve(const size_t num_elements) { + elements_.reserve(num_elements); +} + +void Track::Compress() { elements_.shrink_to_fit(); } + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_TRACK_H_ diff --git a/colmap/include/colmap/base/triangulation.h b/colmap/include/colmap/base/triangulation.h new file mode 100644 index 0000000000000000000000000000000000000000..ddba2bfce7835fdbf30f5d0dbd3df841d2cb7944 --- /dev/null +++ b/colmap/include/colmap/base/triangulation.h @@ -0,0 +1,118 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_TRIANGULATION_H_ +#define COLMAP_SRC_BASE_TRIANGULATION_H_ + +#include + +#include + +#include "base/camera.h" +#include "util/alignment.h" +#include "util/math.h" +#include "util/types.h" + +namespace colmap { + +// Triangulate 3D point from corresponding image point observations. +// +// Implementation of the direct linear transform triangulation method in +// R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision, +// Cambridge Univ. Press, 2003. +// +// @param proj_matrix1 Projection matrix of the first image as 3x4 matrix. +// @param proj_matrix2 Projection matrix of the second image as 3x4 matrix. +// @param point1 Corresponding 2D point in first image. +// @param point2 Corresponding 2D point in second image. +// +// @return Triangulated 3D point. +Eigen::Vector3d TriangulatePoint(const Eigen::Matrix3x4d& proj_matrix1, + const Eigen::Matrix3x4d& proj_matrix2, + const Eigen::Vector2d& point1, + const Eigen::Vector2d& point2); + +// Triangulate multiple 3D points from multiple image correspondences. +std::vector TriangulatePoints( + const Eigen::Matrix3x4d& proj_matrix1, + const Eigen::Matrix3x4d& proj_matrix2, + const std::vector& points1, + const std::vector& points2); + +// Triangulate point from multiple views minimizing the L2 error. +// +// @param proj_matrices Projection matrices of multi-view observations. +// @param points Image observations of multi-view observations. +// +// @return Estimated 3D point. +Eigen::Vector3d TriangulateMultiViewPoint( + const std::vector& proj_matrices, + const std::vector& points); + +// Triangulate optimal 3D point from corresponding image point observations by +// finding the optimal image observations. +// +// Note that camera poses should be very good in order for this method to yield +// good results. Otherwise just use `TriangulatePoint`. +// +// Implementation of the method described in +// P. Lindstrom, "Triangulation Made Easy," IEEE Computer Vision and Pattern +// Recognition 2010, pp. 1554-1561, June 2010. +// +// @param proj_matrix1 Projection matrix of the first image as 3x4 matrix. +// @param proj_matrix2 Projection matrix of the second image as 3x4 matrix. +// @param point1 Corresponding 2D point in first image. +// @param point2 Corresponding 2D point in second image. +// +// @return Triangulated optimal 3D point. +Eigen::Vector3d TriangulateOptimalPoint(const Eigen::Matrix3x4d& proj_matrix1, + const Eigen::Matrix3x4d& proj_matrix2, + const Eigen::Vector2d& point1, + const Eigen::Vector2d& point2); + +// Triangulate multiple optimal 3D points from multiple image correspondences. +std::vector TriangulateOptimalPoints( + const Eigen::Matrix3x4d& proj_matrix1, + const Eigen::Matrix3x4d& proj_matrix2, + const std::vector& points1, + const std::vector& points2); + +// Calculate angle in radians between the two rays of a triangulated point. +double CalculateTriangulationAngle(const Eigen::Vector3d& proj_center1, + const Eigen::Vector3d& proj_center2, + const Eigen::Vector3d& point3D); +std::vector CalculateTriangulationAngles( + const Eigen::Vector3d& proj_center1, const Eigen::Vector3d& proj_center2, + const std::vector& points3D); + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_TRIANGULATION_H_ diff --git a/colmap/include/colmap/base/undistortion.h b/colmap/include/colmap/base/undistortion.h new file mode 100644 index 0000000000000000000000000000000000000000..31814598bc617703d8fff795fd6c699ba06fcf91 --- /dev/null +++ b/colmap/include/colmap/base/undistortion.h @@ -0,0 +1,234 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_UNDISTORTION_H_ +#define COLMAP_SRC_BASE_UNDISTORTION_H_ + +#include "base/reconstruction.h" +#include "util/alignment.h" +#include "util/bitmap.h" +#include "util/misc.h" +#include "util/threading.h" + +namespace colmap { + +struct UndistortCameraOptions { + // The amount of blank pixels in the undistorted image in the range [0, 1]. + double blank_pixels = 0.0; + + // Minimum and maximum scale change of camera used to satisfy the blank + // pixel constraint. + double min_scale = 0.2; + double max_scale = 2.0; + + // Maximum image size in terms of width or height of the undistorted camera. + int max_image_size = -1; + + // The 4 factors in the range [0, 1] that define the ROI (region of interest) + // in original image. The bounding box pixel coordinates are calculated as + // (roi_min_x * Width, roi_min_y * Height) and + // (roi_max_x * Width, roi_max_y * Height). + double roi_min_x = 0.0; + double roi_min_y = 0.0; + double roi_max_x = 1.0; + double roi_max_y = 1.0; +}; + +// Undistort images and export undistorted cameras, as required by the +// mvs::PatchMatchController class. +class COLMAPUndistorter : public Thread { + public: + COLMAPUndistorter( + const UndistortCameraOptions& options, + const Reconstruction& reconstruction, const std::string& image_path, + const std::string& output_path, const int num_related_images = 20, + const CopyType copy_type = CopyType::COPY, + const std::vector& image_ids = std::vector()); + + private: + void Run(); + + bool Undistort(const image_t image_id) const; + void WritePatchMatchConfig() const; + void WriteFusionConfig() const; + void WriteScript(const bool geometric) const; + + UndistortCameraOptions options_; + const std::string image_path_; + const std::string output_path_; + const CopyType copy_type_; + const int num_patch_match_src_images_; + const Reconstruction& reconstruction_; + const std::vector image_ids_; + std::vector image_names_; +}; + +// Undistort images and prepare data for CMVS/PMVS. +class PMVSUndistorter : public Thread { + public: + PMVSUndistorter(const UndistortCameraOptions& options, + const Reconstruction& reconstruction, + const std::string& image_path, + const std::string& output_path); + + private: + void Run(); + + bool Undistort(const size_t reg_image_idx) const; + void WriteVisibilityData() const; + void WriteOptionFile() const; + void WritePMVSScript() const; + void WriteCMVSPMVSScript() const; + void WriteCOLMAPScript(const bool geometric) const; + void WriteCMVSCOLMAPScript(const bool geometric) const; + + UndistortCameraOptions options_; + std::string image_path_; + std::string output_path_; + const Reconstruction& reconstruction_; +}; + +// Undistort images and prepare data for CMP-MVS. +class CMPMVSUndistorter : public Thread { + public: + CMPMVSUndistorter(const UndistortCameraOptions& options, + const Reconstruction& reconstruction, + const std::string& image_path, + const std::string& output_path); + + private: + void Run(); + + bool Undistort(const size_t reg_image_idx) const; + + UndistortCameraOptions options_; + std::string image_path_; + std::string output_path_; + const Reconstruction& reconstruction_; +}; + +// Undistort images and export undistorted cameras without the need for a +// reconstruction. Instead, the image names and camera model information are +// read from a text file. +class PureImageUndistorter : public Thread { + public: + PureImageUndistorter(const UndistortCameraOptions& options, + const std::string& image_path, + const std::string& output_path, + const std::vector>& + image_names_and_cameras); + + private: + void Run(); + + bool Undistort(const size_t reg_image_idx) const; + + UndistortCameraOptions options_; + std::string image_path_; + std::string output_path_; + const std::vector>& image_names_and_cameras_; +}; + +// Rectify stereo image pairs. +class StereoImageRectifier : public Thread { + public: + StereoImageRectifier( + const UndistortCameraOptions& options, + const Reconstruction& reconstruction, const std::string& image_path, + const std::string& output_path, + const std::vector>& stereo_pairs); + + private: + void Run(); + + void Rectify(const image_t image_id1, const image_t image_id2) const; + + UndistortCameraOptions options_; + std::string image_path_; + std::string output_path_; + const std::vector>& stereo_pairs_; + const Reconstruction& reconstruction_; +}; + +// Undistort camera by resizing the image and shifting the principal point. +// +// The scaling factor is computed such that no blank pixels are in the +// undistorted image (blank_pixels=0) or all pixels in distorted image are +// contained in output image (blank_pixels=1). +// +// The focal length of the image is preserved and the dimensions of the +// undistorted pinhole camera are adjusted such that either all pixels in +// the undistorted image have a corresponding pixel in the distorted image +// (i.e. no blank pixels at the borders, for `blank_pixels=0`), or all pixels +// in the distorted image project have a corresponding pixel in the undistorted +// image (i.e. blank pixels at the borders, for `blank_pixels=1`). Intermediate +// states can be achieved by setting `blank_pixels` between 0 and 1. +// +// The relative location of the principal point of the distorted camera is +// preserved. The scaling of the image dimensions is subject to the `min_scale`, +// `max_scale`, and `max_image_size` constraints. +Camera UndistortCamera(const UndistortCameraOptions& options, + const Camera& camera); + +// Undistort image such that the viewing geometry of the undistorted image +// follows a pinhole camera model. See `UndistortCamera` for more details +// on the undistortion conventions. +void UndistortImage(const UndistortCameraOptions& options, + const Bitmap& distorted_image, + const Camera& distorted_camera, Bitmap* undistorted_image, + Camera* undistorted_camera); + +// Undistort all cameras in the reconstruction and accordingly all +// observations in their corresponding images. +void UndistortReconstruction(const UndistortCameraOptions& options, + Reconstruction* reconstruction); + +// Compute stereo rectification homographies that transform two images, +// such that corresponding pixels in one image lie on the same scanline in the +// other image. The matrix Q transforms disparity values to world coordinates +// as [x, y, disparity, 1] * Q = [X, Y, Z, 1] * w. Note that this function +// assumes that the two cameras are already undistorted. +void RectifyStereoCameras(const Camera& camera1, const Camera& camera2, + const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec, Eigen::Matrix3d* H1, + Eigen::Matrix3d* H2, Eigen::Matrix4d* Q); + +// Rectify and undistort the stereo image pair using the given geometry. +void RectifyAndUndistortStereoImages( + const UndistortCameraOptions& options, const Bitmap& distorted_image1, + const Bitmap& distorted_image2, const Camera& distorted_camera1, + const Camera& distorted_camera2, const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec, Bitmap* undistorted_image1, + Bitmap* undistorted_image2, Camera* undistorted_camera, Eigen::Matrix4d* Q); + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_UNDISTORTION_H_ diff --git a/colmap/include/colmap/base/visibility_pyramid.h b/colmap/include/colmap/base/visibility_pyramid.h new file mode 100644 index 0000000000000000000000000000000000000000..a4b16b4fdcefe97b5e0e03007c37fa597e2a441a --- /dev/null +++ b/colmap/include/colmap/base/visibility_pyramid.h @@ -0,0 +1,104 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_VISIBILITY_PYRAMID_H_ +#define COLMAP_SRC_BASE_VISIBILITY_PYRAMID_H_ + +#include + +#include + +#include "util/alignment.h" + +namespace colmap { + +// A class that captures the distribution of points in a 2D grid. +// For example, to capture the distribution of visible 3D points in an image. +// +// The class captures the distribution of points by a score. A higher score +// corresponds to a more uniform distribution of the points in the grid. +// +// The score is computed by the number of populated cells in a multi-resolution +// pyramid. A populated cell contributes to the overall score if it is +// populated by at least one point and the contributed score is according +// to its resolution in the pyramid. A cell in a higher resolution level +// contributes a higher score to the overall score. +class VisibilityPyramid { + public: + VisibilityPyramid(); + VisibilityPyramid(const size_t num_levels, const size_t width, + const size_t height); + + void SetPoint(const double x, const double y); + void ResetPoint(const double x, const double y); + + inline size_t NumLevels() const; + inline size_t Width() const; + inline size_t Height() const; + + inline size_t Score() const; + inline size_t MaxScore() const; + + private: + void CellForPoint(const double x, const double y, size_t* cx, + size_t* cy) const; + + // Range of the input points. + size_t width_; + size_t height_; + + // The overall visibility score. + size_t score_; + + // The maximum score when all cells are populated. + size_t max_score_; + + // The visibilty pyramid with multiple levels. + std::vector pyramid_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +size_t VisibilityPyramid::NumLevels() const { return pyramid_.size(); } + +size_t VisibilityPyramid::Width() const { return width_; } + +size_t VisibilityPyramid::Height() const { return height_; } + +size_t VisibilityPyramid::Score() const { return score_; } + +size_t VisibilityPyramid::MaxScore() const { return max_score_; } + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_VISIBILITY_PYRAMID_H_ diff --git a/colmap/include/colmap/base/warp.h b/colmap/include/colmap/base/warp.h new file mode 100644 index 0000000000000000000000000000000000000000..f682a32241b8749921aae18b83004a1e2749e824 --- /dev/null +++ b/colmap/include/colmap/base/warp.h @@ -0,0 +1,80 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_WARP_H_ +#define COLMAP_SRC_BASE_WARP_H_ + +#include "base/camera.h" +#include "util/alignment.h" +#include "util/bitmap.h" + +namespace colmap { + +// Warp source image to target image by projecting the pixels of the target +// image up to infinity and projecting it down into the source image +// (i.e. an inverse mapping). The function allocates the target image. +void WarpImageBetweenCameras(const Camera& source_camera, + const Camera& target_camera, + const Bitmap& source_image, Bitmap* target_image); + +// Warp an image with the given homography, where H defines the pixel mapping +// from the target to source image. Note that the pixel centers are assumed to +// have coordinates (0.5, 0.5). +void WarpImageWithHomography(const Eigen::Matrix3d& H, + const Bitmap& source_image, Bitmap* target_image); + +// First, warp source image to target image by projecting the pixels of the +// target image up to infinity and projecting it down into the source image +// (i.e. an inverse mapping). Second, warp the coordinates from the first +// warping with the given homography. The function allocates the target image. +void WarpImageWithHomographyBetweenCameras(const Eigen::Matrix3d& H, + const Camera& source_camera, + const Camera& target_camera, + const Bitmap& source_image, + Bitmap* target_image); + +// Resample row-major image using bilinear interpolation. +void ResampleImageBilinear(const float* data, const int rows, const int cols, + const int new_rows, const int new_cols, + float* resampled); + +// Smooth row-major image using a Gaussian filter kernel. +void SmoothImage(const float* data, const int rows, const int cols, + const float sigma_r, const float sigma_c, float* smoothed); + +// Downsample row-major image by first smoothing and then resampling. +void DownsampleImage(const float* data, const int rows, const int cols, + const int new_rows, const int new_cols, + float* downsampled); + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_WARP_H_ diff --git a/colmap/include/colmap/controllers/automatic_reconstruction.h b/colmap/include/colmap/controllers/automatic_reconstruction.h new file mode 100644 index 0000000000000000000000000000000000000000..0c3a6c60c03d8f66460ed73435440fcc51159abf --- /dev/null +++ b/colmap/include/colmap/controllers/automatic_reconstruction.h @@ -0,0 +1,123 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_CONTROLLERS_AUTOMATIC_RECONSTRUCTION_H_ +#define COLMAP_SRC_CONTROLLERS_AUTOMATIC_RECONSTRUCTION_H_ + +#include + +#include "base/reconstruction_manager.h" +#include "util/option_manager.h" +#include "util/threading.h" + +namespace colmap { + +class AutomaticReconstructionController : public Thread { + public: + enum class DataType { INDIVIDUAL, VIDEO, INTERNET }; + enum class Quality { LOW, MEDIUM, HIGH, EXTREME }; + enum class Mesher { POISSON, DELAUNAY }; + + struct Options { + // The path to the workspace folder in which all results are stored. + std::string workspace_path; + + // The path to the image folder which are used as input. + std::string image_path; + + // The path to the mask folder which are used as input. + std::string mask_path; + + // The path to the vocabulary tree for feature matching. + std::string vocab_tree_path; + + // The type of input data used to choose optimal mapper settings. + DataType data_type = DataType::INDIVIDUAL; + + // Whether to perform low- or high-quality reconstruction. + Quality quality = Quality::HIGH; + + // Whether to use shared intrinsics or not. + bool single_camera = false; + + // Which camera model to use for images. + std::string camera_model = "SIMPLE_RADIAL"; + + // Whether to perform sparse mapping. + bool sparse = true; + +// Whether to perform dense mapping. +#ifdef CUDA_ENABLED + bool dense = true; +#else + bool dense = false; +#endif + + // The meshing algorithm to be used. + Mesher mesher = Mesher::POISSON; + + // The number of threads to use in all stages. + int num_threads = -1; + + // Whether to use the GPU in feature extraction and matching. + bool use_gpu = true; + + // Index of the GPU used for GPU stages. For multi-GPU computation, + // you should separate multiple GPU indices by comma, e.g., "0,1,2,3". + // By default, all GPUs will be used in all stages. + std::string gpu_index = "-1"; + }; + + AutomaticReconstructionController( + const Options& options, ReconstructionManager* reconstruction_manager); + + void Stop() override; + + private: + void Run() override; + void RunFeatureExtraction(); + void RunFeatureMatching(); + void RunSparseMapper(); + void RunDenseMapper(); + + const Options options_; + OptionManager option_manager_; + ReconstructionManager* reconstruction_manager_; + Thread* active_thread_; + std::unique_ptr feature_extractor_; + std::unique_ptr exhaustive_matcher_; + std::unique_ptr sequential_matcher_; + std::unique_ptr vocab_tree_matcher_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_CONTROLLERS_AUTOMATIC_RECONSTRUCTION_H_ diff --git a/colmap/include/colmap/controllers/bundle_adjustment.h b/colmap/include/colmap/controllers/bundle_adjustment.h new file mode 100644 index 0000000000000000000000000000000000000000..b1ca9dfbfe4a2a4655f9ed72696cc511b37d7f7a --- /dev/null +++ b/colmap/include/colmap/controllers/bundle_adjustment.h @@ -0,0 +1,56 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_CONTROLLERS_BUNDLE_ADJUSTMENT_H_ +#define COLMAP_SRC_CONTROLLERS_BUNDLE_ADJUSTMENT_H_ + +#include "base/reconstruction.h" +#include "util/option_manager.h" +#include "util/threading.h" + +namespace colmap { + +// Class that controls the global bundle adjustment procedure. +class BundleAdjustmentController : public Thread { + public: + BundleAdjustmentController(const OptionManager& options, + Reconstruction* reconstruction); + + private: + void Run(); + + const OptionManager options_; + Reconstruction* reconstruction_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_CONTROLLERS_BUNDLE_ADJUSTMENT_H_ diff --git a/colmap/include/colmap/controllers/hierarchical_mapper.h b/colmap/include/colmap/controllers/hierarchical_mapper.h new file mode 100644 index 0000000000000000000000000000000000000000..9d87b0b16f6305f21495e5f973d8cf32ed1a2e44 --- /dev/null +++ b/colmap/include/colmap/controllers/hierarchical_mapper.h @@ -0,0 +1,82 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_CONTROLLERS_HIERARCHICAL_MAPPER_H_ +#define COLMAP_SRC_CONTROLLERS_HIERARCHICAL_MAPPER_H_ + +#include "base/reconstruction_manager.h" +#include "base/scene_clustering.h" +#include "controllers/incremental_mapper.h" +#include "util/threading.h" + +namespace colmap { + +// Hierarchical mapping first hierarchically partitions the scene into multiple +// overlapping clusters, then reconstructs them separately using incremental +// mapping, and finally merges them all into a globally consistent +// reconstruction. This is especially useful for larger-scale scenes, since +// incremental mapping becomes slow with an increasing number of images. +class HierarchicalMapperController : public Thread { + public: + struct Options { + // The path to the image folder which are used as input. + std::string image_path; + + // The path to the database file which is used as input. + std::string database_path; + + // The maximum number of trials to initialize a cluster. + int init_num_trials = 10; + + // The number of workers used to reconstruct clusters in parallel. + int num_workers = -1; + + bool Check() const; + }; + + HierarchicalMapperController( + const Options& options, + const SceneClustering::Options& clustering_options, + const IncrementalMapperOptions& mapper_options, + ReconstructionManager* reconstruction_manager); + + private: + void Run() override; + + const Options options_; + const SceneClustering::Options clustering_options_; + const IncrementalMapperOptions mapper_options_; + ReconstructionManager* reconstruction_manager_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_CONTROLLERS_HIERARCHICAL_MAPPER_H_ diff --git a/colmap/include/colmap/controllers/incremental_mapper.h b/colmap/include/colmap/controllers/incremental_mapper.h new file mode 100644 index 0000000000000000000000000000000000000000..3686a58e03075ed7950106854c13da2987527a77 --- /dev/null +++ b/colmap/include/colmap/controllers/incremental_mapper.h @@ -0,0 +1,199 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_CONTROLLERS_INCREMENTAL_MAPPER_H_ +#define COLMAP_SRC_CONTROLLERS_INCREMENTAL_MAPPER_H_ + +#include "base/reconstruction_manager.h" +#include "sfm/incremental_mapper.h" +#include "util/threading.h" + +namespace colmap { + +struct IncrementalMapperOptions { + public: + // The minimum number of matches for inlier matches to be considered. + int min_num_matches = 15; + + // Whether to ignore the inlier matches of watermark image pairs. + bool ignore_watermarks = false; + + // Whether to reconstruct multiple sub-models. + bool multiple_models = true; + + // The number of sub-models to reconstruct. + int max_num_models = 50; + + // The maximum number of overlapping images between sub-models. If the + // current sub-models shares more than this number of images with another + // model, then the reconstruction is stopped. + int max_model_overlap = 20; + + // The minimum number of registered images of a sub-model, otherwise the + // sub-model is discarded. + int min_model_size = 10; + + // The image identifiers used to initialize the reconstruction. Note that + // only one or both image identifiers can be specified. In the former case, + // the second image is automatically determined. + int init_image_id1 = -1; + int init_image_id2 = -1; + + // The number of trials to initialize the reconstruction. + int init_num_trials = 200; + + // Whether to extract colors for reconstructed points. + bool extract_colors = true; + + // The number of threads to use during reconstruction. + int num_threads = -1; + + // Thresholds for filtering images with degenerate intrinsics. + double min_focal_length_ratio = 0.1; + double max_focal_length_ratio = 10.0; + double max_extra_param = 1.0; + + // Which intrinsic parameters to optimize during the reconstruction. + bool ba_refine_focal_length = true; + bool ba_refine_principal_point = false; + bool ba_refine_extra_params = true; + + // The minimum number of residuals per bundle adjustment problem to + // enable multi-threading solving of the problems. + int ba_min_num_residuals_for_multi_threading = 50000; + + // The number of images to optimize in local bundle adjustment. + int ba_local_num_images = 6; + + // Ceres solver function tolerance for local bundle adjustment + double ba_local_function_tolerance = 0.0; + + // The maximum number of local bundle adjustment iterations. + int ba_local_max_num_iterations = 25; + + // Whether to use PBA in global bundle adjustment. + bool ba_global_use_pba = false; + + // The GPU index for PBA bundle adjustment. + int ba_global_pba_gpu_index = -1; + + // The growth rates after which to perform global bundle adjustment. + double ba_global_images_ratio = 1.1; + double ba_global_points_ratio = 1.1; + int ba_global_images_freq = 500; + int ba_global_points_freq = 250000; + + // Ceres solver function tolerance for global bundle adjustment + double ba_global_function_tolerance = 0.0; + + // The maximum number of global bundle adjustment iterations. + int ba_global_max_num_iterations = 50; + + // The thresholds for iterative bundle adjustment refinements. + int ba_local_max_refinements = 2; + double ba_local_max_refinement_change = 0.001; + int ba_global_max_refinements = 5; + double ba_global_max_refinement_change = 0.0005; + + // Path to a folder with reconstruction snapshots during incremental + // reconstruction. Snapshots will be saved according to the specified + // frequency of registered images. + std::string snapshot_path = ""; + int snapshot_images_freq = 0; + + // Which images to reconstruct. If no images are specified, all images will + // be reconstructed by default. + std::unordered_set image_names; + + // If reconstruction is provided as input, fix the existing image poses. + bool fix_existing_images = false; + + IncrementalMapper::Options Mapper() const; + IncrementalTriangulator::Options Triangulation() const; + BundleAdjustmentOptions LocalBundleAdjustment() const; + BundleAdjustmentOptions GlobalBundleAdjustment() const; + ParallelBundleAdjuster::Options ParallelGlobalBundleAdjustment() const; + + bool Check() const; + + private: + friend class OptionManager; + friend class MapperGeneralOptionsWidget; + friend class MapperTriangulationOptionsWidget; + friend class MapperRegistrationOptionsWidget; + friend class MapperInitializationOptionsWidget; + friend class MapperBundleAdjustmentOptionsWidget; + friend class MapperFilteringOptionsWidget; + friend class ReconstructionOptionsWidget; + IncrementalMapper::Options mapper; + IncrementalTriangulator::Options triangulation; +}; + +// Class that controls the incremental mapping procedure by iteratively +// initializing reconstructions from the same scene graph. +class IncrementalMapperController : public Thread { + public: + enum { + INITIAL_IMAGE_PAIR_REG_CALLBACK, + NEXT_IMAGE_REG_CALLBACK, + LAST_IMAGE_REG_CALLBACK, + }; + + IncrementalMapperController(const IncrementalMapperOptions* options, + const std::string& image_path, + const std::string& database_path, + ReconstructionManager* reconstruction_manager); + + private: + void Run(); + bool LoadDatabase(); + void Reconstruct(const IncrementalMapper::Options& init_mapper_options); + + const IncrementalMapperOptions* options_; + const std::string image_path_; + const std::string database_path_; + ReconstructionManager* reconstruction_manager_; + DatabaseCache database_cache_; +}; + +// Globally filter points and images in mapper. +size_t FilterPoints(const IncrementalMapperOptions& options, + IncrementalMapper* mapper); +size_t FilterImages(const IncrementalMapperOptions& options, + IncrementalMapper* mapper); + +// Globally complete and merge tracks in mapper. +size_t CompleteAndMergeTracks(const IncrementalMapperOptions& options, + IncrementalMapper* mapper); + +} // namespace colmap + +#endif // COLMAP_SRC_CONTROLLERS_INCREMENTAL_MAPPER_H_ diff --git a/colmap/include/colmap/estimators/absolute_pose.h b/colmap/include/colmap/estimators/absolute_pose.h new file mode 100644 index 0000000000000000000000000000000000000000..953b03d3c913d5e72762b28f5191a7c1a0b3c795 --- /dev/null +++ b/colmap/include/colmap/estimators/absolute_pose.h @@ -0,0 +1,182 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_ESTIMATORS_ABSOLUTE_POSE_H_ +#define COLMAP_SRC_ESTIMATORS_ABSOLUTE_POSE_H_ + +#include +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Analytic solver for the P3P (Perspective-Three-Point) problem. +// +// The algorithm is based on the following paper: +// +// X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang. Complete Solution +// Classification for the Perspective-Three-Point Problem. +// http://www.mmrc.iss.ac.cn/~xgao/paper/ieee.pdf +class P3PEstimator { + public: + // The 2D image feature observations. + typedef Eigen::Vector2d X_t; + // The observed 3D features in the world frame. + typedef Eigen::Vector3d Y_t; + // The transformation from the world to the camera frame. + typedef Eigen::Matrix3x4d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 3; + + // Estimate the most probable solution of the P3P problem from a set of + // three 2D-3D point correspondences. + // + // @param points2D Normalized 2D image points as 3x2 matrix. + // @param points3D 3D world points as 3x3 matrix. + // + // @return Most probable pose as length-1 vector of a 3x4 matrix. + static std::vector Estimate(const std::vector& points2D, + const std::vector& points3D); + + // Calculate the squared reprojection error given a set of 2D-3D point + // correspondences and a projection matrix. + // + // @param points2D Normalized 2D image points as Nx2 matrix. + // @param points3D 3D world points as Nx3 matrix. + // @param proj_matrix 3x4 projection matrix. + // @param residuals Output vector of residuals. + static void Residuals(const std::vector& points2D, + const std::vector& points3D, + const M_t& proj_matrix, std::vector* residuals); +}; + +// EPNP solver for the PNP (Perspective-N-Point) problem. The solver needs a +// minimum of 4 2D-3D correspondences. +// +// The algorithm is based on the following paper: +// +// Lepetit, Vincent, Francesc Moreno-Noguer, and Pascal Fua. +// "Epnp: An accurate o (n) solution to the pnp problem." +// International journal of computer vision 81.2 (2009): 155-166. +// +// The implementation is based on their original open-source release, but is +// ported to Eigen and contains several improvements over the original code. +class EPNPEstimator { + public: + // The 2D image feature observations. + typedef Eigen::Vector2d X_t; + // The observed 3D features in the world frame. + typedef Eigen::Vector3d Y_t; + // The transformation from the world to the camera frame. + typedef Eigen::Matrix3x4d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 4; + + // Estimate the most probable solution of the P3P problem from a set of + // three 2D-3D point correspondences. + // + // @param points2D Normalized 2D image points as 3x2 matrix. + // @param points3D 3D world points as 3x3 matrix. + // + // @return Most probable pose as length-1 vector of a 3x4 matrix. + static std::vector Estimate(const std::vector& points2D, + const std::vector& points3D); + + // Calculate the squared reprojection error given a set of 2D-3D point + // correspondences and a projection matrix. + // + // @param points2D Normalized 2D image points as Nx2 matrix. + // @param points3D 3D world points as Nx3 matrix. + // @param proj_matrix 3x4 projection matrix. + // @param residuals Output vector of residuals. + static void Residuals(const std::vector& points2D, + const std::vector& points3D, + const M_t& proj_matrix, std::vector* residuals); + + private: + bool ComputePose(const std::vector& points2D, + const std::vector& points3D, + Eigen::Matrix3x4d* proj_matrix); + + void ChooseControlPoints(); + bool ComputeBarycentricCoordinates(); + + Eigen::Matrix ComputeM(); + Eigen::Matrix ComputeL6x10( + const Eigen::Matrix& Ut); + Eigen::Matrix ComputeRho(); + + void FindBetasApprox1(const Eigen::Matrix& L_6x10, + const Eigen::Matrix& rho, + Eigen::Vector4d* betas); + void FindBetasApprox2(const Eigen::Matrix& L_6x10, + const Eigen::Matrix& rho, + Eigen::Vector4d* betas); + void FindBetasApprox3(const Eigen::Matrix& L_6x10, + const Eigen::Matrix& rho, + Eigen::Vector4d* betas); + + void RunGaussNewton(const Eigen::Matrix& L_6x10, + const Eigen::Matrix& rho, + Eigen::Vector4d* betas); + + double ComputeRT(const Eigen::Matrix& Ut, + const Eigen::Vector4d& betas, Eigen::Matrix3d* R, + Eigen::Vector3d* t); + + void ComputeCcs(const Eigen::Vector4d& betas, + const Eigen::Matrix& Ut); + void ComputePcs(); + + void SolveForSign(); + + void EstimateRT(Eigen::Matrix3d* R, Eigen::Vector3d* t); + + double ComputeTotalReprojectionError(const Eigen::Matrix3d& R, + const Eigen::Vector3d& t); + + const std::vector* points2D_ = nullptr; + const std::vector* points3D_ = nullptr; + std::vector pcs_; + std::vector alphas_; + std::array cws_; + std::array ccs_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_ABSOLUTE_POSE_H_ diff --git a/colmap/include/colmap/estimators/affine_transform.h b/colmap/include/colmap/estimators/affine_transform.h new file mode 100644 index 0000000000000000000000000000000000000000..4381aa3381392e693cdcf122a5a69bf3845b7b8b --- /dev/null +++ b/colmap/include/colmap/estimators/affine_transform.h @@ -0,0 +1,65 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_ESTIMATORS_AFFINE_TRANSFORM_H_ +#define COLMAP_SRC_ESTIMATORS_AFFINE_TRANSFORM_H_ + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +class AffineTransformEstimator { + public: + typedef Eigen::Vector2d X_t; + typedef Eigen::Vector2d Y_t; + typedef Eigen::Matrix M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 3; + + // Estimate the affine transformation from at least 3 correspondences. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Compute the squared transformation error. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& E, + std::vector* residuals); +}; + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_AFFINE_TRANSFORM_H_ diff --git a/colmap/include/colmap/estimators/coordinate_frame.h b/colmap/include/colmap/estimators/coordinate_frame.h new file mode 100644 index 0000000000000000000000000000000000000000..0002e8c91737aadb92b4f87ce343c8b38feac0b8 --- /dev/null +++ b/colmap/include/colmap/estimators/coordinate_frame.h @@ -0,0 +1,88 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_ESTIMATORS_COORDINATE_AXES_H_ +#define COLMAP_SRC_ESTIMATORS_COORDINATE_AXES_H_ + +#include + +#include "base/reconstruction.h" + +namespace colmap { + +struct ManhattanWorldFrameEstimationOptions { + // The maximum image size for line detection. + int max_image_size = 1024; + // The minimum length of line segments in pixels. + double min_line_length = 3; + // The tolerance for classifying lines into horizontal/vertical. + double line_orientation_tolerance = 0.2; + // The maximum distance in pixels between lines and the vanishing points. + double max_line_vp_distance = 0.5; + // The maximum cosine distance between estimated axes to be inliers. + double max_axis_distance = 0.05; +}; + +// Estimate gravity vector by assuming gravity-aligned image orientation, i.e. +// the majority of images is assumed to have the gravity vector aligned with an +// upright image plane. +Eigen::Vector3d EstimateGravityVectorFromImageOrientation( + const Reconstruction& reconstruction, + const double max_axis_distance = 0.05); + +// Estimate the coordinate frame of the reconstruction assuming a Manhattan +// world by finding the major vanishing points in each image. This function +// assumes that the majority of images is taken in upright direction, i.e. +// people are standing upright in the image. The orthonormal axes of the +// estimated coordinate frame will be given in the columns of the returned +// matrix. If one axis could not be determined, the respective column will be +// zero. The axes are specified in the world coordinate system in the order +// rightward, downward, forward. +Eigen::Matrix3d EstimateManhattanWorldFrame( + const ManhattanWorldFrameEstimationOptions& options, + const Reconstruction& reconstruction, const std::string& image_path); + +// Aligns the reconstruction to the plane defined by running PCA on the 3D +// points. The model centroid is at the origin of the new coordinate system +// and the X axis is the first principal component with the Y axis being the +// second principal component +void AlignToPrincipalPlane(Reconstruction* recon, SimilarityTransform3* tform); + +// Aligns the reconstruction to the local ENU plane orientation. Rotates the +// reconstruction such that the x-y plane aligns with the ENU tangent plane at +// the point cloud centroid and translates the origin to the centroid. +// If unscaled == true, then the original scale of the model remains unchanged. +void AlignToENUPlane(Reconstruction* recon, SimilarityTransform3* tform, + bool unscaled); + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_COORDINATE_AXES_H_ diff --git a/colmap/include/colmap/estimators/essential_matrix.h b/colmap/include/colmap/estimators/essential_matrix.h new file mode 100644 index 0000000000000000000000000000000000000000..f58ed0e8ba3ad9284bd145f0c1982c410a2c12ae --- /dev/null +++ b/colmap/include/colmap/estimators/essential_matrix.h @@ -0,0 +1,127 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_ESTIMATORS_ESSENTIAL_MATRIX_H_ +#define COLMAP_SRC_ESTIMATORS_ESSENTIAL_MATRIX_H_ + +#include + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Essential matrix estimator from corresponding normalized point pairs. +// +// This algorithm solves the 5-Point problem based on the following paper: +// +// D. Nister, An efficient solution to the five-point relative pose problem, +// IEEE-T-PAMI, 26(6), 2004. +// http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.86.8769 +class EssentialMatrixFivePointEstimator { + public: + typedef Eigen::Vector2d X_t; + typedef Eigen::Vector2d Y_t; + typedef Eigen::Matrix3d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 5; + + // Estimate up to 10 possible essential matrix solutions from a set of + // corresponding points. + // + // The number of corresponding points must be at least 5. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points. + // + // @return Up to 10 solutions as a vector of 3x3 essential matrices. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Calculate the residuals of a set of corresponding points and a given + // essential matrix. + // + // Residuals are defined as the squared Sampson error. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points. + // @param E 3x3 essential matrix. + // @param residuals Output vector of residuals. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& E, + std::vector* residuals); +}; + +// Essential matrix estimator from corresponding normalized point pairs. +// +// This algorithm solves the 8-Point problem based on the following paper: +// +// Hartley and Zisserman, Multiple View Geometry, algorithm 11.1, page 282. +class EssentialMatrixEightPointEstimator { + public: + typedef Eigen::Vector2d X_t; + typedef Eigen::Vector2d Y_t; + typedef Eigen::Matrix3d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 8; + + // Estimate essential matrix solutions from set of corresponding points. + // + // The number of corresponding points must be at least 8. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Calculate the residuals of a set of corresponding points and a given + // essential matrix. + // + // Residuals are defined as the squared Sampson error. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points. + // @param E 3x3 essential matrix. + // @param residuals Output vector of residuals. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& E, + std::vector* residuals); +}; + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_ESSENTIAL_MATRIX_H_ diff --git a/colmap/include/colmap/estimators/essential_matrix_coeffs.h b/colmap/include/colmap/estimators/essential_matrix_coeffs.h new file mode 100644 index 0000000000000000000000000000000000000000..013ae18457b9958a40aaa9cca6d7b042d94b9d47 --- /dev/null +++ b/colmap/include/colmap/estimators/essential_matrix_coeffs.h @@ -0,0 +1,206 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +{ + const double* b = B.data(); + + coeffs(0) = b[0] * b[17] * b[34] + b[26] * b[4] * b[21] - + b[26] * b[17] * b[8] - b[13] * b[4] * b[34] - + b[0] * b[21] * b[30] + b[13] * b[30] * b[8]; + coeffs(1) = + b[26] * b[4] * b[22] + b[14] * b[30] * b[8] + b[13] * b[31] * b[8] + + b[1] * b[17] * b[34] - b[13] * b[5] * b[34] + b[26] * b[5] * b[21] - + b[0] * b[21] * b[31] - b[26] * b[17] * b[9] - b[1] * b[21] * b[30] + + b[27] * b[4] * b[21] + b[0] * b[17] * b[35] - b[0] * b[22] * b[30] + + b[13] * b[30] * b[9] + b[0] * b[18] * b[34] - b[27] * b[17] * b[8] - + b[14] * b[4] * b[34] - b[13] * b[4] * b[35] - b[26] * b[18] * b[8]; + coeffs(2) = + b[14] * b[30] * b[9] + b[14] * b[31] * b[8] + b[13] * b[31] * b[9] - + b[13] * b[4] * b[36] - b[13] * b[5] * b[35] + b[15] * b[30] * b[8] - + b[13] * b[6] * b[34] + b[13] * b[30] * b[10] + b[13] * b[32] * b[8] - + b[14] * b[4] * b[35] - b[14] * b[5] * b[34] + b[26] * b[4] * b[23] + + b[26] * b[5] * b[22] + b[26] * b[6] * b[21] - b[26] * b[17] * b[10] - + b[15] * b[4] * b[34] - b[26] * b[18] * b[9] - b[26] * b[19] * b[8] + + b[27] * b[4] * b[22] + b[27] * b[5] * b[21] - b[27] * b[17] * b[9] - + b[27] * b[18] * b[8] - b[1] * b[21] * b[31] - b[0] * b[23] * b[30] - + b[0] * b[21] * b[32] + b[28] * b[4] * b[21] - b[28] * b[17] * b[8] + + b[2] * b[17] * b[34] + b[0] * b[18] * b[35] - b[0] * b[22] * b[31] + + b[0] * b[17] * b[36] + b[0] * b[19] * b[34] - b[1] * b[22] * b[30] + + b[1] * b[18] * b[34] + b[1] * b[17] * b[35] - b[2] * b[21] * b[30]; + coeffs(3) = + b[14] * b[30] * b[10] + b[14] * b[32] * b[8] - b[3] * b[21] * b[30] + + b[3] * b[17] * b[34] + b[13] * b[32] * b[9] + b[13] * b[33] * b[8] - + b[13] * b[4] * b[37] - b[13] * b[5] * b[36] + b[15] * b[30] * b[9] + + b[15] * b[31] * b[8] - b[16] * b[4] * b[34] - b[13] * b[6] * b[35] - + b[13] * b[7] * b[34] + b[13] * b[30] * b[11] + b[13] * b[31] * b[10] + + b[14] * b[31] * b[9] - b[14] * b[4] * b[36] - b[14] * b[5] * b[35] - + b[14] * b[6] * b[34] + b[16] * b[30] * b[8] - b[26] * b[20] * b[8] + + b[26] * b[4] * b[24] + b[26] * b[5] * b[23] + b[26] * b[6] * b[22] + + b[26] * b[7] * b[21] - b[26] * b[17] * b[11] - b[15] * b[4] * b[35] - + b[15] * b[5] * b[34] - b[26] * b[18] * b[10] - b[26] * b[19] * b[9] + + b[27] * b[4] * b[23] + b[27] * b[5] * b[22] + b[27] * b[6] * b[21] - + b[27] * b[17] * b[10] - b[27] * b[18] * b[9] - b[27] * b[19] * b[8] + + b[0] * b[17] * b[37] - b[0] * b[23] * b[31] - b[0] * b[24] * b[30] - + b[0] * b[21] * b[33] - b[29] * b[17] * b[8] + b[28] * b[4] * b[22] + + b[28] * b[5] * b[21] - b[28] * b[17] * b[9] - b[28] * b[18] * b[8] + + b[29] * b[4] * b[21] + b[1] * b[19] * b[34] - b[2] * b[21] * b[31] + + b[0] * b[20] * b[34] + b[0] * b[19] * b[35] + b[0] * b[18] * b[36] - + b[0] * b[22] * b[32] - b[1] * b[23] * b[30] - b[1] * b[21] * b[32] + + b[1] * b[18] * b[35] - b[1] * b[22] * b[31] - b[2] * b[22] * b[30] + + b[2] * b[17] * b[35] + b[1] * b[17] * b[36] + b[2] * b[18] * b[34]; + coeffs(4) = + -b[14] * b[6] * b[35] - b[14] * b[7] * b[34] - b[3] * b[22] * b[30] - + b[3] * b[21] * b[31] + b[3] * b[17] * b[35] + b[3] * b[18] * b[34] + + b[13] * b[32] * b[10] + b[13] * b[33] * b[9] - b[13] * b[4] * b[38] - + b[13] * b[5] * b[37] - b[15] * b[6] * b[34] + b[15] * b[30] * b[10] + + b[15] * b[32] * b[8] - b[16] * b[4] * b[35] - b[13] * b[6] * b[36] - + b[13] * b[7] * b[35] + b[13] * b[31] * b[11] + b[13] * b[30] * b[12] + + b[14] * b[32] * b[9] + b[14] * b[33] * b[8] - b[14] * b[4] * b[37] - + b[14] * b[5] * b[36] + b[16] * b[30] * b[9] + b[16] * b[31] * b[8] - + b[26] * b[20] * b[9] + b[26] * b[4] * b[25] + b[26] * b[5] * b[24] + + b[26] * b[6] * b[23] + b[26] * b[7] * b[22] - b[26] * b[17] * b[12] + + b[14] * b[30] * b[11] + b[14] * b[31] * b[10] + b[15] * b[31] * b[9] - + b[15] * b[4] * b[36] - b[15] * b[5] * b[35] - b[26] * b[18] * b[11] - + b[26] * b[19] * b[10] - b[27] * b[20] * b[8] + b[27] * b[4] * b[24] + + b[27] * b[5] * b[23] + b[27] * b[6] * b[22] + b[27] * b[7] * b[21] - + b[27] * b[17] * b[11] - b[27] * b[18] * b[10] - b[27] * b[19] * b[9] - + b[16] * b[5] * b[34] - b[29] * b[17] * b[9] - b[29] * b[18] * b[8] + + b[28] * b[4] * b[23] + b[28] * b[5] * b[22] + b[28] * b[6] * b[21] - + b[28] * b[17] * b[10] - b[28] * b[18] * b[9] - b[28] * b[19] * b[8] + + b[29] * b[4] * b[22] + b[29] * b[5] * b[21] - b[2] * b[23] * b[30] + + b[2] * b[18] * b[35] - b[1] * b[22] * b[32] - b[2] * b[21] * b[32] + + b[2] * b[19] * b[34] + b[0] * b[19] * b[36] - b[0] * b[22] * b[33] + + b[0] * b[20] * b[35] - b[0] * b[23] * b[32] - b[0] * b[25] * b[30] + + b[0] * b[17] * b[38] + b[0] * b[18] * b[37] - b[0] * b[24] * b[31] + + b[1] * b[17] * b[37] - b[1] * b[23] * b[31] - b[1] * b[24] * b[30] - + b[1] * b[21] * b[33] + b[1] * b[20] * b[34] + b[1] * b[19] * b[35] + + b[1] * b[18] * b[36] + b[2] * b[17] * b[36] - b[2] * b[22] * b[31]; + coeffs(5) = + -b[14] * b[6] * b[36] - b[14] * b[7] * b[35] + b[14] * b[31] * b[11] - + b[3] * b[23] * b[30] - b[3] * b[21] * b[32] + b[3] * b[18] * b[35] - + b[3] * b[22] * b[31] + b[3] * b[17] * b[36] + b[3] * b[19] * b[34] + + b[13] * b[32] * b[11] + b[13] * b[33] * b[10] - b[13] * b[5] * b[38] - + b[15] * b[6] * b[35] - b[15] * b[7] * b[34] + b[15] * b[30] * b[11] + + b[15] * b[31] * b[10] + b[16] * b[31] * b[9] - b[13] * b[6] * b[37] - + b[13] * b[7] * b[36] + b[13] * b[31] * b[12] + b[14] * b[32] * b[10] + + b[14] * b[33] * b[9] - b[14] * b[4] * b[38] - b[14] * b[5] * b[37] - + b[16] * b[6] * b[34] + b[16] * b[30] * b[10] + b[16] * b[32] * b[8] - + b[26] * b[20] * b[10] + b[26] * b[5] * b[25] + b[26] * b[6] * b[24] + + b[26] * b[7] * b[23] + b[14] * b[30] * b[12] + b[15] * b[32] * b[9] + + b[15] * b[33] * b[8] - b[15] * b[4] * b[37] - b[15] * b[5] * b[36] + + b[29] * b[5] * b[22] + b[29] * b[6] * b[21] - b[26] * b[18] * b[12] - + b[26] * b[19] * b[11] - b[27] * b[20] * b[9] + b[27] * b[4] * b[25] + + b[27] * b[5] * b[24] + b[27] * b[6] * b[23] + b[27] * b[7] * b[22] - + b[27] * b[17] * b[12] - b[27] * b[18] * b[11] - b[27] * b[19] * b[10] - + b[28] * b[20] * b[8] - b[16] * b[4] * b[36] - b[16] * b[5] * b[35] - + b[29] * b[17] * b[10] - b[29] * b[18] * b[9] - b[29] * b[19] * b[8] + + b[28] * b[4] * b[24] + b[28] * b[5] * b[23] + b[28] * b[6] * b[22] + + b[28] * b[7] * b[21] - b[28] * b[17] * b[11] - b[28] * b[18] * b[10] - + b[28] * b[19] * b[9] + b[29] * b[4] * b[23] - b[2] * b[22] * b[32] - + b[2] * b[21] * b[33] - b[1] * b[24] * b[31] + b[0] * b[18] * b[38] - + b[0] * b[24] * b[32] + b[0] * b[19] * b[37] + b[0] * b[20] * b[36] - + b[0] * b[25] * b[31] - b[0] * b[23] * b[33] + b[1] * b[19] * b[36] - + b[1] * b[22] * b[33] + b[1] * b[20] * b[35] + b[2] * b[19] * b[35] - + b[2] * b[24] * b[30] - b[2] * b[23] * b[31] + b[2] * b[20] * b[34] + + b[2] * b[17] * b[37] - b[1] * b[25] * b[30] + b[1] * b[18] * b[37] + + b[1] * b[17] * b[38] - b[1] * b[23] * b[32] + b[2] * b[18] * b[36]; + coeffs(6) = + -b[14] * b[6] * b[37] - b[14] * b[7] * b[36] + b[14] * b[31] * b[12] + + b[3] * b[17] * b[37] - b[3] * b[23] * b[31] - b[3] * b[24] * b[30] - + b[3] * b[21] * b[33] + b[3] * b[20] * b[34] + b[3] * b[19] * b[35] + + b[3] * b[18] * b[36] - b[3] * b[22] * b[32] + b[13] * b[32] * b[12] + + b[13] * b[33] * b[11] - b[15] * b[6] * b[36] - b[15] * b[7] * b[35] + + b[15] * b[31] * b[11] + b[15] * b[30] * b[12] + b[16] * b[32] * b[9] + + b[16] * b[33] * b[8] - b[13] * b[6] * b[38] - b[13] * b[7] * b[37] + + b[14] * b[32] * b[11] + b[14] * b[33] * b[10] - b[14] * b[5] * b[38] - + b[16] * b[6] * b[35] - b[16] * b[7] * b[34] + b[16] * b[30] * b[11] + + b[16] * b[31] * b[10] - b[26] * b[19] * b[12] - b[26] * b[20] * b[11] + + b[26] * b[6] * b[25] + b[26] * b[7] * b[24] + b[15] * b[32] * b[10] + + b[15] * b[33] * b[9] - b[15] * b[4] * b[38] - b[15] * b[5] * b[37] + + b[29] * b[5] * b[23] + b[29] * b[6] * b[22] + b[29] * b[7] * b[21] - + b[27] * b[20] * b[10] + b[27] * b[5] * b[25] + b[27] * b[6] * b[24] + + b[27] * b[7] * b[23] - b[27] * b[18] * b[12] - b[27] * b[19] * b[11] - + b[28] * b[20] * b[9] - b[16] * b[4] * b[37] - b[16] * b[5] * b[36] + + b[0] * b[19] * b[38] - b[0] * b[24] * b[33] + b[0] * b[20] * b[37] - + b[29] * b[17] * b[11] - b[29] * b[18] * b[10] - b[29] * b[19] * b[9] + + b[28] * b[4] * b[25] + b[28] * b[5] * b[24] + b[28] * b[6] * b[23] + + b[28] * b[7] * b[22] - b[28] * b[17] * b[12] - b[28] * b[18] * b[11] - + b[28] * b[19] * b[10] - b[29] * b[20] * b[8] + b[29] * b[4] * b[24] + + b[2] * b[18] * b[37] - b[0] * b[25] * b[32] + b[1] * b[18] * b[38] - + b[1] * b[24] * b[32] + b[1] * b[19] * b[37] + b[1] * b[20] * b[36] - + b[1] * b[25] * b[31] + b[2] * b[17] * b[38] + b[2] * b[19] * b[36] - + b[2] * b[24] * b[31] - b[2] * b[22] * b[33] - b[2] * b[23] * b[32] + + b[2] * b[20] * b[35] - b[1] * b[23] * b[33] - b[2] * b[25] * b[30]; + coeffs(7) = + -b[14] * b[6] * b[38] - b[14] * b[7] * b[37] + b[3] * b[19] * b[36] - + b[3] * b[22] * b[33] + b[3] * b[20] * b[35] - b[3] * b[23] * b[32] - + b[3] * b[25] * b[30] + b[3] * b[17] * b[38] + b[3] * b[18] * b[37] - + b[3] * b[24] * b[31] - b[15] * b[6] * b[37] - b[15] * b[7] * b[36] + + b[15] * b[31] * b[12] + b[16] * b[32] * b[10] + b[16] * b[33] * b[9] + + b[13] * b[33] * b[12] - b[13] * b[7] * b[38] + b[14] * b[32] * b[12] + + b[14] * b[33] * b[11] - b[16] * b[6] * b[36] - b[16] * b[7] * b[35] + + b[16] * b[31] * b[11] + b[16] * b[30] * b[12] + b[15] * b[32] * b[11] + + b[15] * b[33] * b[10] - b[15] * b[5] * b[38] + b[29] * b[5] * b[24] + + b[29] * b[6] * b[23] - b[26] * b[20] * b[12] + b[26] * b[7] * b[25] - + b[27] * b[19] * b[12] - b[27] * b[20] * b[11] + b[27] * b[6] * b[25] + + b[27] * b[7] * b[24] - b[28] * b[20] * b[10] - b[16] * b[4] * b[38] - + b[16] * b[5] * b[37] + b[29] * b[7] * b[22] - b[29] * b[17] * b[12] - + b[29] * b[18] * b[11] - b[29] * b[19] * b[10] + b[28] * b[5] * b[25] + + b[28] * b[6] * b[24] + b[28] * b[7] * b[23] - b[28] * b[18] * b[12] - + b[28] * b[19] * b[11] - b[29] * b[20] * b[9] + b[29] * b[4] * b[25] - + b[2] * b[24] * b[32] + b[0] * b[20] * b[38] - b[0] * b[25] * b[33] + + b[1] * b[19] * b[38] - b[1] * b[24] * b[33] + b[1] * b[20] * b[37] - + b[2] * b[25] * b[31] + b[2] * b[20] * b[36] - b[1] * b[25] * b[32] + + b[2] * b[19] * b[37] + b[2] * b[18] * b[38] - b[2] * b[23] * b[33]; + coeffs(8) = + b[3] * b[18] * b[38] - b[3] * b[24] * b[32] + b[3] * b[19] * b[37] + + b[3] * b[20] * b[36] - b[3] * b[25] * b[31] - b[3] * b[23] * b[33] - + b[15] * b[6] * b[38] - b[15] * b[7] * b[37] + b[16] * b[32] * b[11] + + b[16] * b[33] * b[10] - b[16] * b[5] * b[38] - b[16] * b[6] * b[37] - + b[16] * b[7] * b[36] + b[16] * b[31] * b[12] + b[14] * b[33] * b[12] - + b[14] * b[7] * b[38] + b[15] * b[32] * b[12] + b[15] * b[33] * b[11] + + b[29] * b[5] * b[25] + b[29] * b[6] * b[24] - b[27] * b[20] * b[12] + + b[27] * b[7] * b[25] - b[28] * b[19] * b[12] - b[28] * b[20] * b[11] + + b[29] * b[7] * b[23] - b[29] * b[18] * b[12] - b[29] * b[19] * b[11] + + b[28] * b[6] * b[25] + b[28] * b[7] * b[24] - b[29] * b[20] * b[10] + + b[2] * b[19] * b[38] - b[1] * b[25] * b[33] + b[2] * b[20] * b[37] - + b[2] * b[24] * b[33] - b[2] * b[25] * b[32] + b[1] * b[20] * b[38]; + coeffs(9) = + b[29] * b[7] * b[24] - b[29] * b[20] * b[11] + b[2] * b[20] * b[38] - + b[2] * b[25] * b[33] - b[28] * b[20] * b[12] + b[28] * b[7] * b[25] - + b[29] * b[19] * b[12] - b[3] * b[24] * b[33] + b[15] * b[33] * b[12] + + b[3] * b[19] * b[38] - b[16] * b[6] * b[38] + b[3] * b[20] * b[37] + + b[16] * b[32] * b[12] + b[29] * b[6] * b[25] - b[16] * b[7] * b[37] - + b[3] * b[25] * b[32] - b[15] * b[7] * b[38] + b[16] * b[33] * b[11]; + coeffs(10) = -b[29] * b[20] * b[12] + b[29] * b[7] * b[25] + + b[16] * b[33] * b[12] - b[16] * b[7] * b[38] + + b[3] * b[20] * b[38] - b[3] * b[25] * b[33]; +} diff --git a/colmap/include/colmap/estimators/essential_matrix_poly.h b/colmap/include/colmap/estimators/essential_matrix_poly.h new file mode 100644 index 0000000000000000000000000000000000000000..56648d639f686001b3f650491a7c7f0d47c75978 --- /dev/null +++ b/colmap/include/colmap/estimators/essential_matrix_poly.h @@ -0,0 +1,2115 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +{ + double* a = A.data(); + const double* e = E.data(); + + double e2[36]; + double e3[36]; + for (size_t i = 0; i < 36; ++i) { + e2[i] = e[i] * e[i]; + e3[i] = e2[i] * e[i]; + } + + a[190] = e[33] * e[28] * e[32] - e[33] * e[31] * e[29] + + e[30] * e[34] * e[29] - e[30] * e[28] * e[35] - + e[27] * e[32] * e[34] + e[27] * e[31] * e[35]; + a[7] = 0.5 * e[6] * e2[8] - 0.5 * e[6] * e2[5] + 0.5 * e3[6] + + 0.5 * e[6] * e2[7] - 0.5 * e[6] * e2[4] + e[0] * e[2] * e[8] + + e[3] * e[4] * e[7] + e[3] * e[5] * e[8] + e[0] * e[1] * e[7] - + 0.5 * e[6] * e2[1] - 0.5 * e[6] * e2[2] + 0.5 * e2[0] * e[6] + + 0.5 * e2[3] * e[6]; + a[120] = e[30] * e[34] * e[2] + e[33] * e[1] * e[32] - e[3] * e[28] * e[35] + + e[0] * e[31] * e[35] + e[3] * e[34] * e[29] - e[30] * e[1] * e[35] + + e[27] * e[31] * e[8] - e[27] * e[32] * e[7] - e[30] * e[28] * e[8] - + e[33] * e[31] * e[2] - e[0] * e[32] * e[34] + e[6] * e[28] * e[32] - + e[33] * e[4] * e[29] + e[33] * e[28] * e[5] + e[30] * e[7] * e[29] + + e[27] * e[4] * e[35] - e[27] * e[5] * e[34] - e[6] * e[31] * e[29]; + a[77] = + e[9] * e[27] * e[15] + e[9] * e[29] * e[17] + e[9] * e[11] * e[35] + + e[9] * e[28] * e[16] + e[9] * e[10] * e[34] + e[27] * e[11] * e[17] + + e[27] * e[10] * e[16] + e[12] * e[30] * e[15] + e[12] * e[32] * e[17] + + e[12] * e[14] * e[35] + e[12] * e[31] * e[16] + e[12] * e[13] * e[34] + + e[30] * e[14] * e[17] + e[30] * e[13] * e[16] + e[15] * e[35] * e[17] + + e[15] * e[34] * e[16] - e[15] * e[28] * e[10] - e[15] * e[31] * e[13] - + e[15] * e[32] * e[14] - e[15] * e[29] * e[11] + 0.5 * e2[9] * e[33] + + 0.5 * e[33] * e2[16] - 0.5 * e[33] * e2[11] + 0.5 * e[33] * e2[12] + + 1.5 * e[33] * e2[15] + 0.5 * e[33] * e2[17] - 0.5 * e[33] * e2[10] - + 0.5 * e[33] * e2[14] - 0.5 * e[33] * e2[13]; + a[180] = + -e[33] * e[22] * e[29] - e[33] * e[31] * e[20] - e[27] * e[32] * e[25] + + e[27] * e[22] * e[35] - e[27] * e[23] * e[34] + e[27] * e[31] * e[26] + + e[33] * e[28] * e[23] - e[21] * e[28] * e[35] + e[30] * e[25] * e[29] + + e[24] * e[28] * e[32] - e[24] * e[31] * e[29] + e[18] * e[31] * e[35] - + e[30] * e[28] * e[26] - e[30] * e[19] * e[35] + e[21] * e[34] * e[29] + + e[33] * e[19] * e[32] - e[18] * e[32] * e[34] + e[30] * e[34] * e[20]; + a[87] = e[18] * e[2] * e[17] + e[3] * e[21] * e[15] + e[3] * e[12] * e[24] + + e[3] * e[23] * e[17] + e[3] * e[14] * e[26] + e[3] * e[22] * e[16] + + e[3] * e[13] * e[25] + 3. * e[6] * e[24] * e[15] + + e[6] * e[26] * e[17] + e[6] * e[25] * e[16] + e[0] * e[20] * e[17] + + e[0] * e[11] * e[26] + e[0] * e[19] * e[16] + e[0] * e[10] * e[25] + + e[15] * e[26] * e[8] - e[15] * e[20] * e[2] - e[15] * e[19] * e[1] - + e[15] * e[22] * e[4] + e[15] * e[25] * e[7] - e[15] * e[23] * e[5] + + e[12] * e[21] * e[6] + e[12] * e[22] * e[7] + e[12] * e[4] * e[25] + + e[12] * e[23] * e[8] + e[12] * e[5] * e[26] - e[24] * e[11] * e[2] - + e[24] * e[10] * e[1] - e[24] * e[13] * e[4] + e[24] * e[16] * e[7] - + e[24] * e[14] * e[5] + e[24] * e[17] * e[8] + e[21] * e[13] * e[7] + + e[21] * e[4] * e[16] + e[21] * e[14] * e[8] + e[21] * e[5] * e[17] - + e[6] * e[23] * e[14] - e[6] * e[20] * e[11] - e[6] * e[19] * e[10] - + e[6] * e[22] * e[13] + e[9] * e[18] * e[6] + e[9] * e[0] * e[24] + + e[9] * e[19] * e[7] + e[9] * e[1] * e[25] + e[9] * e[20] * e[8] + + e[9] * e[2] * e[26] + e[18] * e[0] * e[15] + e[18] * e[10] * e[7] + + e[18] * e[1] * e[16] + e[18] * e[11] * e[8]; + a[150] = + e[33] * e[10] * e[32] + e[33] * e[28] * e[14] - e[33] * e[13] * e[29] - + e[33] * e[31] * e[11] + e[9] * e[31] * e[35] - e[9] * e[32] * e[34] + + e[27] * e[13] * e[35] - e[27] * e[32] * e[16] + e[27] * e[31] * e[17] - + e[27] * e[14] * e[34] + e[12] * e[34] * e[29] - e[12] * e[28] * e[35] + + e[30] * e[34] * e[11] + e[30] * e[16] * e[29] - e[30] * e[10] * e[35] - + e[30] * e[28] * e[17] + e[15] * e[28] * e[32] - e[15] * e[31] * e[29]; + a[57] = e[0] * e[27] * e[6] + e[0] * e[28] * e[7] + e[0] * e[1] * e[34] + + e[0] * e[29] * e[8] + e[0] * e[2] * e[35] + e[6] * e[34] * e[7] - + e[6] * e[32] * e[5] + e[6] * e[30] * e[3] + e[6] * e[35] * e[8] - + e[6] * e[29] * e[2] - e[6] * e[28] * e[1] - e[6] * e[31] * e[4] + + e[27] * e[1] * e[7] + e[27] * e[2] * e[8] + e[3] * e[31] * e[7] + + e[3] * e[4] * e[34] + e[3] * e[32] * e[8] + e[3] * e[5] * e[35] + + e[30] * e[4] * e[7] + e[30] * e[5] * e[8] + 0.5 * e2[0] * e[33] + + 1.5 * e[33] * e2[6] - 0.5 * e[33] * e2[4] - 0.5 * e[33] * e2[5] - + 0.5 * e[33] * e2[1] + 0.5 * e[33] * e2[7] + 0.5 * e[33] * e2[3] - + 0.5 * e[33] * e2[2] + 0.5 * e[33] * e2[8]; + a[80] = -e[0] * e[23] * e[16] + e[9] * e[4] * e[26] + e[9] * e[22] * e[8] - + e[9] * e[5] * e[25] - e[9] * e[23] * e[7] + e[18] * e[4] * e[17] + + e[18] * e[13] * e[8] - e[18] * e[5] * e[16] - e[18] * e[14] * e[7] + + e[3] * e[16] * e[20] + e[3] * e[25] * e[11] - e[3] * e[10] * e[26] - + e[3] * e[19] * e[17] + e[12] * e[7] * e[20] + e[12] * e[25] * e[2] - + e[12] * e[1] * e[26] - e[12] * e[19] * e[8] + e[21] * e[7] * e[11] + + e[21] * e[16] * e[2] - e[21] * e[1] * e[17] - e[21] * e[10] * e[8] + + e[6] * e[10] * e[23] + e[6] * e[19] * e[14] - e[6] * e[13] * e[20] - + e[6] * e[22] * e[11] + e[15] * e[1] * e[23] + e[15] * e[19] * e[5] - + e[15] * e[4] * e[20] - e[15] * e[22] * e[2] + e[24] * e[1] * e[14] + + e[24] * e[10] * e[5] - e[24] * e[4] * e[11] - e[24] * e[13] * e[2] + + e[0] * e[13] * e[26] + e[0] * e[22] * e[17] - e[0] * e[14] * e[25]; + a[167] = e[18] * e[19] * e[25] + 0.5 * e3[24] - 0.5 * e[24] * e2[23] + + e[18] * e[20] * e[26] + e[21] * e[22] * e[25] + + e[21] * e[23] * e[26] - 0.5 * e[24] * e2[19] + 0.5 * e2[21] * e[24] + + 0.5 * e[24] * e2[26] - 0.5 * e[24] * e2[20] + 0.5 * e2[18] * e[24] - + 0.5 * e[24] * e2[22] + 0.5 * e[24] * e2[25]; + a[50] = -e[3] * e[1] * e[35] - e[0] * e[32] * e[7] + e[27] * e[4] * e[8] + + e[33] * e[1] * e[5] - e[33] * e[4] * e[2] + e[0] * e[4] * e[35] + + e[3] * e[34] * e[2] - e[30] * e[1] * e[8] + e[30] * e[7] * e[2] - + e[6] * e[4] * e[29] + e[3] * e[7] * e[29] + e[6] * e[1] * e[32] - + e[0] * e[5] * e[34] - e[3] * e[28] * e[8] + e[0] * e[31] * e[8] + + e[6] * e[28] * e[5] - e[6] * e[31] * e[2] - e[27] * e[5] * e[7]; + a[97] = e[33] * e[16] * e[7] - e[33] * e[14] * e[5] + e[33] * e[17] * e[8] + + e[30] * e[13] * e[7] + e[30] * e[4] * e[16] + e[30] * e[14] * e[8] + + e[30] * e[5] * e[17] + e[6] * e[27] * e[9] - e[6] * e[28] * e[10] - + e[6] * e[31] * e[13] - e[6] * e[32] * e[14] - e[6] * e[29] * e[11] + + e[9] * e[28] * e[7] + e[9] * e[1] * e[34] + e[9] * e[29] * e[8] + + e[9] * e[2] * e[35] + e[27] * e[10] * e[7] + e[27] * e[1] * e[16] + + e[27] * e[11] * e[8] + e[27] * e[2] * e[17] + e[3] * e[30] * e[15] + + e[3] * e[12] * e[33] + e[3] * e[32] * e[17] + e[3] * e[14] * e[35] + + e[3] * e[31] * e[16] + e[3] * e[13] * e[34] + + 3. * e[6] * e[33] * e[15] + e[6] * e[35] * e[17] + + e[6] * e[34] * e[16] + e[0] * e[27] * e[15] + e[0] * e[9] * e[33] + + e[0] * e[29] * e[17] + e[0] * e[11] * e[35] + e[0] * e[28] * e[16] + + e[0] * e[10] * e[34] + e[15] * e[34] * e[7] - e[15] * e[32] * e[5] + + e[15] * e[35] * e[8] - e[15] * e[29] * e[2] - e[15] * e[28] * e[1] - + e[15] * e[31] * e[4] + e[12] * e[30] * e[6] + e[12] * e[31] * e[7] + + e[12] * e[4] * e[34] + e[12] * e[32] * e[8] + e[12] * e[5] * e[35] - + e[33] * e[11] * e[2] - e[33] * e[10] * e[1] - e[33] * e[13] * e[4]; + a[0] = e[6] * e[1] * e[5] - e[6] * e[4] * e[2] + e[3] * e[7] * e[2] + + e[0] * e[4] * e[8] - e[0] * e[5] * e[7] - e[3] * e[1] * e[8]; + a[17] = 0.5 * e3[15] + e[9] * e[10] * e[16] - 0.5 * e[15] * e2[11] + + e[9] * e[11] * e[17] + 0.5 * e2[12] * e[15] + 0.5 * e[15] * e2[16] + + 0.5 * e[15] * e2[17] - 0.5 * e[15] * e2[13] + 0.5 * e2[9] * e[15] + + e[12] * e[14] * e[17] - 0.5 * e[15] * e2[10] - 0.5 * e[15] * e2[14] + + e[12] * e[13] * e[16]; + a[70] = + e[15] * e[28] * e[14] - e[15] * e[13] * e[29] - e[15] * e[31] * e[11] + + e[33] * e[10] * e[14] - e[33] * e[13] * e[11] + e[9] * e[13] * e[35] - + e[9] * e[32] * e[16] + e[9] * e[31] * e[17] - e[9] * e[14] * e[34] + + e[27] * e[13] * e[17] - e[27] * e[14] * e[16] + e[12] * e[34] * e[11] + + e[12] * e[16] * e[29] - e[12] * e[10] * e[35] - e[12] * e[28] * e[17] + + e[30] * e[16] * e[11] - e[30] * e[10] * e[17] + e[15] * e[10] * e[32]; + a[177] = + e[18] * e[27] * e[24] + e[18] * e[28] * e[25] + e[18] * e[19] * e[34] + + e[18] * e[29] * e[26] + e[18] * e[20] * e[35] + e[27] * e[19] * e[25] + + e[27] * e[20] * e[26] + e[21] * e[30] * e[24] + e[21] * e[31] * e[25] + + e[21] * e[22] * e[34] + e[21] * e[32] * e[26] + e[21] * e[23] * e[35] + + e[30] * e[22] * e[25] + e[30] * e[23] * e[26] + e[24] * e[34] * e[25] + + e[24] * e[35] * e[26] - e[24] * e[29] * e[20] - e[24] * e[31] * e[22] - + e[24] * e[32] * e[23] - e[24] * e[28] * e[19] + 1.5 * e[33] * e2[24] + + 0.5 * e[33] * e2[25] + 0.5 * e[33] * e2[26] - 0.5 * e[33] * e2[23] - + 0.5 * e[33] * e2[19] - 0.5 * e[33] * e2[20] - 0.5 * e[33] * e2[22] + + 0.5 * e2[18] * e[33] + 0.5 * e2[21] * e[33]; + a[170] = + e[21] * e[25] * e[29] - e[27] * e[23] * e[25] + e[24] * e[19] * e[32] - + e[21] * e[28] * e[26] - e[21] * e[19] * e[35] + e[18] * e[31] * e[26] - + e[30] * e[19] * e[26] - e[24] * e[31] * e[20] + e[24] * e[28] * e[23] + + e[27] * e[22] * e[26] + e[30] * e[25] * e[20] - e[33] * e[22] * e[20] + + e[33] * e[19] * e[23] + e[21] * e[34] * e[20] - e[18] * e[23] * e[34] - + e[24] * e[22] * e[29] - e[18] * e[32] * e[25] + e[18] * e[22] * e[35]; + a[37] = e[12] * e[14] * e[8] + e[12] * e[5] * e[17] + e[15] * e[16] * e[7] + + e[15] * e[17] * e[8] + e[0] * e[11] * e[17] + e[0] * e[9] * e[15] + + e[0] * e[10] * e[16] + e[3] * e[14] * e[17] + e[3] * e[13] * e[16] + + e[9] * e[10] * e[7] + e[9] * e[1] * e[16] + e[9] * e[11] * e[8] + + e[9] * e[2] * e[17] - e[15] * e[11] * e[2] - e[15] * e[10] * e[1] - + e[15] * e[13] * e[4] - e[15] * e[14] * e[5] + e[12] * e[3] * e[15] + + e[12] * e[13] * e[7] + e[12] * e[4] * e[16] + 0.5 * e2[12] * e[6] + + 1.5 * e2[15] * e[6] + 0.5 * e[6] * e2[17] + 0.5 * e[6] * e2[16] + + 0.5 * e[6] * e2[9] - 0.5 * e[6] * e2[11] - 0.5 * e[6] * e2[10] - + 0.5 * e[6] * e2[14] - 0.5 * e[6] * e2[13]; + a[10] = -e[9] * e[14] * e[16] - e[12] * e[10] * e[17] + e[9] * e[13] * e[17] - + e[15] * e[13] * e[11] + e[15] * e[10] * e[14] + e[12] * e[16] * e[11]; + a[67] = e[21] * e[14] * e[17] + e[21] * e[13] * e[16] + + e[15] * e[26] * e[17] + e[15] * e[25] * e[16] - + e[15] * e[23] * e[14] - e[15] * e[20] * e[11] - + e[15] * e[19] * e[10] - e[15] * e[22] * e[13] + e[9] * e[20] * e[17] + + e[9] * e[11] * e[26] + e[9] * e[19] * e[16] + e[9] * e[10] * e[25] + + 0.5 * e2[12] * e[24] + 1.5 * e[24] * e2[15] + 0.5 * e[24] * e2[17] + + 0.5 * e[24] * e2[16] + 0.5 * e2[9] * e[24] - 0.5 * e[24] * e2[11] - + 0.5 * e[24] * e2[10] - 0.5 * e[24] * e2[14] - 0.5 * e[24] * e2[13] + + e[18] * e[11] * e[17] + e[18] * e[9] * e[15] + e[18] * e[10] * e[16] + + e[12] * e[21] * e[15] + e[12] * e[23] * e[17] + + e[12] * e[14] * e[26] + e[12] * e[22] * e[16] + e[12] * e[13] * e[25]; + a[90] = -e[9] * e[5] * e[34] + e[9] * e[31] * e[8] - e[9] * e[32] * e[7] + + e[27] * e[4] * e[17] + e[27] * e[13] * e[8] - e[27] * e[5] * e[16] - + e[27] * e[14] * e[7] + e[0] * e[13] * e[35] - e[0] * e[32] * e[16] + + e[0] * e[31] * e[17] - e[0] * e[14] * e[34] + e[9] * e[4] * e[35] + + e[6] * e[10] * e[32] + e[6] * e[28] * e[14] - e[6] * e[13] * e[29] - + e[6] * e[31] * e[11] + e[15] * e[1] * e[32] + e[3] * e[34] * e[11] + + e[3] * e[16] * e[29] - e[3] * e[10] * e[35] - e[3] * e[28] * e[17] - + e[12] * e[1] * e[35] + e[12] * e[7] * e[29] + e[12] * e[34] * e[2] - + e[12] * e[28] * e[8] + e[15] * e[28] * e[5] - e[15] * e[4] * e[29] - + e[15] * e[31] * e[2] + e[33] * e[1] * e[14] + e[33] * e[10] * e[5] - + e[33] * e[4] * e[11] - e[33] * e[13] * e[2] + e[30] * e[7] * e[11] + + e[30] * e[16] * e[2] - e[30] * e[1] * e[17] - e[30] * e[10] * e[8]; + a[117] = e[21] * e[31] * e[7] + e[21] * e[4] * e[34] + e[21] * e[32] * e[8] + + e[21] * e[5] * e[35] + e[30] * e[22] * e[7] + e[30] * e[4] * e[25] + + e[30] * e[23] * e[8] + e[30] * e[5] * e[26] + + 3. * e[24] * e[33] * e[6] + e[24] * e[34] * e[7] + + e[24] * e[35] * e[8] + e[33] * e[25] * e[7] + e[33] * e[26] * e[8] + + e[0] * e[27] * e[24] + e[0] * e[18] * e[33] + e[0] * e[28] * e[25] + + e[0] * e[19] * e[34] + e[0] * e[29] * e[26] + e[0] * e[20] * e[35] + + e[18] * e[27] * e[6] + e[18] * e[28] * e[7] + e[18] * e[1] * e[34] + + e[18] * e[29] * e[8] + e[18] * e[2] * e[35] + e[27] * e[19] * e[7] + + e[27] * e[1] * e[25] + e[27] * e[20] * e[8] + e[27] * e[2] * e[26] + + e[3] * e[30] * e[24] + e[3] * e[21] * e[33] + e[3] * e[31] * e[25] + + e[3] * e[22] * e[34] + e[3] * e[32] * e[26] + e[3] * e[23] * e[35] + + e[6] * e[30] * e[21] - e[6] * e[29] * e[20] + e[6] * e[35] * e[26] - + e[6] * e[31] * e[22] - e[6] * e[32] * e[23] - e[6] * e[28] * e[19] + + e[6] * e[34] * e[25] - e[24] * e[32] * e[5] - e[24] * e[29] * e[2] - + e[24] * e[28] * e[1] - e[24] * e[31] * e[4] - e[33] * e[20] * e[2] - + e[33] * e[19] * e[1] - e[33] * e[22] * e[4] - e[33] * e[23] * e[5]; + a[160] = e[21] * e[25] * e[20] - e[21] * e[19] * e[26] + + e[18] * e[22] * e[26] - e[18] * e[23] * e[25] - + e[24] * e[22] * e[20] + e[24] * e[19] * e[23]; + a[47] = e[3] * e[4] * e[25] + e[3] * e[23] * e[8] + e[3] * e[5] * e[26] + + e[21] * e[4] * e[7] + e[21] * e[5] * e[8] + e[6] * e[25] * e[7] + + e[6] * e[26] * e[8] + e[0] * e[19] * e[7] + e[0] * e[1] * e[25] + + e[0] * e[20] * e[8] + e[0] * e[2] * e[26] - e[6] * e[20] * e[2] - + e[6] * e[19] * e[1] - e[6] * e[22] * e[4] - e[6] * e[23] * e[5] + + e[18] * e[1] * e[7] + e[18] * e[0] * e[6] + e[18] * e[2] * e[8] + + e[3] * e[21] * e[6] + e[3] * e[22] * e[7] - 0.5 * e[24] * e2[4] + + 0.5 * e[24] * e2[0] + 1.5 * e[24] * e2[6] - 0.5 * e[24] * e2[5] - + 0.5 * e[24] * e2[1] + 0.5 * e[24] * e2[7] + 0.5 * e[24] * e2[3] - + 0.5 * e[24] * e2[2] + 0.5 * e[24] * e2[8]; + a[110] = e[6] * e[28] * e[23] - e[6] * e[22] * e[29] - e[6] * e[31] * e[20] - + e[3] * e[19] * e[35] + e[3] * e[34] * e[20] + e[3] * e[25] * e[29] - + e[21] * e[1] * e[35] + e[21] * e[7] * e[29] + e[21] * e[34] * e[2] + + e[24] * e[1] * e[32] + e[24] * e[28] * e[5] - e[24] * e[4] * e[29] - + e[24] * e[31] * e[2] + e[33] * e[1] * e[23] + e[33] * e[19] * e[5] - + e[33] * e[4] * e[20] - e[33] * e[22] * e[2] - e[21] * e[28] * e[8] + + e[30] * e[7] * e[20] + e[30] * e[25] * e[2] - e[30] * e[1] * e[26] + + e[18] * e[4] * e[35] - e[18] * e[5] * e[34] + e[18] * e[31] * e[8] - + e[18] * e[32] * e[7] + e[27] * e[4] * e[26] + e[27] * e[22] * e[8] - + e[27] * e[5] * e[25] - e[27] * e[23] * e[7] - e[3] * e[28] * e[26] - + e[0] * e[32] * e[25] + e[0] * e[22] * e[35] - e[0] * e[23] * e[34] + + e[0] * e[31] * e[26] - e[30] * e[19] * e[8] + e[6] * e[19] * e[32]; + a[107] = 0.5 * e2[18] * e[6] + 0.5 * e2[21] * e[6] + 1.5 * e2[24] * e[6] + + 0.5 * e[6] * e2[26] - 0.5 * e[6] * e2[23] - 0.5 * e[6] * e2[19] - + 0.5 * e[6] * e2[20] - 0.5 * e[6] * e2[22] + 0.5 * e[6] * e2[25] + + e[21] * e[3] * e[24] + e[18] * e[20] * e[8] + e[21] * e[4] * e[25] + + e[18] * e[19] * e[7] + e[18] * e[1] * e[25] + e[21] * e[22] * e[7] + + e[21] * e[23] * e[8] + e[18] * e[0] * e[24] + e[18] * e[2] * e[26] + + e[21] * e[5] * e[26] + e[24] * e[26] * e[8] - e[24] * e[20] * e[2] - + e[24] * e[19] * e[1] - e[24] * e[22] * e[4] + e[24] * e[25] * e[7] - + e[24] * e[23] * e[5] + e[0] * e[19] * e[25] + e[0] * e[20] * e[26] + + e[3] * e[22] * e[25] + e[3] * e[23] * e[26]; + a[40] = e[18] * e[4] * e[8] + e[3] * e[7] * e[20] + e[3] * e[25] * e[2] - + e[3] * e[1] * e[26] - e[18] * e[5] * e[7] + e[6] * e[1] * e[23] + + e[6] * e[19] * e[5] - e[6] * e[4] * e[20] - e[6] * e[22] * e[2] + + e[21] * e[7] * e[2] - e[21] * e[1] * e[8] + e[24] * e[1] * e[5] - + e[24] * e[4] * e[2] - e[3] * e[19] * e[8] + e[0] * e[4] * e[26] + + e[0] * e[22] * e[8] - e[0] * e[5] * e[25] - e[0] * e[23] * e[7]; + a[27] = e[9] * e[1] * e[7] + e[9] * e[0] * e[6] + e[9] * e[2] * e[8] + + e[3] * e[12] * e[6] + e[3] * e[13] * e[7] + e[3] * e[4] * e[16] + + e[3] * e[14] * e[8] + e[3] * e[5] * e[17] + e[12] * e[4] * e[7] + + e[12] * e[5] * e[8] + e[6] * e[16] * e[7] + e[6] * e[17] * e[8] - + e[6] * e[11] * e[2] - e[6] * e[10] * e[1] - e[6] * e[13] * e[4] - + e[6] * e[14] * e[5] + e[0] * e[10] * e[7] + e[0] * e[1] * e[16] + + e[0] * e[11] * e[8] + e[0] * e[2] * e[17] + 0.5 * e2[3] * e[15] + + 1.5 * e[15] * e2[6] + 0.5 * e[15] * e2[7] + 0.5 * e[15] * e2[8] + + 0.5 * e2[0] * e[15] - 0.5 * e[15] * e2[4] - 0.5 * e[15] * e2[5] - + 0.5 * e[15] * e2[1] - 0.5 * e[15] * e2[2]; + a[30] = -e[15] * e[13] * e[2] - e[6] * e[13] * e[11] - e[15] * e[4] * e[11] + + e[12] * e[16] * e[2] - e[3] * e[10] * e[17] + e[3] * e[16] * e[11] + + e[0] * e[13] * e[17] - e[0] * e[14] * e[16] + e[15] * e[1] * e[14] - + e[12] * e[10] * e[8] + e[9] * e[4] * e[17] + e[9] * e[13] * e[8] - + e[9] * e[5] * e[16] - e[9] * e[14] * e[7] + e[15] * e[10] * e[5] + + e[12] * e[7] * e[11] + e[6] * e[10] * e[14] - e[12] * e[1] * e[17]; + a[147] = + e[12] * e[30] * e[24] + e[12] * e[21] * e[33] + e[12] * e[31] * e[25] + + e[12] * e[22] * e[34] + e[12] * e[32] * e[26] + e[12] * e[23] * e[35] + + e[9] * e[27] * e[24] + e[9] * e[18] * e[33] + e[9] * e[28] * e[25] + + e[9] * e[19] * e[34] + e[9] * e[29] * e[26] + e[9] * e[20] * e[35] + + e[21] * e[30] * e[15] + e[21] * e[32] * e[17] + e[21] * e[14] * e[35] + + e[21] * e[31] * e[16] + e[21] * e[13] * e[34] + e[30] * e[23] * e[17] + + e[30] * e[14] * e[26] + e[30] * e[22] * e[16] + e[30] * e[13] * e[25] + + e[15] * e[27] * e[18] + 3. * e[15] * e[33] * e[24] - + e[15] * e[29] * e[20] + e[15] * e[35] * e[26] - e[15] * e[31] * e[22] - + e[15] * e[32] * e[23] - e[15] * e[28] * e[19] + e[15] * e[34] * e[25] + + e[18] * e[29] * e[17] + e[18] * e[11] * e[35] + e[18] * e[28] * e[16] + + e[18] * e[10] * e[34] + e[27] * e[20] * e[17] + e[27] * e[11] * e[26] + + e[27] * e[19] * e[16] + e[27] * e[10] * e[25] - e[24] * e[28] * e[10] - + e[24] * e[31] * e[13] - e[24] * e[32] * e[14] + e[24] * e[34] * e[16] + + e[24] * e[35] * e[17] - e[24] * e[29] * e[11] - e[33] * e[23] * e[14] + + e[33] * e[25] * e[16] + e[33] * e[26] * e[17] - e[33] * e[20] * e[11] - + e[33] * e[19] * e[10] - e[33] * e[22] * e[13]; + a[60] = e[18] * e[13] * e[17] + e[9] * e[13] * e[26] + e[9] * e[22] * e[17] - + e[9] * e[14] * e[25] - e[18] * e[14] * e[16] - e[15] * e[13] * e[20] - + e[15] * e[22] * e[11] + e[12] * e[16] * e[20] + + e[12] * e[25] * e[11] - e[12] * e[10] * e[26] - + e[12] * e[19] * e[17] + e[21] * e[16] * e[11] - + e[21] * e[10] * e[17] - e[9] * e[23] * e[16] + e[24] * e[10] * e[14] - + e[24] * e[13] * e[11] + e[15] * e[10] * e[23] + e[15] * e[19] * e[14]; + a[137] = + e[21] * e[12] * e[24] + e[21] * e[23] * e[17] + e[21] * e[14] * e[26] + + e[21] * e[22] * e[16] + e[21] * e[13] * e[25] + e[24] * e[26] * e[17] + + e[24] * e[25] * e[16] + e[9] * e[19] * e[25] + e[9] * e[18] * e[24] + + e[9] * e[20] * e[26] + e[12] * e[22] * e[25] + e[12] * e[23] * e[26] + + e[18] * e[20] * e[17] + e[18] * e[11] * e[26] + e[18] * e[19] * e[16] + + e[18] * e[10] * e[25] - e[24] * e[23] * e[14] - e[24] * e[20] * e[11] - + e[24] * e[19] * e[10] - e[24] * e[22] * e[13] + 0.5 * e2[21] * e[15] + + 1.5 * e2[24] * e[15] + 0.5 * e[15] * e2[25] + 0.5 * e[15] * e2[26] + + 0.5 * e[15] * e2[18] - 0.5 * e[15] * e2[23] - 0.5 * e[15] * e2[19] - + 0.5 * e[15] * e2[20] - 0.5 * e[15] * e2[22]; + a[20] = e[6] * e[1] * e[14] + e[15] * e[1] * e[5] - e[0] * e[5] * e[16] - + e[0] * e[14] * e[7] + e[0] * e[13] * e[8] - e[15] * e[4] * e[2] + + e[12] * e[7] * e[2] + e[6] * e[10] * e[5] + e[3] * e[7] * e[11] - + e[6] * e[4] * e[11] + e[3] * e[16] * e[2] - e[6] * e[13] * e[2] - + e[3] * e[1] * e[17] - e[9] * e[5] * e[7] - e[3] * e[10] * e[8] - + e[12] * e[1] * e[8] + e[0] * e[4] * e[17] + e[9] * e[4] * e[8]; + a[16] = -0.5 * e[14] * e2[16] - 0.5 * e[14] * e2[10] - 0.5 * e[14] * e2[9] + + e[11] * e[9] * e[12] + 0.5 * e3[14] + e[17] * e[13] * e[16] + + 0.5 * e[14] * e2[12] + e[11] * e[10] * e[13] - 0.5 * e[14] * e2[15] + + 0.5 * e[14] * e2[17] + e[17] * e[12] * e[15] + 0.5 * e2[11] * e[14] + + 0.5 * e[14] * e2[13]; + a[100] = -e[21] * e[19] * e[8] + e[18] * e[4] * e[26] - e[18] * e[5] * e[25] - + e[18] * e[23] * e[7] + e[21] * e[25] * e[2] - e[21] * e[1] * e[26] + + e[6] * e[19] * e[23] + e[18] * e[22] * e[8] - e[0] * e[23] * e[25] - + e[6] * e[22] * e[20] + e[24] * e[1] * e[23] + e[24] * e[19] * e[5] - + e[24] * e[4] * e[20] - e[24] * e[22] * e[2] + e[3] * e[25] * e[20] - + e[3] * e[19] * e[26] + e[0] * e[22] * e[26] + e[21] * e[7] * e[20]; + a[176] = + 0.5 * e2[20] * e[32] + 1.5 * e[32] * e2[23] + 0.5 * e[32] * e2[22] + + 0.5 * e[32] * e2[21] + 0.5 * e[32] * e2[26] - 0.5 * e[32] * e2[18] - + 0.5 * e[32] * e2[19] - 0.5 * e[32] * e2[24] - 0.5 * e[32] * e2[25] + + e[20] * e[27] * e[21] + e[20] * e[18] * e[30] + e[20] * e[28] * e[22] + + e[20] * e[19] * e[31] + e[20] * e[29] * e[23] + e[29] * e[19] * e[22] + + e[29] * e[18] * e[21] + e[23] * e[30] * e[21] + e[23] * e[31] * e[22] + + e[26] * e[30] * e[24] + e[26] * e[21] * e[33] + e[26] * e[31] * e[25] + + e[26] * e[22] * e[34] + e[26] * e[23] * e[35] + e[35] * e[22] * e[25] + + e[35] * e[21] * e[24] - e[23] * e[27] * e[18] - e[23] * e[33] * e[24] - + e[23] * e[28] * e[19] - e[23] * e[34] * e[25]; + a[130] = + -e[9] * e[23] * e[25] - e[21] * e[10] * e[26] - e[21] * e[19] * e[17] - + e[18] * e[23] * e[16] + e[18] * e[13] * e[26] + e[12] * e[25] * e[20] - + e[12] * e[19] * e[26] - e[15] * e[22] * e[20] + e[21] * e[16] * e[20] + + e[21] * e[25] * e[11] + e[24] * e[10] * e[23] + e[24] * e[19] * e[14] - + e[24] * e[13] * e[20] - e[24] * e[22] * e[11] + e[18] * e[22] * e[17] - + e[18] * e[14] * e[25] + e[9] * e[22] * e[26] + e[15] * e[19] * e[23]; + a[166] = 0.5 * e[23] * e2[21] + e[20] * e[19] * e[22] + + e[20] * e[18] * e[21] + 0.5 * e3[23] + e[26] * e[22] * e[25] + + 0.5 * e[23] * e2[26] - 0.5 * e[23] * e2[18] + 0.5 * e[23] * e2[22] - + 0.5 * e[23] * e2[19] + e[26] * e[21] * e[24] + 0.5 * e2[20] * e[23] - + 0.5 * e[23] * e2[24] - 0.5 * e[23] * e2[25]; + a[140] = + e[18] * e[13] * e[35] - e[18] * e[32] * e[16] + e[18] * e[31] * e[17] - + e[18] * e[14] * e[34] + e[27] * e[13] * e[26] + e[27] * e[22] * e[17] - + e[27] * e[14] * e[25] - e[27] * e[23] * e[16] - e[9] * e[32] * e[25] + + e[9] * e[22] * e[35] - e[9] * e[23] * e[34] + e[9] * e[31] * e[26] + + e[15] * e[19] * e[32] + e[15] * e[28] * e[23] - e[15] * e[22] * e[29] - + e[15] * e[31] * e[20] + e[24] * e[10] * e[32] + e[24] * e[28] * e[14] - + e[24] * e[13] * e[29] - e[24] * e[31] * e[11] + e[33] * e[10] * e[23] + + e[33] * e[19] * e[14] - e[33] * e[13] * e[20] - e[33] * e[22] * e[11] + + e[21] * e[16] * e[29] - e[21] * e[10] * e[35] - e[21] * e[28] * e[17] + + e[30] * e[16] * e[20] + e[30] * e[25] * e[11] - e[30] * e[10] * e[26] - + e[30] * e[19] * e[17] - e[12] * e[28] * e[26] - e[12] * e[19] * e[35] + + e[12] * e[34] * e[20] + e[12] * e[25] * e[29] + e[21] * e[34] * e[11]; + a[96] = -e[32] * e[10] * e[1] + e[32] * e[13] * e[4] - e[32] * e[16] * e[7] - + e[32] * e[15] * e[6] - e[32] * e[9] * e[0] + e[32] * e[12] * e[3] + + e[17] * e[30] * e[6] + e[17] * e[3] * e[33] + e[17] * e[31] * e[7] + + e[17] * e[4] * e[34] + e[17] * e[5] * e[35] - e[5] * e[27] * e[9] - + e[5] * e[28] * e[10] - e[5] * e[33] * e[15] - e[5] * e[34] * e[16] + + e[5] * e[29] * e[11] + e[35] * e[12] * e[6] + e[35] * e[3] * e[15] + + e[35] * e[13] * e[7] + e[35] * e[4] * e[16] + e[11] * e[27] * e[3] + + e[11] * e[0] * e[30] + e[11] * e[28] * e[4] + e[11] * e[1] * e[31] + + e[29] * e[9] * e[3] + e[29] * e[0] * e[12] + e[29] * e[10] * e[4] + + e[29] * e[1] * e[13] + e[5] * e[30] * e[12] + + 3. * e[5] * e[32] * e[14] + e[5] * e[31] * e[13] + + e[8] * e[30] * e[15] + e[8] * e[12] * e[33] + e[8] * e[32] * e[17] + + e[8] * e[14] * e[35] + e[8] * e[31] * e[16] + e[8] * e[13] * e[34] + + e[2] * e[27] * e[12] + e[2] * e[9] * e[30] + e[2] * e[29] * e[14] + + e[2] * e[11] * e[32] + e[2] * e[28] * e[13] + e[2] * e[10] * e[31] - + e[14] * e[27] * e[0] - e[14] * e[34] * e[7] - e[14] * e[33] * e[6] + + e[14] * e[30] * e[3] - e[14] * e[28] * e[1] + e[14] * e[31] * e[4]; + a[181] = + 0.5 * e[18] * e2[29] + 0.5 * e[18] * e2[28] + 0.5 * e[18] * e2[30] + + 0.5 * e[18] * e2[33] - 0.5 * e[18] * e2[32] - 0.5 * e[18] * e2[31] - + 0.5 * e[18] * e2[34] - 0.5 * e[18] * e2[35] + 1.5 * e[18] * e2[27] + + e[27] * e[28] * e[19] + e[27] * e[29] * e[20] + e[21] * e[27] * e[30] + + e[21] * e[29] * e[32] + e[21] * e[28] * e[31] + e[30] * e[28] * e[22] + + e[30] * e[19] * e[31] + e[30] * e[29] * e[23] + e[30] * e[20] * e[32] + + e[24] * e[27] * e[33] + e[24] * e[29] * e[35] + e[24] * e[28] * e[34] + + e[33] * e[28] * e[25] + e[33] * e[19] * e[34] + e[33] * e[29] * e[26] + + e[33] * e[20] * e[35] - e[27] * e[35] * e[26] - e[27] * e[31] * e[22] - + e[27] * e[32] * e[23] - e[27] * e[34] * e[25]; + a[46] = e[20] * e[1] * e[4] + e[20] * e[0] * e[3] + e[20] * e[2] * e[5] + + e[5] * e[21] * e[3] + e[5] * e[22] * e[4] + e[8] * e[21] * e[6] + + e[8] * e[3] * e[24] + e[8] * e[22] * e[7] + e[8] * e[4] * e[25] + + e[8] * e[5] * e[26] + e[26] * e[4] * e[7] + e[26] * e[3] * e[6] + + e[2] * e[18] * e[3] + e[2] * e[0] * e[21] + e[2] * e[19] * e[4] + + e[2] * e[1] * e[22] - e[5] * e[19] * e[1] - e[5] * e[18] * e[0] - + e[5] * e[25] * e[7] - e[5] * e[24] * e[6] + 0.5 * e[23] * e2[4] - + 0.5 * e[23] * e2[0] - 0.5 * e[23] * e2[6] + 1.5 * e[23] * e2[5] - + 0.5 * e[23] * e2[1] - 0.5 * e[23] * e2[7] + 0.5 * e[23] * e2[3] + + 0.5 * e[23] * e2[2] + 0.5 * e[23] * e2[8]; + a[151] = + 1.5 * e[9] * e2[27] + 0.5 * e[9] * e2[29] + 0.5 * e[9] * e2[28] - + 0.5 * e[9] * e2[32] - 0.5 * e[9] * e2[31] + 0.5 * e[9] * e2[33] + + 0.5 * e[9] * e2[30] - 0.5 * e[9] * e2[34] - 0.5 * e[9] * e2[35] + + e[33] * e[27] * e[15] + e[33] * e[29] * e[17] + e[33] * e[11] * e[35] + + e[33] * e[28] * e[16] + e[33] * e[10] * e[34] + e[27] * e[29] * e[11] + + e[27] * e[28] * e[10] + e[27] * e[30] * e[12] - e[27] * e[31] * e[13] - + e[27] * e[32] * e[14] - e[27] * e[34] * e[16] - e[27] * e[35] * e[17] + + e[30] * e[29] * e[14] + e[30] * e[11] * e[32] + e[30] * e[28] * e[13] + + e[30] * e[10] * e[31] + e[12] * e[29] * e[32] + e[12] * e[28] * e[31] + + e[15] * e[29] * e[35] + e[15] * e[28] * e[34]; + a[116] = -e[32] * e[24] * e[6] + e[8] * e[30] * e[24] + e[8] * e[21] * e[33] + + e[8] * e[31] * e[25] + e[8] * e[22] * e[34] + e[26] * e[30] * e[6] + + e[26] * e[3] * e[33] + e[26] * e[31] * e[7] + e[26] * e[4] * e[34] + + e[26] * e[32] * e[8] + e[26] * e[5] * e[35] + e[35] * e[21] * e[6] + + e[35] * e[3] * e[24] + e[35] * e[22] * e[7] + e[35] * e[4] * e[25] + + e[35] * e[23] * e[8] + e[2] * e[27] * e[21] + e[2] * e[18] * e[30] + + e[2] * e[28] * e[22] + e[2] * e[19] * e[31] + e[2] * e[29] * e[23] + + e[2] * e[20] * e[32] + e[20] * e[27] * e[3] + e[20] * e[0] * e[30] + + e[20] * e[28] * e[4] + e[20] * e[1] * e[31] + e[20] * e[29] * e[5] + + e[29] * e[18] * e[3] + e[29] * e[0] * e[21] + e[29] * e[19] * e[4] + + e[29] * e[1] * e[22] + e[5] * e[30] * e[21] + e[5] * e[31] * e[22] + + 3. * e[5] * e[32] * e[23] - e[5] * e[27] * e[18] - + e[5] * e[33] * e[24] - e[5] * e[28] * e[19] - e[5] * e[34] * e[25] - + e[23] * e[27] * e[0] - e[23] * e[34] * e[7] - e[23] * e[33] * e[6] + + e[23] * e[30] * e[3] - e[23] * e[28] * e[1] + e[23] * e[31] * e[4] + + e[32] * e[21] * e[3] - e[32] * e[19] * e[1] + e[32] * e[22] * e[4] - + e[32] * e[18] * e[0] - e[32] * e[25] * e[7]; + a[191] = 0.5 * e[27] * e2[33] - 0.5 * e[27] * e2[32] - 0.5 * e[27] * e2[31] - + 0.5 * e[27] * e2[34] - 0.5 * e[27] * e2[35] + e[33] * e[29] * e[35] + + 0.5 * e[27] * e2[29] + e[30] * e[29] * e[32] + + e[30] * e[28] * e[31] + e[33] * e[28] * e[34] + + 0.5 * e[27] * e2[28] + 0.5 * e[27] * e2[30] + 0.5 * e3[27]; + a[66] = + e[14] * e[21] * e[12] + e[14] * e[22] * e[13] + e[17] * e[21] * e[15] + + e[17] * e[12] * e[24] + e[17] * e[14] * e[26] + e[17] * e[22] * e[16] + + e[17] * e[13] * e[25] + e[26] * e[12] * e[15] + e[26] * e[13] * e[16] - + e[14] * e[24] * e[15] - e[14] * e[25] * e[16] - e[14] * e[18] * e[9] - + e[14] * e[19] * e[10] + e[11] * e[18] * e[12] + e[11] * e[9] * e[21] + + e[11] * e[19] * e[13] + e[11] * e[10] * e[22] + e[20] * e[11] * e[14] + + e[20] * e[9] * e[12] + e[20] * e[10] * e[13] + 1.5 * e[23] * e2[14] + + 0.5 * e[23] * e2[12] + 0.5 * e[23] * e2[13] + 0.5 * e[23] * e2[17] + + 0.5 * e2[11] * e[23] - 0.5 * e[23] * e2[16] - 0.5 * e[23] * e2[9] - + 0.5 * e[23] * e2[15] - 0.5 * e[23] * e2[10]; + a[121] = 1.5 * e[0] * e2[27] + 0.5 * e[0] * e2[29] + 0.5 * e[0] * e2[28] + + 0.5 * e[0] * e2[30] - 0.5 * e[0] * e2[32] - 0.5 * e[0] * e2[31] + + 0.5 * e[0] * e2[33] - 0.5 * e[0] * e2[34] - 0.5 * e[0] * e2[35] - + e[27] * e[31] * e[4] + e[3] * e[27] * e[30] + e[3] * e[29] * e[32] + + e[3] * e[28] * e[31] + e[30] * e[28] * e[4] + e[30] * e[1] * e[31] + + e[30] * e[29] * e[5] + e[30] * e[2] * e[32] + e[6] * e[27] * e[33] + + e[6] * e[29] * e[35] + e[6] * e[28] * e[34] + e[27] * e[28] * e[1] + + e[27] * e[29] * e[2] + e[33] * e[28] * e[7] + e[33] * e[1] * e[34] + + e[33] * e[29] * e[8] + e[33] * e[2] * e[35] - e[27] * e[34] * e[7] - + e[27] * e[32] * e[5] - e[27] * e[35] * e[8]; + a[36] = e[14] * e[12] * e[3] + e[14] * e[13] * e[4] + e[17] * e[12] * e[6] + + e[17] * e[3] * e[15] + e[17] * e[13] * e[7] + e[17] * e[4] * e[16] + + e[17] * e[14] * e[8] + e[8] * e[12] * e[15] + e[8] * e[13] * e[16] + + e[2] * e[11] * e[14] + e[2] * e[9] * e[12] + e[2] * e[10] * e[13] + + e[11] * e[9] * e[3] + e[11] * e[0] * e[12] + e[11] * e[10] * e[4] + + e[11] * e[1] * e[13] - e[14] * e[10] * e[1] - e[14] * e[16] * e[7] - + e[14] * e[15] * e[6] - e[14] * e[9] * e[0] - 0.5 * e[5] * e2[16] - + 0.5 * e[5] * e2[9] + 0.5 * e[5] * e2[11] + 0.5 * e[5] * e2[12] - + 0.5 * e[5] * e2[15] - 0.5 * e[5] * e2[10] + 0.5 * e[5] * e2[13] + + 1.5 * e2[14] * e[5] + 0.5 * e[5] * e2[17]; + a[71] = 1.5 * e[27] * e2[9] - 0.5 * e[27] * e2[16] + 0.5 * e[27] * e2[11] + + 0.5 * e[27] * e2[12] + 0.5 * e[27] * e2[15] - 0.5 * e[27] * e2[17] + + 0.5 * e[27] * e2[10] - 0.5 * e[27] * e2[14] - 0.5 * e[27] * e2[13] + + e[12] * e[10] * e[31] + e[30] * e[11] * e[14] + + e[30] * e[10] * e[13] + e[15] * e[9] * e[33] + e[15] * e[29] * e[17] + + e[15] * e[11] * e[35] + e[15] * e[28] * e[16] + + e[15] * e[10] * e[34] + e[33] * e[11] * e[17] + + e[33] * e[10] * e[16] - e[9] * e[31] * e[13] - e[9] * e[32] * e[14] - + e[9] * e[34] * e[16] - e[9] * e[35] * e[17] + e[9] * e[29] * e[11] + + e[9] * e[28] * e[10] + e[12] * e[9] * e[30] + e[12] * e[29] * e[14] + + e[12] * e[11] * e[32] + e[12] * e[28] * e[13]; + a[146] = + e[29] * e[18] * e[12] + e[29] * e[9] * e[21] + e[29] * e[19] * e[13] + + e[29] * e[10] * e[22] + e[17] * e[30] * e[24] + e[17] * e[21] * e[33] + + e[17] * e[31] * e[25] + e[17] * e[22] * e[34] + e[17] * e[32] * e[26] + + e[17] * e[23] * e[35] - e[23] * e[27] * e[9] - e[23] * e[28] * e[10] - + e[23] * e[33] * e[15] - e[23] * e[34] * e[16] - e[32] * e[24] * e[15] - + e[32] * e[25] * e[16] - e[32] * e[18] * e[9] - e[32] * e[19] * e[10] + + e[26] * e[30] * e[15] + e[26] * e[12] * e[33] + e[26] * e[31] * e[16] + + e[26] * e[13] * e[34] + e[35] * e[21] * e[15] + e[35] * e[12] * e[24] + + e[35] * e[22] * e[16] + e[35] * e[13] * e[25] + e[14] * e[30] * e[21] + + e[14] * e[31] * e[22] + 3. * e[14] * e[32] * e[23] + + e[11] * e[27] * e[21] + e[11] * e[18] * e[30] + e[11] * e[28] * e[22] + + e[11] * e[19] * e[31] + e[11] * e[29] * e[23] + e[11] * e[20] * e[32] + + e[23] * e[30] * e[12] + e[23] * e[31] * e[13] + e[32] * e[21] * e[12] + + e[32] * e[22] * e[13] - e[14] * e[27] * e[18] - e[14] * e[33] * e[24] + + e[14] * e[29] * e[20] + e[14] * e[35] * e[26] - e[14] * e[28] * e[19] - + e[14] * e[34] * e[25] + e[20] * e[27] * e[12] + e[20] * e[9] * e[30] + + e[20] * e[28] * e[13] + e[20] * e[10] * e[31]; + a[1] = 0.5 * e[0] * e2[1] + 0.5 * e[0] * e2[2] + e[6] * e[2] * e[8] + + e[6] * e[1] * e[7] + 0.5 * e[0] * e2[3] + e[3] * e[1] * e[4] + + 0.5 * e[0] * e2[6] + e[3] * e[2] * e[5] - 0.5 * e[0] * e2[5] - + 0.5 * e[0] * e2[8] + 0.5 * e3[0] - 0.5 * e[0] * e2[7] - + 0.5 * e[0] * e2[4]; + a[136] = + 1.5 * e2[23] * e[14] + 0.5 * e[14] * e2[26] - 0.5 * e[14] * e2[18] - + 0.5 * e[14] * e2[19] + 0.5 * e[14] * e2[20] + 0.5 * e[14] * e2[22] - + 0.5 * e[14] * e2[24] + 0.5 * e[14] * e2[21] - 0.5 * e[14] * e2[25] + + e[23] * e[21] * e[12] + e[23] * e[22] * e[13] + e[26] * e[21] * e[15] + + e[26] * e[12] * e[24] + e[26] * e[23] * e[17] + e[26] * e[22] * e[16] + + e[26] * e[13] * e[25] + e[17] * e[22] * e[25] + e[17] * e[21] * e[24] + + e[11] * e[19] * e[22] + e[11] * e[18] * e[21] + e[11] * e[20] * e[23] + + e[20] * e[18] * e[12] + e[20] * e[9] * e[21] + e[20] * e[19] * e[13] + + e[20] * e[10] * e[22] - e[23] * e[24] * e[15] - e[23] * e[25] * e[16] - + e[23] * e[18] * e[9] - e[23] * e[19] * e[10]; + a[51] = 1.5 * e[27] * e2[0] - 0.5 * e[27] * e2[4] + 0.5 * e[27] * e2[6] - + 0.5 * e[27] * e2[5] + 0.5 * e[27] * e2[1] - 0.5 * e[27] * e2[7] + + 0.5 * e[27] * e2[3] + 0.5 * e[27] * e2[2] - 0.5 * e[27] * e2[8] + + e[0] * e[33] * e[6] + e[0] * e[30] * e[3] - e[0] * e[35] * e[8] - + e[0] * e[31] * e[4] + e[3] * e[28] * e[4] + e[3] * e[1] * e[31] + + e[3] * e[29] * e[5] + e[3] * e[2] * e[32] + e[30] * e[1] * e[4] + + e[30] * e[2] * e[5] + e[6] * e[28] * e[7] + e[6] * e[1] * e[34] + + e[6] * e[29] * e[8] + e[6] * e[2] * e[35] + e[33] * e[1] * e[7] + + e[33] * e[2] * e[8] + e[0] * e[28] * e[1] + e[0] * e[29] * e[2] - + e[0] * e[34] * e[7] - e[0] * e[32] * e[5]; + a[106] = e[8] * e[22] * e[25] + e[8] * e[21] * e[24] + e[20] * e[18] * e[3] + + e[20] * e[0] * e[21] + e[20] * e[19] * e[4] + e[20] * e[1] * e[22] + + e[20] * e[2] * e[23] + e[23] * e[21] * e[3] + e[23] * e[22] * e[4] + + e[23] * e[26] * e[8] - e[23] * e[19] * e[1] - e[23] * e[18] * e[0] - + e[23] * e[25] * e[7] - e[23] * e[24] * e[6] + e[2] * e[19] * e[22] + + e[2] * e[18] * e[21] + e[26] * e[21] * e[6] + e[26] * e[3] * e[24] + + e[26] * e[22] * e[7] + e[26] * e[4] * e[25] + 0.5 * e2[20] * e[5] + + 1.5 * e2[23] * e[5] + 0.5 * e[5] * e2[22] + 0.5 * e[5] * e2[21] + + 0.5 * e[5] * e2[26] - 0.5 * e[5] * e2[18] - 0.5 * e[5] * e2[19] - + 0.5 * e[5] * e2[24] - 0.5 * e[5] * e2[25]; + a[81] = e[24] * e[11] * e[8] + e[24] * e[2] * e[17] + + 3. * e[9] * e[18] * e[0] + e[9] * e[19] * e[1] + e[9] * e[20] * e[2] + + e[18] * e[10] * e[1] + e[18] * e[11] * e[2] + e[3] * e[18] * e[12] + + e[3] * e[9] * e[21] + e[3] * e[20] * e[14] + e[3] * e[11] * e[23] + + e[3] * e[19] * e[13] + e[3] * e[10] * e[22] + e[6] * e[18] * e[15] + + e[6] * e[9] * e[24] + e[6] * e[20] * e[17] + e[6] * e[11] * e[26] + + e[6] * e[19] * e[16] + e[6] * e[10] * e[25] + e[0] * e[20] * e[11] + + e[0] * e[19] * e[10] - e[9] * e[26] * e[8] - e[9] * e[22] * e[4] - + e[9] * e[25] * e[7] - e[9] * e[23] * e[5] + e[12] * e[0] * e[21] + + e[12] * e[19] * e[4] + e[12] * e[1] * e[22] + e[12] * e[20] * e[5] + + e[12] * e[2] * e[23] - e[18] * e[13] * e[4] - e[18] * e[16] * e[7] - + e[18] * e[14] * e[5] - e[18] * e[17] * e[8] + e[21] * e[10] * e[4] + + e[21] * e[1] * e[13] + e[21] * e[11] * e[5] + e[21] * e[2] * e[14] + + e[15] * e[0] * e[24] + e[15] * e[19] * e[7] + e[15] * e[1] * e[25] + + e[15] * e[20] * e[8] + e[15] * e[2] * e[26] - e[0] * e[23] * e[14] - + e[0] * e[25] * e[16] - e[0] * e[26] * e[17] - e[0] * e[22] * e[13] + + e[24] * e[10] * e[7] + e[24] * e[1] * e[16]; + a[26] = e[11] * e[1] * e[4] + e[11] * e[0] * e[3] + e[11] * e[2] * e[5] + + e[5] * e[12] * e[3] + e[5] * e[13] * e[4] + e[8] * e[12] * e[6] + + e[8] * e[3] * e[15] + e[8] * e[13] * e[7] + e[8] * e[4] * e[16] + + e[8] * e[5] * e[17] + e[17] * e[4] * e[7] + e[17] * e[3] * e[6] - + e[5] * e[10] * e[1] - e[5] * e[16] * e[7] - e[5] * e[15] * e[6] - + e[5] * e[9] * e[0] + e[2] * e[9] * e[3] + e[2] * e[0] * e[12] + + e[2] * e[10] * e[4] + e[2] * e[1] * e[13] + 0.5 * e2[2] * e[14] - + 0.5 * e[14] * e2[0] - 0.5 * e[14] * e2[6] - 0.5 * e[14] * e2[1] - + 0.5 * e[14] * e2[7] + 1.5 * e[14] * e2[5] + 0.5 * e[14] * e2[4] + + 0.5 * e[14] * e2[3] + 0.5 * e[14] * e2[8]; + a[91] = e[3] * e[27] * e[12] + e[3] * e[9] * e[30] + e[3] * e[29] * e[14] + + e[3] * e[11] * e[32] + e[3] * e[28] * e[13] + e[3] * e[10] * e[31] + + e[6] * e[27] * e[15] + e[6] * e[9] * e[33] + e[6] * e[29] * e[17] + + e[6] * e[11] * e[35] + e[6] * e[28] * e[16] + e[6] * e[10] * e[34] + + 3. * e[0] * e[27] * e[9] + e[0] * e[29] * e[11] + + e[0] * e[28] * e[10] - e[9] * e[34] * e[7] - e[9] * e[32] * e[5] - + e[9] * e[35] * e[8] + e[9] * e[29] * e[2] + e[9] * e[28] * e[1] - + e[9] * e[31] * e[4] + e[12] * e[0] * e[30] + e[12] * e[28] * e[4] + + e[12] * e[1] * e[31] + e[12] * e[29] * e[5] + e[12] * e[2] * e[32] + + e[27] * e[11] * e[2] + e[27] * e[10] * e[1] - e[27] * e[13] * e[4] - + e[27] * e[16] * e[7] - e[27] * e[14] * e[5] - e[27] * e[17] * e[8] + + e[30] * e[10] * e[4] + e[30] * e[1] * e[13] + e[30] * e[11] * e[5] + + e[30] * e[2] * e[14] + e[15] * e[0] * e[33] + e[15] * e[28] * e[7] + + e[15] * e[1] * e[34] + e[15] * e[29] * e[8] + e[15] * e[2] * e[35] - + e[0] * e[31] * e[13] - e[0] * e[32] * e[14] - e[0] * e[34] * e[16] - + e[0] * e[35] * e[17] + e[33] * e[10] * e[7] + e[33] * e[1] * e[16] + + e[33] * e[11] * e[8] + e[33] * e[2] * e[17]; + a[127] = 0.5 * e2[30] * e[6] + 0.5 * e[6] * e2[27] - 0.5 * e[6] * e2[32] - + 0.5 * e[6] * e2[28] - 0.5 * e[6] * e2[29] - 0.5 * e[6] * e2[31] + + 1.5 * e[6] * e2[33] + 0.5 * e[6] * e2[34] + 0.5 * e[6] * e2[35] + + e[0] * e[27] * e[33] + e[0] * e[29] * e[35] + e[0] * e[28] * e[34] + + e[3] * e[30] * e[33] + e[3] * e[32] * e[35] + e[3] * e[31] * e[34] + + e[30] * e[31] * e[7] + e[30] * e[4] * e[34] + e[30] * e[32] * e[8] + + e[30] * e[5] * e[35] + e[27] * e[28] * e[7] + e[27] * e[1] * e[34] + + e[27] * e[29] * e[8] + e[27] * e[2] * e[35] + e[33] * e[34] * e[7] + + e[33] * e[35] * e[8] - e[33] * e[32] * e[5] - e[33] * e[29] * e[2] - + e[33] * e[28] * e[1] - e[33] * e[31] * e[4]; + a[161] = e[24] * e[20] * e[26] + e[21] * e[19] * e[22] - + 0.5 * e[18] * e2[22] - 0.5 * e[18] * e2[25] + 0.5 * e3[18] + + 0.5 * e[18] * e2[21] + e[21] * e[20] * e[23] + 0.5 * e[18] * e2[20] + + 0.5 * e[18] * e2[19] + 0.5 * e[18] * e2[24] + e[24] * e[19] * e[25] - + 0.5 * e[18] * e2[23] - 0.5 * e[18] * e2[26]; + a[197] = 0.5 * e[33] * e2[35] + 0.5 * e3[33] + 0.5 * e2[27] * e[33] + + 0.5 * e2[30] * e[33] - 0.5 * e[33] * e2[29] + 0.5 * e[33] * e2[34] - + 0.5 * e[33] * e2[32] - 0.5 * e[33] * e2[28] + e[30] * e[32] * e[35] - + 0.5 * e[33] * e2[31] + e[27] * e[29] * e[35] + + e[27] * e[28] * e[34] + e[30] * e[31] * e[34]; + a[171] = + 1.5 * e[27] * e2[18] + 0.5 * e[27] * e2[19] + 0.5 * e[27] * e2[20] + + 0.5 * e[27] * e2[21] + 0.5 * e[27] * e2[24] - 0.5 * e[27] * e2[26] - + 0.5 * e[27] * e2[23] - 0.5 * e[27] * e2[22] - 0.5 * e[27] * e2[25] + + e[33] * e[20] * e[26] - e[18] * e[35] * e[26] - e[18] * e[31] * e[22] - + e[18] * e[32] * e[23] - e[18] * e[34] * e[25] + e[18] * e[28] * e[19] + + e[18] * e[29] * e[20] + e[21] * e[18] * e[30] + e[21] * e[28] * e[22] + + e[21] * e[19] * e[31] + e[21] * e[29] * e[23] + e[21] * e[20] * e[32] + + e[30] * e[19] * e[22] + e[30] * e[20] * e[23] + e[24] * e[18] * e[33] + + e[24] * e[28] * e[25] + e[24] * e[19] * e[34] + e[24] * e[29] * e[26] + + e[24] * e[20] * e[35] + e[33] * e[19] * e[25]; + a[157] = + e[9] * e[27] * e[33] + e[9] * e[29] * e[35] + e[9] * e[28] * e[34] + + e[33] * e[35] * e[17] + e[33] * e[34] * e[16] + e[27] * e[29] * e[17] + + e[27] * e[11] * e[35] + e[27] * e[28] * e[16] + e[27] * e[10] * e[34] + + e[33] * e[30] * e[12] - e[33] * e[28] * e[10] - e[33] * e[31] * e[13] - + e[33] * e[32] * e[14] - e[33] * e[29] * e[11] + e[30] * e[32] * e[17] + + e[30] * e[14] * e[35] + e[30] * e[31] * e[16] + e[30] * e[13] * e[34] + + e[12] * e[32] * e[35] + e[12] * e[31] * e[34] + 0.5 * e[15] * e2[27] - + 0.5 * e[15] * e2[32] - 0.5 * e[15] * e2[28] - 0.5 * e[15] * e2[29] - + 0.5 * e[15] * e2[31] + 1.5 * e[15] * e2[33] + 0.5 * e[15] * e2[30] + + 0.5 * e[15] * e2[34] + 0.5 * e[15] * e2[35]; + a[11] = 0.5 * e[9] * e2[12] - 0.5 * e[9] * e2[16] + 0.5 * e[9] * e2[10] - + 0.5 * e[9] * e2[17] - 0.5 * e[9] * e2[13] + e[15] * e[10] * e[16] + + e[12] * e[11] * e[14] + 0.5 * e[9] * e2[11] + 0.5 * e[9] * e2[15] - + 0.5 * e[9] * e2[14] + e[15] * e[11] * e[17] + 0.5 * e3[9] + + e[12] * e[10] * e[13]; + a[187] = + e[18] * e[27] * e[33] + e[18] * e[29] * e[35] + e[18] * e[28] * e[34] + + e[27] * e[28] * e[25] + e[27] * e[19] * e[34] + e[27] * e[29] * e[26] + + e[27] * e[20] * e[35] + e[21] * e[30] * e[33] + e[21] * e[32] * e[35] + + e[21] * e[31] * e[34] + e[30] * e[31] * e[25] + e[30] * e[22] * e[34] + + e[30] * e[32] * e[26] + e[30] * e[23] * e[35] + e[33] * e[34] * e[25] + + e[33] * e[35] * e[26] - e[33] * e[29] * e[20] - e[33] * e[31] * e[22] - + e[33] * e[32] * e[23] - e[33] * e[28] * e[19] + 0.5 * e2[27] * e[24] + + 0.5 * e2[30] * e[24] + 1.5 * e[24] * e2[33] + 0.5 * e[24] * e2[35] + + 0.5 * e[24] * e2[34] - 0.5 * e[24] * e2[32] - 0.5 * e[24] * e2[28] - + 0.5 * e[24] * e2[29] - 0.5 * e[24] * e2[31]; + a[131] = + 0.5 * e[9] * e2[21] + 0.5 * e[9] * e2[24] + 0.5 * e[9] * e2[19] + + 1.5 * e[9] * e2[18] + 0.5 * e[9] * e2[20] - 0.5 * e[9] * e2[26] - + 0.5 * e[9] * e2[23] - 0.5 * e[9] * e2[22] - 0.5 * e[9] * e2[25] + + e[21] * e[18] * e[12] + e[21] * e[20] * e[14] + e[21] * e[11] * e[23] + + e[21] * e[19] * e[13] + e[21] * e[10] * e[22] + e[24] * e[18] * e[15] + + e[24] * e[20] * e[17] + e[24] * e[11] * e[26] + e[24] * e[19] * e[16] + + e[24] * e[10] * e[25] + e[15] * e[19] * e[25] + e[15] * e[20] * e[26] + + e[12] * e[19] * e[22] + e[12] * e[20] * e[23] + e[18] * e[20] * e[11] + + e[18] * e[19] * e[10] - e[18] * e[23] * e[14] - e[18] * e[25] * e[16] - + e[18] * e[26] * e[17] - e[18] * e[22] * e[13]; + a[189] = + 0.5 * e2[29] * e[26] + 0.5 * e2[32] * e[26] + 0.5 * e[26] * e2[33] + + 1.5 * e[26] * e2[35] + 0.5 * e[26] * e2[34] - 0.5 * e[26] * e2[27] - + 0.5 * e[26] * e2[28] - 0.5 * e[26] * e2[31] - 0.5 * e[26] * e2[30] + + e[20] * e[27] * e[33] + e[20] * e[29] * e[35] + e[20] * e[28] * e[34] + + e[29] * e[27] * e[24] + e[29] * e[18] * e[33] + e[29] * e[28] * e[25] + + e[29] * e[19] * e[34] + e[23] * e[30] * e[33] + e[23] * e[32] * e[35] + + e[23] * e[31] * e[34] + e[32] * e[30] * e[24] + e[32] * e[21] * e[33] + + e[32] * e[31] * e[25] + e[32] * e[22] * e[34] + e[35] * e[33] * e[24] + + e[35] * e[34] * e[25] - e[35] * e[27] * e[18] - e[35] * e[30] * e[21] - + e[35] * e[31] * e[22] - e[35] * e[28] * e[19]; + a[141] = + e[12] * e[19] * e[31] + e[12] * e[29] * e[23] + e[12] * e[20] * e[32] + + 3. * e[9] * e[27] * e[18] + e[9] * e[28] * e[19] + e[9] * e[29] * e[20] + + e[21] * e[9] * e[30] + e[21] * e[29] * e[14] + e[21] * e[11] * e[32] + + e[21] * e[28] * e[13] + e[21] * e[10] * e[31] + e[30] * e[20] * e[14] + + e[30] * e[11] * e[23] + e[30] * e[19] * e[13] + e[30] * e[10] * e[22] + + e[9] * e[33] * e[24] - e[9] * e[35] * e[26] - e[9] * e[31] * e[22] - + e[9] * e[32] * e[23] - e[9] * e[34] * e[25] + e[18] * e[29] * e[11] + + e[18] * e[28] * e[10] + e[27] * e[20] * e[11] + e[27] * e[19] * e[10] + + e[15] * e[27] * e[24] + e[15] * e[18] * e[33] + e[15] * e[28] * e[25] + + e[15] * e[19] * e[34] + e[15] * e[29] * e[26] + e[15] * e[20] * e[35] - + e[18] * e[31] * e[13] - e[18] * e[32] * e[14] - e[18] * e[34] * e[16] - + e[18] * e[35] * e[17] - e[27] * e[23] * e[14] - e[27] * e[25] * e[16] - + e[27] * e[26] * e[17] - e[27] * e[22] * e[13] + e[24] * e[29] * e[17] + + e[24] * e[11] * e[35] + e[24] * e[28] * e[16] + e[24] * e[10] * e[34] + + e[33] * e[20] * e[17] + e[33] * e[11] * e[26] + e[33] * e[19] * e[16] + + e[33] * e[10] * e[25] + e[12] * e[27] * e[21] + e[12] * e[18] * e[30] + + e[12] * e[28] * e[22]; + a[159] = + -0.5 * e[17] * e2[27] + 0.5 * e[17] * e2[32] - 0.5 * e[17] * e2[28] + + 0.5 * e[17] * e2[29] - 0.5 * e[17] * e2[31] + 0.5 * e[17] * e2[33] - + 0.5 * e[17] * e2[30] + 0.5 * e[17] * e2[34] + 1.5 * e[17] * e2[35] + + e[32] * e[30] * e[15] + e[32] * e[12] * e[33] + e[32] * e[31] * e[16] + + e[32] * e[13] * e[34] + e[14] * e[30] * e[33] + e[14] * e[31] * e[34] + + e[11] * e[27] * e[33] + e[11] * e[29] * e[35] + e[11] * e[28] * e[34] + + e[35] * e[33] * e[15] + e[35] * e[34] * e[16] + e[29] * e[27] * e[15] + + e[29] * e[9] * e[33] + e[29] * e[28] * e[16] + e[29] * e[10] * e[34] - + e[35] * e[27] * e[9] - e[35] * e[30] * e[12] - e[35] * e[28] * e[10] - + e[35] * e[31] * e[13] + e[35] * e[32] * e[14]; + a[21] = 0.5 * e[9] * e2[1] + 1.5 * e[9] * e2[0] + 0.5 * e[9] * e2[2] + + 0.5 * e[9] * e2[3] + 0.5 * e[9] * e2[6] - 0.5 * e[9] * e2[4] - + 0.5 * e[9] * e2[5] - 0.5 * e[9] * e2[7] - 0.5 * e[9] * e2[8] + + e[6] * e[0] * e[15] + e[6] * e[10] * e[7] + e[6] * e[1] * e[16] + + e[6] * e[11] * e[8] + e[6] * e[2] * e[17] + e[15] * e[1] * e[7] + + e[15] * e[2] * e[8] + e[0] * e[11] * e[2] + e[0] * e[10] * e[1] - + e[0] * e[13] * e[4] - e[0] * e[16] * e[7] - e[0] * e[14] * e[5] - + e[0] * e[17] * e[8] + e[3] * e[0] * e[12] + e[3] * e[10] * e[4] + + e[3] * e[1] * e[13] + e[3] * e[11] * e[5] + e[3] * e[2] * e[14] + + e[12] * e[1] * e[4] + e[12] * e[2] * e[5]; + a[199] = 0.5 * e[35] * e2[33] + 0.5 * e[35] * e2[34] - 0.5 * e[35] * e2[27] - + 0.5 * e[35] * e2[28] - 0.5 * e[35] * e2[31] - 0.5 * e[35] * e2[30] + + e[32] * e[31] * e[34] + 0.5 * e2[29] * e[35] + 0.5 * e2[32] * e[35] + + e[29] * e[28] * e[34] + e[32] * e[30] * e[33] + 0.5 * e3[35] + + e[29] * e[27] * e[33]; + a[101] = 0.5 * e[0] * e2[19] + 0.5 * e[0] * e2[20] + 0.5 * e[0] * e2[24] - + 0.5 * e[0] * e2[26] - 0.5 * e[0] * e2[23] - 0.5 * e[0] * e2[22] - + 0.5 * e[0] * e2[25] + 1.5 * e2[18] * e[0] + 0.5 * e[0] * e2[21] + + e[18] * e[19] * e[1] + e[18] * e[20] * e[2] + e[21] * e[18] * e[3] + + e[21] * e[19] * e[4] + e[21] * e[1] * e[22] + e[21] * e[20] * e[5] + + e[21] * e[2] * e[23] - e[18] * e[26] * e[8] - e[18] * e[22] * e[4] - + e[18] * e[25] * e[7] - e[18] * e[23] * e[5] + e[18] * e[24] * e[6] + + e[3] * e[19] * e[22] + e[3] * e[20] * e[23] + e[24] * e[19] * e[7] + + e[24] * e[1] * e[25] + e[24] * e[20] * e[8] + e[24] * e[2] * e[26] + + e[6] * e[19] * e[25] + e[6] * e[20] * e[26]; + a[129] = 0.5 * e2[32] * e[8] - 0.5 * e[8] * e2[27] - 0.5 * e[8] * e2[28] + + 0.5 * e[8] * e2[29] - 0.5 * e[8] * e2[31] + 0.5 * e[8] * e2[33] - + 0.5 * e[8] * e2[30] + 0.5 * e[8] * e2[34] + 1.5 * e[8] * e2[35] + + e[2] * e[27] * e[33] + e[2] * e[29] * e[35] + e[2] * e[28] * e[34] + + e[5] * e[30] * e[33] + e[5] * e[32] * e[35] + e[5] * e[31] * e[34] + + e[32] * e[30] * e[6] + e[32] * e[3] * e[33] + e[32] * e[31] * e[7] + + e[32] * e[4] * e[34] + e[29] * e[27] * e[6] + e[29] * e[0] * e[33] + + e[29] * e[28] * e[7] + e[29] * e[1] * e[34] + e[35] * e[33] * e[6] + + e[35] * e[34] * e[7] - e[35] * e[27] * e[0] - e[35] * e[30] * e[3] - + e[35] * e[28] * e[1] - e[35] * e[31] * e[4]; + a[41] = -0.5 * e[18] * e2[4] + 1.5 * e[18] * e2[0] + 0.5 * e[18] * e2[6] - + 0.5 * e[18] * e2[5] + 0.5 * e[18] * e2[1] - 0.5 * e[18] * e2[7] + + 0.5 * e[18] * e2[3] + 0.5 * e[18] * e2[2] - 0.5 * e[18] * e2[8] + + e[3] * e[0] * e[21] + e[3] * e[19] * e[4] + e[3] * e[1] * e[22] + + e[3] * e[20] * e[5] + e[3] * e[2] * e[23] + e[21] * e[1] * e[4] + + e[21] * e[2] * e[5] + e[6] * e[0] * e[24] + e[6] * e[19] * e[7] + + e[6] * e[1] * e[25] + e[6] * e[20] * e[8] + e[6] * e[2] * e[26] + + e[24] * e[1] * e[7] + e[24] * e[2] * e[8] + e[0] * e[19] * e[1] + + e[0] * e[20] * e[2] - e[0] * e[26] * e[8] - e[0] * e[22] * e[4] - + e[0] * e[25] * e[7] - e[0] * e[23] * e[5]; + a[28] = e[10] * e[1] * e[7] + e[10] * e[0] * e[6] + e[10] * e[2] * e[8] + + e[4] * e[12] * e[6] + e[4] * e[3] * e[15] + e[4] * e[13] * e[7] + + e[4] * e[14] * e[8] + e[4] * e[5] * e[17] + e[13] * e[3] * e[6] + + e[13] * e[5] * e[8] + e[7] * e[15] * e[6] + e[7] * e[17] * e[8] - + e[7] * e[11] * e[2] - e[7] * e[9] * e[0] - e[7] * e[14] * e[5] - + e[7] * e[12] * e[3] + e[1] * e[9] * e[6] + e[1] * e[0] * e[15] + + e[1] * e[11] * e[8] + e[1] * e[2] * e[17] + 1.5 * e[16] * e2[7] + + 0.5 * e[16] * e2[6] + 0.5 * e[16] * e2[8] + 0.5 * e2[1] * e[16] - + 0.5 * e[16] * e2[0] - 0.5 * e[16] * e2[5] - 0.5 * e[16] * e2[3] - + 0.5 * e[16] * e2[2] + 0.5 * e2[4] * e[16]; + a[111] = e[0] * e[30] * e[21] - e[0] * e[35] * e[26] - e[0] * e[31] * e[22] - + e[0] * e[32] * e[23] - e[0] * e[34] * e[25] - e[18] * e[34] * e[7] - + e[18] * e[32] * e[5] - e[18] * e[35] * e[8] - e[18] * e[31] * e[4] - + e[27] * e[26] * e[8] - e[27] * e[22] * e[4] - e[27] * e[25] * e[7] - + e[27] * e[23] * e[5] + e[6] * e[28] * e[25] + e[6] * e[19] * e[34] + + e[6] * e[29] * e[26] + e[6] * e[20] * e[35] + e[21] * e[28] * e[4] + + e[21] * e[1] * e[31] + e[21] * e[29] * e[5] + e[21] * e[2] * e[32] + + e[30] * e[19] * e[4] + e[30] * e[1] * e[22] + e[30] * e[20] * e[5] + + e[30] * e[2] * e[23] + e[24] * e[27] * e[6] + e[24] * e[0] * e[33] + + e[24] * e[28] * e[7] + e[24] * e[1] * e[34] + e[24] * e[29] * e[8] + + e[24] * e[2] * e[35] + e[33] * e[18] * e[6] + e[33] * e[19] * e[7] + + e[33] * e[1] * e[25] + e[33] * e[20] * e[8] + e[33] * e[2] * e[26] + + 3. * e[0] * e[27] * e[18] + e[0] * e[28] * e[19] + + e[0] * e[29] * e[20] + e[18] * e[28] * e[1] + e[18] * e[29] * e[2] + + e[27] * e[19] * e[1] + e[27] * e[20] * e[2] + e[3] * e[27] * e[21] + + e[3] * e[18] * e[30] + e[3] * e[28] * e[22] + e[3] * e[19] * e[31] + + e[3] * e[29] * e[23] + e[3] * e[20] * e[32]; + a[108] = e[19] * e[18] * e[6] + e[19] * e[0] * e[24] + e[19] * e[1] * e[25] + + e[19] * e[20] * e[8] + e[19] * e[2] * e[26] + e[22] * e[21] * e[6] + + e[22] * e[3] * e[24] + e[22] * e[4] * e[25] + e[22] * e[23] * e[8] + + e[22] * e[5] * e[26] - e[25] * e[21] * e[3] + e[25] * e[26] * e[8] - + e[25] * e[20] * e[2] - e[25] * e[18] * e[0] - e[25] * e[23] * e[5] + + e[25] * e[24] * e[6] + e[1] * e[18] * e[24] + e[1] * e[20] * e[26] + + e[4] * e[21] * e[24] + e[4] * e[23] * e[26] + 0.5 * e2[19] * e[7] + + 0.5 * e2[22] * e[7] + 1.5 * e2[25] * e[7] + 0.5 * e[7] * e2[26] - + 0.5 * e[7] * e2[18] - 0.5 * e[7] * e2[23] - 0.5 * e[7] * e2[20] + + 0.5 * e[7] * e2[24] - 0.5 * e[7] * e2[21]; + a[61] = 0.5 * e[18] * e2[11] + 1.5 * e[18] * e2[9] + 0.5 * e[18] * e2[10] + + 0.5 * e[18] * e2[12] + 0.5 * e[18] * e2[15] - 0.5 * e[18] * e2[16] - + 0.5 * e[18] * e2[17] - 0.5 * e[18] * e2[14] - 0.5 * e[18] * e2[13] + + e[12] * e[9] * e[21] + e[12] * e[20] * e[14] + e[12] * e[11] * e[23] + + e[12] * e[19] * e[13] + e[12] * e[10] * e[22] + + e[21] * e[11] * e[14] + e[21] * e[10] * e[13] + e[15] * e[9] * e[24] + + e[15] * e[20] * e[17] + e[15] * e[11] * e[26] + + e[15] * e[19] * e[16] + e[15] * e[10] * e[25] + + e[24] * e[11] * e[17] + e[24] * e[10] * e[16] - e[9] * e[23] * e[14] - + e[9] * e[25] * e[16] - e[9] * e[26] * e[17] + e[9] * e[20] * e[11] + + e[9] * e[19] * e[10] - e[9] * e[22] * e[13]; + a[138] = + e[13] * e[21] * e[24] + e[13] * e[23] * e[26] + e[19] * e[18] * e[15] + + e[19] * e[9] * e[24] + e[19] * e[20] * e[17] + e[19] * e[11] * e[26] - + e[25] * e[23] * e[14] - e[25] * e[20] * e[11] - e[25] * e[18] * e[9] - + e[25] * e[21] * e[12] + e[22] * e[21] * e[15] + e[22] * e[12] * e[24] + + e[22] * e[23] * e[17] + e[22] * e[14] * e[26] + e[22] * e[13] * e[25] + + e[25] * e[24] * e[15] + e[25] * e[26] * e[17] + e[10] * e[19] * e[25] + + e[10] * e[18] * e[24] + e[10] * e[20] * e[26] - 0.5 * e[16] * e2[18] - + 0.5 * e[16] * e2[23] + 0.5 * e[16] * e2[19] - 0.5 * e[16] * e2[20] - + 0.5 * e[16] * e2[21] + 0.5 * e2[22] * e[16] + 1.5 * e2[25] * e[16] + + 0.5 * e[16] * e2[24] + 0.5 * e[16] * e2[26]; + a[31] = 0.5 * e[0] * e2[12] + 0.5 * e[0] * e2[15] + 0.5 * e[0] * e2[11] + + 1.5 * e[0] * e2[9] + 0.5 * e[0] * e2[10] - 0.5 * e[0] * e2[16] - + 0.5 * e[0] * e2[17] - 0.5 * e[0] * e2[14] - 0.5 * e[0] * e2[13] + + e[12] * e[9] * e[3] + e[12] * e[10] * e[4] + e[12] * e[1] * e[13] + + e[12] * e[11] * e[5] + e[12] * e[2] * e[14] + e[15] * e[9] * e[6] + + e[15] * e[10] * e[7] + e[15] * e[1] * e[16] + e[15] * e[11] * e[8] + + e[15] * e[2] * e[17] + e[6] * e[11] * e[17] + e[6] * e[10] * e[16] + + e[3] * e[11] * e[14] + e[3] * e[10] * e[13] + e[9] * e[10] * e[1] + + e[9] * e[11] * e[2] - e[9] * e[13] * e[4] - e[9] * e[16] * e[7] - + e[9] * e[14] * e[5] - e[9] * e[17] * e[8]; + a[148] = + e[19] * e[11] * e[35] + e[28] * e[18] * e[15] + e[28] * e[9] * e[24] + + e[28] * e[20] * e[17] + e[28] * e[11] * e[26] - e[25] * e[27] * e[9] - + e[25] * e[30] * e[12] - e[25] * e[32] * e[14] + e[25] * e[33] * e[15] + + e[25] * e[35] * e[17] - e[25] * e[29] * e[11] - e[34] * e[23] * e[14] + + e[34] * e[24] * e[15] + e[34] * e[26] * e[17] - e[34] * e[20] * e[11] - + e[34] * e[18] * e[9] - e[34] * e[21] * e[12] + e[13] * e[30] * e[24] + + e[13] * e[21] * e[33] + e[13] * e[31] * e[25] + e[13] * e[22] * e[34] + + e[13] * e[32] * e[26] + e[13] * e[23] * e[35] + e[10] * e[27] * e[24] + + e[10] * e[18] * e[33] + e[10] * e[28] * e[25] + e[10] * e[19] * e[34] + + e[10] * e[29] * e[26] + e[10] * e[20] * e[35] + e[22] * e[30] * e[15] + + e[22] * e[12] * e[33] + e[22] * e[32] * e[17] + e[22] * e[14] * e[35] + + e[22] * e[31] * e[16] + e[31] * e[21] * e[15] + e[31] * e[12] * e[24] + + e[31] * e[23] * e[17] + e[31] * e[14] * e[26] - e[16] * e[27] * e[18] + + e[16] * e[33] * e[24] - e[16] * e[30] * e[21] - e[16] * e[29] * e[20] + + e[16] * e[35] * e[26] - e[16] * e[32] * e[23] + e[16] * e[28] * e[19] + + 3. * e[16] * e[34] * e[25] + e[19] * e[27] * e[15] + + e[19] * e[9] * e[33] + e[19] * e[29] * e[17]; + a[52] = e[4] * e[27] * e[3] + e[4] * e[0] * e[30] + e[4] * e[29] * e[5] + + e[4] * e[2] * e[32] + e[31] * e[0] * e[3] + e[31] * e[2] * e[5] + + e[7] * e[27] * e[6] + e[7] * e[0] * e[33] + e[7] * e[29] * e[8] + + e[7] * e[2] * e[35] + e[34] * e[0] * e[6] + e[34] * e[2] * e[8] + + e[1] * e[27] * e[0] + e[1] * e[29] * e[2] + e[1] * e[34] * e[7] - + e[1] * e[32] * e[5] - e[1] * e[33] * e[6] - e[1] * e[30] * e[3] - + e[1] * e[35] * e[8] + e[1] * e[31] * e[4] + 1.5 * e[28] * e2[1] + + 0.5 * e[28] * e2[4] + 0.5 * e[28] * e2[0] - 0.5 * e[28] * e2[6] - + 0.5 * e[28] * e2[5] + 0.5 * e[28] * e2[7] - 0.5 * e[28] * e2[3] + + 0.5 * e[28] * e2[2] - 0.5 * e[28] * e2[8]; + a[99] = -e[35] * e[10] * e[1] - e[35] * e[13] * e[4] + e[35] * e[16] * e[7] + + e[35] * e[15] * e[6] - e[35] * e[9] * e[0] - e[35] * e[12] * e[3] + + e[32] * e[12] * e[6] + e[32] * e[3] * e[15] + e[32] * e[13] * e[7] + + e[32] * e[4] * e[16] - e[8] * e[27] * e[9] - e[8] * e[30] * e[12] - + e[8] * e[28] * e[10] - e[8] * e[31] * e[13] + e[8] * e[29] * e[11] + + e[11] * e[27] * e[6] + e[11] * e[0] * e[33] + e[11] * e[28] * e[7] + + e[11] * e[1] * e[34] + e[29] * e[9] * e[6] + e[29] * e[0] * e[15] + + e[29] * e[10] * e[7] + e[29] * e[1] * e[16] + e[5] * e[30] * e[15] + + e[5] * e[12] * e[33] + e[5] * e[32] * e[17] + e[5] * e[14] * e[35] + + e[5] * e[31] * e[16] + e[5] * e[13] * e[34] + e[8] * e[33] * e[15] + + 3. * e[8] * e[35] * e[17] + e[8] * e[34] * e[16] + + e[2] * e[27] * e[15] + e[2] * e[9] * e[33] + e[2] * e[29] * e[17] + + e[2] * e[11] * e[35] + e[2] * e[28] * e[16] + e[2] * e[10] * e[34] - + e[17] * e[27] * e[0] + e[17] * e[34] * e[7] + e[17] * e[33] * e[6] - + e[17] * e[30] * e[3] - e[17] * e[28] * e[1] - e[17] * e[31] * e[4] + + e[14] * e[30] * e[6] + e[14] * e[3] * e[33] + e[14] * e[31] * e[7] + + e[14] * e[4] * e[34] + e[14] * e[32] * e[8]; + a[82] = e[19] * e[11] * e[2] + e[4] * e[18] * e[12] + e[4] * e[9] * e[21] + + e[4] * e[20] * e[14] + e[4] * e[11] * e[23] + e[4] * e[19] * e[13] + + e[4] * e[10] * e[22] + e[7] * e[18] * e[15] + e[7] * e[9] * e[24] + + e[7] * e[20] * e[17] + e[7] * e[11] * e[26] + e[7] * e[19] * e[16] + + e[7] * e[10] * e[25] + e[1] * e[18] * e[9] + e[1] * e[20] * e[11] - + e[10] * e[21] * e[3] - e[10] * e[26] * e[8] - e[10] * e[23] * e[5] - + e[10] * e[24] * e[6] + e[13] * e[18] * e[3] + e[13] * e[0] * e[21] + + e[13] * e[1] * e[22] + e[13] * e[20] * e[5] + e[13] * e[2] * e[23] - + e[19] * e[15] * e[6] - e[19] * e[14] * e[5] - e[19] * e[12] * e[3] - + e[19] * e[17] * e[8] + e[22] * e[9] * e[3] + e[22] * e[0] * e[12] + + e[22] * e[11] * e[5] + e[22] * e[2] * e[14] + e[16] * e[18] * e[6] + + e[16] * e[0] * e[24] + e[16] * e[1] * e[25] + e[16] * e[20] * e[8] + + e[16] * e[2] * e[26] - e[1] * e[23] * e[14] - e[1] * e[24] * e[15] - + e[1] * e[26] * e[17] - e[1] * e[21] * e[12] + e[25] * e[9] * e[6] + + e[25] * e[0] * e[15] + e[25] * e[11] * e[8] + e[25] * e[2] * e[17] + + e[10] * e[18] * e[0] + 3. * e[10] * e[19] * e[1] + + e[10] * e[20] * e[2] + e[19] * e[9] * e[0]; + a[169] = 0.5 * e2[23] * e[26] + 0.5 * e[26] * e2[25] + 0.5 * e2[20] * e[26] - + 0.5 * e[26] * e2[18] + 0.5 * e3[26] + 0.5 * e[26] * e2[24] + + e[20] * e[19] * e[25] - 0.5 * e[26] * e2[19] - 0.5 * e[26] * e2[21] + + e[20] * e[18] * e[24] - 0.5 * e[26] * e2[22] + + e[23] * e[21] * e[24] + e[23] * e[22] * e[25]; + a[72] = e[16] * e[9] * e[33] + e[16] * e[29] * e[17] + e[16] * e[11] * e[35] + + e[16] * e[10] * e[34] + e[34] * e[11] * e[17] + e[34] * e[9] * e[15] - + e[10] * e[30] * e[12] - e[10] * e[32] * e[14] - + e[10] * e[33] * e[15] - e[10] * e[35] * e[17] + e[10] * e[27] * e[9] + + e[10] * e[29] * e[11] + e[13] * e[27] * e[12] + e[13] * e[9] * e[30] + + e[13] * e[29] * e[14] + e[13] * e[11] * e[32] + + e[13] * e[10] * e[31] + e[31] * e[11] * e[14] + e[31] * e[9] * e[12] + + e[16] * e[27] * e[15] + 1.5 * e[28] * e2[10] + 0.5 * e[28] * e2[16] + + 0.5 * e[28] * e2[9] + 0.5 * e[28] * e2[11] - 0.5 * e[28] * e2[12] - + 0.5 * e[28] * e2[15] - 0.5 * e[28] * e2[17] - 0.5 * e[28] * e2[14] + + 0.5 * e[28] * e2[13]; + a[179] = + 0.5 * e2[20] * e[35] + 0.5 * e2[23] * e[35] + 1.5 * e[35] * e2[26] + + 0.5 * e[35] * e2[25] + 0.5 * e[35] * e2[24] - 0.5 * e[35] * e2[18] - + 0.5 * e[35] * e2[19] - 0.5 * e[35] * e2[22] - 0.5 * e[35] * e2[21] + + e[20] * e[27] * e[24] + e[20] * e[18] * e[33] + e[20] * e[28] * e[25] + + e[20] * e[19] * e[34] + e[20] * e[29] * e[26] + e[29] * e[19] * e[25] + + e[29] * e[18] * e[24] + e[23] * e[30] * e[24] + e[23] * e[21] * e[33] + + e[23] * e[31] * e[25] + e[23] * e[22] * e[34] + e[23] * e[32] * e[26] + + e[32] * e[22] * e[25] + e[32] * e[21] * e[24] + e[26] * e[33] * e[24] + + e[26] * e[34] * e[25] - e[26] * e[27] * e[18] - e[26] * e[30] * e[21] - + e[26] * e[31] * e[22] - e[26] * e[28] * e[19]; + a[2] = e[4] * e[2] * e[5] + 0.5 * e[1] * e2[0] - 0.5 * e[1] * e2[6] + + e[7] * e[0] * e[6] + 0.5 * e[1] * e2[7] + 0.5 * e[1] * e2[4] - + 0.5 * e[1] * e2[8] + 0.5 * e[1] * e2[2] - 0.5 * e[1] * e2[3] + + 0.5 * e3[1] + e[7] * e[2] * e[8] - 0.5 * e[1] * e2[5] + + e[4] * e[0] * e[3]; + a[19] = -0.5 * e[17] * e2[13] - 0.5 * e[17] * e2[9] + 0.5 * e[17] * e2[16] + + 0.5 * e[17] * e2[15] + 0.5 * e3[17] - 0.5 * e[17] * e2[10] + + e[14] * e[13] * e[16] + e[14] * e[12] * e[15] + 0.5 * e2[14] * e[17] + + e[11] * e[10] * e[16] - 0.5 * e[17] * e2[12] + 0.5 * e2[11] * e[17] + + e[11] * e[9] * e[15]; + a[122] = e[4] * e[27] * e[30] + e[4] * e[29] * e[32] + e[4] * e[28] * e[31] + + e[31] * e[27] * e[3] + e[31] * e[0] * e[30] + e[31] * e[29] * e[5] + + e[31] * e[2] * e[32] + e[7] * e[27] * e[33] + e[7] * e[29] * e[35] + + e[7] * e[28] * e[34] + e[28] * e[27] * e[0] + e[28] * e[29] * e[2] + + e[34] * e[27] * e[6] + e[34] * e[0] * e[33] + e[34] * e[29] * e[8] + + e[34] * e[2] * e[35] - e[28] * e[32] * e[5] - e[28] * e[33] * e[6] - + e[28] * e[30] * e[3] - e[28] * e[35] * e[8] + 0.5 * e[1] * e2[27] + + 0.5 * e[1] * e2[29] + 1.5 * e[1] * e2[28] + 0.5 * e[1] * e2[31] - + 0.5 * e[1] * e2[32] - 0.5 * e[1] * e2[33] - 0.5 * e[1] * e2[30] + + 0.5 * e[1] * e2[34] - 0.5 * e[1] * e2[35]; + a[79] = 0.5 * e2[11] * e[35] + 0.5 * e[35] * e2[16] - 0.5 * e[35] * e2[9] - + 0.5 * e[35] * e2[12] + 0.5 * e[35] * e2[15] + 1.5 * e[35] * e2[17] - + 0.5 * e[35] * e2[10] + 0.5 * e[35] * e2[14] - 0.5 * e[35] * e2[13] + + e[11] * e[27] * e[15] + e[11] * e[9] * e[33] + e[11] * e[29] * e[17] + + e[11] * e[28] * e[16] + e[11] * e[10] * e[34] + e[29] * e[9] * e[15] + + e[29] * e[10] * e[16] + e[14] * e[30] * e[15] + + e[14] * e[12] * e[33] + e[14] * e[32] * e[17] + + e[14] * e[31] * e[16] + e[14] * e[13] * e[34] + + e[32] * e[12] * e[15] + e[32] * e[13] * e[16] + + e[17] * e[33] * e[15] + e[17] * e[34] * e[16] - e[17] * e[27] * e[9] - + e[17] * e[30] * e[12] - e[17] * e[28] * e[10] - e[17] * e[31] * e[13]; + a[192] = e[34] * e[27] * e[33] + e[34] * e[29] * e[35] - + 0.5 * e[28] * e2[30] - 0.5 * e[28] * e2[35] + 0.5 * e3[28] + + 0.5 * e[28] * e2[27] + 0.5 * e[28] * e2[29] + e[31] * e[27] * e[30] + + e[31] * e[29] * e[32] - 0.5 * e[28] * e2[32] - 0.5 * e[28] * e2[33] + + 0.5 * e[28] * e2[31] + 0.5 * e[28] * e2[34]; + a[9] = 0.5 * e2[5] * e[8] + e[2] * e[0] * e[6] + 0.5 * e2[2] * e[8] + + 0.5 * e3[8] - 0.5 * e[8] * e2[0] + e[5] * e[4] * e[7] + + e[5] * e[3] * e[6] + 0.5 * e[8] * e2[7] + e[2] * e[1] * e[7] - + 0.5 * e[8] * e2[1] - 0.5 * e[8] * e2[4] - 0.5 * e[8] * e2[3] + + 0.5 * e[8] * e2[6]; + a[152] = + e[28] * e[27] * e[9] + e[28] * e[29] * e[11] - e[28] * e[30] * e[12] + + e[28] * e[31] * e[13] - e[28] * e[32] * e[14] - e[28] * e[33] * e[15] - + e[28] * e[35] * e[17] + e[31] * e[27] * e[12] + e[31] * e[9] * e[30] + + e[31] * e[29] * e[14] + e[31] * e[11] * e[32] + e[13] * e[27] * e[30] + + e[13] * e[29] * e[32] + e[16] * e[27] * e[33] + e[16] * e[29] * e[35] + + e[34] * e[27] * e[15] + e[34] * e[9] * e[33] + e[34] * e[29] * e[17] + + e[34] * e[11] * e[35] + e[34] * e[28] * e[16] + 0.5 * e[10] * e2[27] + + 0.5 * e[10] * e2[29] + 1.5 * e[10] * e2[28] - 0.5 * e[10] * e2[32] + + 0.5 * e[10] * e2[31] - 0.5 * e[10] * e2[33] - 0.5 * e[10] * e2[30] + + 0.5 * e[10] * e2[34] - 0.5 * e[10] * e2[35]; + a[59] = -0.5 * e[35] * e2[1] + 0.5 * e[35] * e2[7] - 0.5 * e[35] * e2[3] + + 0.5 * e2[2] * e[35] + 1.5 * e[35] * e2[8] - 0.5 * e[35] * e2[4] - + 0.5 * e[35] * e2[0] + 0.5 * e[35] * e2[6] + 0.5 * e[35] * e2[5] + + e[2] * e[27] * e[6] + e[2] * e[0] * e[33] + e[2] * e[28] * e[7] + + e[2] * e[1] * e[34] + e[2] * e[29] * e[8] - e[8] * e[27] * e[0] + + e[8] * e[34] * e[7] + e[8] * e[32] * e[5] + e[8] * e[33] * e[6] - + e[8] * e[30] * e[3] - e[8] * e[28] * e[1] - e[8] * e[31] * e[4] + + e[29] * e[1] * e[7] + e[29] * e[0] * e[6] + e[5] * e[30] * e[6] + + e[5] * e[3] * e[33] + e[5] * e[31] * e[7] + e[5] * e[4] * e[34] + + e[32] * e[4] * e[7] + e[32] * e[3] * e[6]; + a[182] = + e[28] * e[27] * e[18] + e[28] * e[29] * e[20] + e[22] * e[27] * e[30] + + e[22] * e[29] * e[32] + e[22] * e[28] * e[31] + e[31] * e[27] * e[21] + + e[31] * e[18] * e[30] + e[31] * e[29] * e[23] + e[31] * e[20] * e[32] + + e[25] * e[27] * e[33] + e[25] * e[29] * e[35] + e[25] * e[28] * e[34] + + e[34] * e[27] * e[24] + e[34] * e[18] * e[33] + e[34] * e[29] * e[26] + + e[34] * e[20] * e[35] - e[28] * e[33] * e[24] - e[28] * e[30] * e[21] - + e[28] * e[35] * e[26] - e[28] * e[32] * e[23] - 0.5 * e[19] * e2[33] - + 0.5 * e[19] * e2[30] - 0.5 * e[19] * e2[35] + 0.5 * e[19] * e2[27] + + 0.5 * e[19] * e2[29] + 1.5 * e[19] * e2[28] + 0.5 * e[19] * e2[31] + + 0.5 * e[19] * e2[34] - 0.5 * e[19] * e2[32]; + a[89] = e[23] * e[3] * e[15] - e[17] * e[19] * e[1] - e[17] * e[22] * e[4] - + e[17] * e[18] * e[0] + e[17] * e[25] * e[7] + e[17] * e[24] * e[6] + + e[14] * e[21] * e[6] + e[14] * e[3] * e[24] + e[14] * e[22] * e[7] + + e[14] * e[4] * e[25] + e[14] * e[23] * e[8] - e[26] * e[10] * e[1] - + e[26] * e[13] * e[4] + e[26] * e[16] * e[7] + e[26] * e[15] * e[6] - + e[26] * e[9] * e[0] - e[26] * e[12] * e[3] + e[23] * e[12] * e[6] + + e[11] * e[18] * e[6] + e[11] * e[0] * e[24] + e[11] * e[19] * e[7] + + e[11] * e[1] * e[25] + e[11] * e[20] * e[8] + e[11] * e[2] * e[26] + + e[20] * e[9] * e[6] + e[20] * e[0] * e[15] + e[20] * e[10] * e[7] + + e[20] * e[1] * e[16] + e[20] * e[2] * e[17] + e[5] * e[21] * e[15] + + e[5] * e[12] * e[24] + e[5] * e[23] * e[17] + e[5] * e[14] * e[26] + + e[5] * e[22] * e[16] + e[5] * e[13] * e[25] + e[8] * e[24] * e[15] + + 3. * e[8] * e[26] * e[17] + e[8] * e[25] * e[16] + + e[2] * e[18] * e[15] + e[2] * e[9] * e[24] + e[2] * e[19] * e[16] + + e[2] * e[10] * e[25] - e[17] * e[21] * e[3] + e[23] * e[4] * e[16] + + e[23] * e[13] * e[7] - e[8] * e[18] * e[9] - e[8] * e[21] * e[12] - + e[8] * e[19] * e[10] - e[8] * e[22] * e[13]; + a[62] = e[13] * e[18] * e[12] + e[13] * e[9] * e[21] + e[13] * e[20] * e[14] + + e[13] * e[11] * e[23] + e[13] * e[10] * e[22] + + e[22] * e[11] * e[14] + e[22] * e[9] * e[12] + e[16] * e[18] * e[15] + + e[16] * e[9] * e[24] + e[16] * e[20] * e[17] + e[16] * e[11] * e[26] + + e[16] * e[10] * e[25] + e[25] * e[11] * e[17] + e[25] * e[9] * e[15] - + e[10] * e[23] * e[14] - e[10] * e[24] * e[15] - + e[10] * e[26] * e[17] + e[10] * e[20] * e[11] + e[10] * e[18] * e[9] - + e[10] * e[21] * e[12] + 0.5 * e[19] * e2[11] + 0.5 * e[19] * e2[9] + + 1.5 * e[19] * e2[10] + 0.5 * e[19] * e2[13] + 0.5 * e[19] * e2[16] - + 0.5 * e[19] * e2[12] - 0.5 * e[19] * e2[15] - 0.5 * e[19] * e2[17] - + 0.5 * e[19] * e2[14]; + a[88] = e[10] * e[18] * e[6] + e[10] * e[0] * e[24] + e[10] * e[19] * e[7] + + e[10] * e[1] * e[25] + e[10] * e[20] * e[8] + e[10] * e[2] * e[26] + + e[19] * e[9] * e[6] + e[19] * e[0] * e[15] + e[19] * e[1] * e[16] + + e[19] * e[11] * e[8] + e[19] * e[2] * e[17] + e[4] * e[21] * e[15] + + e[4] * e[12] * e[24] + e[4] * e[23] * e[17] + e[4] * e[14] * e[26] + + e[4] * e[22] * e[16] + e[4] * e[13] * e[25] + e[7] * e[24] * e[15] + + e[7] * e[26] * e[17] + 3. * e[7] * e[25] * e[16] + + e[1] * e[18] * e[15] + e[1] * e[9] * e[24] + e[1] * e[20] * e[17] + + e[1] * e[11] * e[26] - e[16] * e[21] * e[3] + e[16] * e[26] * e[8] - + e[16] * e[20] * e[2] - e[16] * e[18] * e[0] - e[16] * e[23] * e[5] + + e[16] * e[24] * e[6] + e[13] * e[21] * e[6] + e[13] * e[3] * e[24] + + e[13] * e[22] * e[7] + e[13] * e[23] * e[8] + e[13] * e[5] * e[26] - + e[25] * e[11] * e[2] + e[25] * e[15] * e[6] - e[25] * e[9] * e[0] - + e[25] * e[14] * e[5] - e[25] * e[12] * e[3] + e[25] * e[17] * e[8] + + e[22] * e[12] * e[6] + e[22] * e[3] * e[15] + e[22] * e[14] * e[8] + + e[22] * e[5] * e[17] - e[7] * e[23] * e[14] - e[7] * e[20] * e[11] - + e[7] * e[18] * e[9] - e[7] * e[21] * e[12]; + a[32] = e[13] * e[9] * e[3] + e[13] * e[0] * e[12] + e[13] * e[10] * e[4] + + e[13] * e[11] * e[5] + e[13] * e[2] * e[14] + e[16] * e[9] * e[6] + + e[16] * e[0] * e[15] + e[16] * e[10] * e[7] + e[16] * e[11] * e[8] + + e[16] * e[2] * e[17] + e[7] * e[11] * e[17] + e[7] * e[9] * e[15] + + e[4] * e[11] * e[14] + e[4] * e[9] * e[12] + e[10] * e[9] * e[0] + + e[10] * e[11] * e[2] - e[10] * e[15] * e[6] - e[10] * e[14] * e[5] - + e[10] * e[12] * e[3] - e[10] * e[17] * e[8] + 0.5 * e[1] * e2[11] + + 0.5 * e[1] * e2[9] + 1.5 * e[1] * e2[10] - 0.5 * e[1] * e2[12] - + 0.5 * e[1] * e2[15] - 0.5 * e[1] * e2[17] - 0.5 * e[1] * e2[14] + + 0.5 * e[1] * e2[13] + 0.5 * e[1] * e2[16]; + a[58] = e[1] * e[27] * e[6] + e[1] * e[0] * e[33] + e[1] * e[28] * e[7] + + e[1] * e[29] * e[8] + e[1] * e[2] * e[35] - e[7] * e[27] * e[0] - + e[7] * e[32] * e[5] + e[7] * e[33] * e[6] - e[7] * e[30] * e[3] + + e[7] * e[35] * e[8] - e[7] * e[29] * e[2] + e[7] * e[31] * e[4] + + e[28] * e[0] * e[6] + e[28] * e[2] * e[8] + e[4] * e[30] * e[6] + + e[4] * e[3] * e[33] + e[4] * e[32] * e[8] + e[4] * e[5] * e[35] + + e[31] * e[3] * e[6] + e[31] * e[5] * e[8] + 0.5 * e2[1] * e[34] + + 1.5 * e[34] * e2[7] + 0.5 * e[34] * e2[4] - 0.5 * e[34] * e2[0] + + 0.5 * e[34] * e2[6] - 0.5 * e[34] * e2[5] - 0.5 * e[34] * e2[3] - + 0.5 * e[34] * e2[2] + 0.5 * e[34] * e2[8]; + a[42] = e[4] * e[18] * e[3] + e[4] * e[0] * e[21] + e[4] * e[1] * e[22] + + e[4] * e[20] * e[5] + e[4] * e[2] * e[23] + e[22] * e[0] * e[3] + + e[22] * e[2] * e[5] + e[7] * e[18] * e[6] + e[7] * e[0] * e[24] + + e[7] * e[1] * e[25] + e[7] * e[20] * e[8] + e[7] * e[2] * e[26] + + e[25] * e[0] * e[6] + e[25] * e[2] * e[8] + e[1] * e[18] * e[0] + + e[1] * e[20] * e[2] - e[1] * e[21] * e[3] - e[1] * e[26] * e[8] - + e[1] * e[23] * e[5] - e[1] * e[24] * e[6] + 0.5 * e[19] * e2[4] + + 0.5 * e[19] * e2[0] - 0.5 * e[19] * e2[6] - 0.5 * e[19] * e2[5] + + 1.5 * e[19] * e2[1] + 0.5 * e[19] * e2[7] - 0.5 * e[19] * e2[3] + + 0.5 * e[19] * e2[2] - 0.5 * e[19] * e2[8]; + a[8] = -0.5 * e[7] * e2[0] + e[4] * e[5] * e[8] + 0.5 * e2[4] * e[7] - + 0.5 * e[7] * e2[2] + 0.5 * e[7] * e2[8] - 0.5 * e[7] * e2[5] + + 0.5 * e[7] * e2[6] + e[1] * e[0] * e[6] + 0.5 * e3[7] + + e[4] * e[3] * e[6] + e[1] * e[2] * e[8] - 0.5 * e[7] * e2[3] + + 0.5 * e2[1] * e[7]; + a[112] = -e[1] * e[32] * e[23] - e[19] * e[32] * e[5] - e[19] * e[33] * e[6] - + e[19] * e[30] * e[3] - e[19] * e[35] * e[8] - e[28] * e[21] * e[3] - + e[28] * e[26] * e[8] - e[28] * e[23] * e[5] - e[28] * e[24] * e[6] + + e[7] * e[27] * e[24] + e[7] * e[18] * e[33] + e[7] * e[29] * e[26] + + e[7] * e[20] * e[35] + e[22] * e[27] * e[3] + e[22] * e[0] * e[30] + + e[22] * e[29] * e[5] + e[22] * e[2] * e[32] + e[31] * e[18] * e[3] + + e[31] * e[0] * e[21] + e[31] * e[20] * e[5] + e[31] * e[2] * e[23] + + e[25] * e[27] * e[6] + e[25] * e[0] * e[33] + e[25] * e[28] * e[7] + + e[25] * e[1] * e[34] + e[25] * e[29] * e[8] + e[25] * e[2] * e[35] + + e[34] * e[18] * e[6] + e[34] * e[0] * e[24] + e[34] * e[19] * e[7] + + e[34] * e[20] * e[8] + e[34] * e[2] * e[26] + e[1] * e[27] * e[18] + + 3. * e[1] * e[28] * e[19] + e[1] * e[29] * e[20] + + e[19] * e[27] * e[0] + e[19] * e[29] * e[2] + e[28] * e[18] * e[0] + + e[28] * e[20] * e[2] + e[4] * e[27] * e[21] + e[4] * e[18] * e[30] + + e[4] * e[28] * e[22] + e[4] * e[19] * e[31] + e[4] * e[29] * e[23] + + e[4] * e[20] * e[32] - e[1] * e[33] * e[24] - e[1] * e[30] * e[21] - + e[1] * e[35] * e[26] + e[1] * e[31] * e[22]; + a[78] = + e[10] * e[27] * e[15] + e[10] * e[9] * e[33] + e[10] * e[29] * e[17] + + e[10] * e[11] * e[35] + e[10] * e[28] * e[16] + e[28] * e[11] * e[17] + + e[28] * e[9] * e[15] + e[13] * e[30] * e[15] + e[13] * e[12] * e[33] + + e[13] * e[32] * e[17] + e[13] * e[14] * e[35] + e[13] * e[31] * e[16] + + e[31] * e[14] * e[17] + e[31] * e[12] * e[15] + e[16] * e[33] * e[15] + + e[16] * e[35] * e[17] - e[16] * e[27] * e[9] - e[16] * e[30] * e[12] - + e[16] * e[32] * e[14] - e[16] * e[29] * e[11] + 0.5 * e2[10] * e[34] + + 1.5 * e[34] * e2[16] - 0.5 * e[34] * e2[9] - 0.5 * e[34] * e2[11] - + 0.5 * e[34] * e2[12] + 0.5 * e[34] * e2[15] + 0.5 * e[34] * e2[17] - + 0.5 * e[34] * e2[14] + 0.5 * e[34] * e2[13]; + a[162] = 0.5 * e[19] * e2[18] + 0.5 * e[19] * e2[25] + 0.5 * e[19] * e2[22] + + e[25] * e[20] * e[26] - 0.5 * e[19] * e2[21] + 0.5 * e[19] * e2[20] - + 0.5 * e[19] * e2[26] - 0.5 * e[19] * e2[23] - 0.5 * e[19] * e2[24] + + 0.5 * e3[19] + e[22] * e[20] * e[23] + e[25] * e[18] * e[24] + + e[22] * e[18] * e[21]; + a[198] = 0.5 * e[34] * e2[33] + 0.5 * e[34] * e2[35] - 0.5 * e[34] * e2[27] - + 0.5 * e[34] * e2[32] - 0.5 * e[34] * e2[29] - 0.5 * e[34] * e2[30] + + 0.5 * e2[28] * e[34] + e[31] * e[30] * e[33] + + e[31] * e[32] * e[35] + e[28] * e[27] * e[33] + 0.5 * e3[34] + + e[28] * e[29] * e[35] + 0.5 * e2[31] * e[34]; + a[92] = e[4] * e[28] * e[13] + e[4] * e[10] * e[31] + e[7] * e[27] * e[15] + + e[7] * e[9] * e[33] + e[7] * e[29] * e[17] + e[7] * e[11] * e[35] + + e[7] * e[28] * e[16] + e[7] * e[10] * e[34] + e[1] * e[27] * e[9] + + e[1] * e[29] * e[11] + 3. * e[1] * e[28] * e[10] + + e[10] * e[27] * e[0] - e[10] * e[32] * e[5] - e[10] * e[33] * e[6] - + e[10] * e[30] * e[3] - e[10] * e[35] * e[8] + e[10] * e[29] * e[2] + + e[13] * e[27] * e[3] + e[13] * e[0] * e[30] + e[13] * e[1] * e[31] + + e[13] * e[29] * e[5] + e[13] * e[2] * e[32] + e[28] * e[11] * e[2] - + e[28] * e[15] * e[6] + e[28] * e[9] * e[0] - e[28] * e[14] * e[5] - + e[28] * e[12] * e[3] - e[28] * e[17] * e[8] + e[31] * e[9] * e[3] + + e[31] * e[0] * e[12] + e[31] * e[11] * e[5] + e[31] * e[2] * e[14] + + e[16] * e[27] * e[6] + e[16] * e[0] * e[33] + e[16] * e[1] * e[34] + + e[16] * e[29] * e[8] + e[16] * e[2] * e[35] - e[1] * e[30] * e[12] - + e[1] * e[32] * e[14] - e[1] * e[33] * e[15] - e[1] * e[35] * e[17] + + e[34] * e[9] * e[6] + e[34] * e[0] * e[15] + e[34] * e[11] * e[8] + + e[34] * e[2] * e[17] + e[4] * e[27] * e[12] + e[4] * e[9] * e[30] + + e[4] * e[29] * e[14] + e[4] * e[11] * e[32]; + a[128] = e[4] * e[30] * e[33] + e[4] * e[32] * e[35] + e[4] * e[31] * e[34] + + e[31] * e[30] * e[6] + e[31] * e[3] * e[33] + e[31] * e[32] * e[8] + + e[31] * e[5] * e[35] + e[28] * e[27] * e[6] + e[28] * e[0] * e[33] + + e[28] * e[29] * e[8] + e[28] * e[2] * e[35] + e[34] * e[33] * e[6] + + e[34] * e[35] * e[8] - e[34] * e[27] * e[0] - e[34] * e[32] * e[5] - + e[34] * e[30] * e[3] - e[34] * e[29] * e[2] + e[1] * e[27] * e[33] + + e[1] * e[29] * e[35] + e[1] * e[28] * e[34] + 0.5 * e2[31] * e[7] - + 0.5 * e[7] * e2[27] - 0.5 * e[7] * e2[32] + 0.5 * e[7] * e2[28] - + 0.5 * e[7] * e2[29] + 0.5 * e[7] * e2[33] - 0.5 * e[7] * e2[30] + + 1.5 * e[7] * e2[34] + 0.5 * e[7] * e2[35]; + a[12] = -0.5 * e[10] * e2[14] - 0.5 * e[10] * e2[17] - 0.5 * e[10] * e2[15] + + e[13] * e[11] * e[14] + e[16] * e[11] * e[17] + 0.5 * e[10] * e2[13] + + e[13] * e[9] * e[12] - 0.5 * e[10] * e2[12] + 0.5 * e3[10] + + e[16] * e[9] * e[15] + 0.5 * e[10] * e2[16] + 0.5 * e[10] * e2[11] + + 0.5 * e[10] * e2[9]; + a[188] = + e[22] * e[32] * e[35] + e[22] * e[31] * e[34] + e[31] * e[30] * e[24] + + e[31] * e[21] * e[33] + e[31] * e[32] * e[26] + e[31] * e[23] * e[35] + + e[34] * e[33] * e[24] + e[34] * e[35] * e[26] - e[34] * e[27] * e[18] - + e[34] * e[30] * e[21] - e[34] * e[29] * e[20] - e[34] * e[32] * e[23] + + e[19] * e[27] * e[33] + e[19] * e[29] * e[35] + e[19] * e[28] * e[34] + + e[28] * e[27] * e[24] + e[28] * e[18] * e[33] + e[28] * e[29] * e[26] + + e[28] * e[20] * e[35] + e[22] * e[30] * e[33] + 0.5 * e2[28] * e[25] + + 0.5 * e2[31] * e[25] + 0.5 * e[25] * e2[33] + 0.5 * e[25] * e2[35] + + 1.5 * e[25] * e2[34] - 0.5 * e[25] * e2[27] - 0.5 * e[25] * e2[32] - + 0.5 * e[25] * e2[29] - 0.5 * e[25] * e2[30]; + a[172] = + -e[19] * e[35] * e[26] - e[19] * e[32] * e[23] + e[19] * e[27] * e[18] + + e[19] * e[29] * e[20] + e[22] * e[27] * e[21] + e[22] * e[18] * e[30] + + e[22] * e[19] * e[31] + e[22] * e[29] * e[23] + e[22] * e[20] * e[32] + + e[31] * e[18] * e[21] + e[31] * e[20] * e[23] + e[25] * e[27] * e[24] + + e[25] * e[18] * e[33] + e[25] * e[19] * e[34] + e[25] * e[29] * e[26] + + e[25] * e[20] * e[35] + e[34] * e[18] * e[24] + e[34] * e[20] * e[26] - + e[19] * e[33] * e[24] - e[19] * e[30] * e[21] + 1.5 * e[28] * e2[19] + + 0.5 * e[28] * e2[18] + 0.5 * e[28] * e2[20] + 0.5 * e[28] * e2[22] + + 0.5 * e[28] * e2[25] - 0.5 * e[28] * e2[26] - 0.5 * e[28] * e2[23] - + 0.5 * e[28] * e2[24] - 0.5 * e[28] * e2[21]; + a[158] = + e[10] * e[27] * e[33] + e[10] * e[29] * e[35] + e[10] * e[28] * e[34] + + e[34] * e[33] * e[15] + e[34] * e[35] * e[17] + e[28] * e[27] * e[15] + + e[28] * e[9] * e[33] + e[28] * e[29] * e[17] + e[28] * e[11] * e[35] - + e[34] * e[27] * e[9] - e[34] * e[30] * e[12] + e[34] * e[31] * e[13] - + e[34] * e[32] * e[14] - e[34] * e[29] * e[11] + e[31] * e[30] * e[15] + + e[31] * e[12] * e[33] + e[31] * e[32] * e[17] + e[31] * e[14] * e[35] + + e[13] * e[30] * e[33] + e[13] * e[32] * e[35] - 0.5 * e[16] * e2[27] - + 0.5 * e[16] * e2[32] + 0.5 * e[16] * e2[28] - 0.5 * e[16] * e2[29] + + 0.5 * e[16] * e2[31] + 0.5 * e[16] * e2[33] - 0.5 * e[16] * e2[30] + + 1.5 * e[16] * e2[34] + 0.5 * e[16] * e2[35]; + a[153] = + e[29] * e[32] * e[14] - e[29] * e[33] * e[15] - e[29] * e[34] * e[16] + + e[32] * e[27] * e[12] + e[32] * e[9] * e[30] + e[32] * e[28] * e[13] + + e[32] * e[10] * e[31] + e[14] * e[27] * e[30] + e[14] * e[28] * e[31] + + e[17] * e[27] * e[33] + e[17] * e[28] * e[34] + e[35] * e[27] * e[15] + + e[35] * e[9] * e[33] + e[35] * e[29] * e[17] + e[35] * e[28] * e[16] + + e[35] * e[10] * e[34] + e[29] * e[27] * e[9] + e[29] * e[28] * e[10] - + e[29] * e[30] * e[12] - e[29] * e[31] * e[13] + 0.5 * e[11] * e2[27] + + 1.5 * e[11] * e2[29] + 0.5 * e[11] * e2[28] + 0.5 * e[11] * e2[32] - + 0.5 * e[11] * e2[31] - 0.5 * e[11] * e2[33] - 0.5 * e[11] * e2[30] - + 0.5 * e[11] * e2[34] + 0.5 * e[11] * e2[35]; + a[118] = e[1] * e[20] * e[35] + e[19] * e[27] * e[6] + e[19] * e[0] * e[33] + + e[19] * e[28] * e[7] + e[19] * e[29] * e[8] + e[19] * e[2] * e[35] + + e[28] * e[18] * e[6] + e[28] * e[0] * e[24] + e[28] * e[20] * e[8] + + e[28] * e[2] * e[26] + e[4] * e[30] * e[24] + e[4] * e[21] * e[33] + + e[4] * e[31] * e[25] + e[4] * e[22] * e[34] + e[4] * e[32] * e[26] + + e[4] * e[23] * e[35] - e[7] * e[27] * e[18] + e[7] * e[33] * e[24] - + e[7] * e[30] * e[21] - e[7] * e[29] * e[20] + e[7] * e[35] * e[26] + + e[7] * e[31] * e[22] - e[7] * e[32] * e[23] - e[25] * e[27] * e[0] - + e[25] * e[32] * e[5] - e[25] * e[30] * e[3] - e[25] * e[29] * e[2] - + e[34] * e[21] * e[3] - e[34] * e[20] * e[2] - e[34] * e[18] * e[0] - + e[34] * e[23] * e[5] + e[22] * e[30] * e[6] + e[22] * e[3] * e[33] + + e[22] * e[32] * e[8] + e[22] * e[5] * e[35] + e[31] * e[21] * e[6] + + e[31] * e[3] * e[24] + e[31] * e[23] * e[8] + e[31] * e[5] * e[26] + + e[34] * e[26] * e[8] + e[1] * e[27] * e[24] + e[1] * e[18] * e[33] + + e[1] * e[28] * e[25] + e[1] * e[19] * e[34] + e[1] * e[29] * e[26] + + e[34] * e[24] * e[6] + e[25] * e[33] * e[6] + + 3. * e[25] * e[34] * e[7] + e[25] * e[35] * e[8]; + a[183] = + 0.5 * e[20] * e2[27] + 1.5 * e[20] * e2[29] + 0.5 * e[20] * e2[28] + + 0.5 * e[20] * e2[32] + 0.5 * e[20] * e2[35] - 0.5 * e[20] * e2[31] - + 0.5 * e[20] * e2[33] - 0.5 * e[20] * e2[30] - 0.5 * e[20] * e2[34] + + e[29] * e[27] * e[18] + e[29] * e[28] * e[19] + e[23] * e[27] * e[30] + + e[23] * e[29] * e[32] + e[23] * e[28] * e[31] + e[32] * e[27] * e[21] + + e[32] * e[18] * e[30] + e[32] * e[28] * e[22] + e[32] * e[19] * e[31] + + e[26] * e[27] * e[33] + e[26] * e[29] * e[35] + e[26] * e[28] * e[34] + + e[35] * e[27] * e[24] + e[35] * e[18] * e[33] + e[35] * e[28] * e[25] + + e[35] * e[19] * e[34] - e[29] * e[33] * e[24] - e[29] * e[30] * e[21] - + e[29] * e[31] * e[22] - e[29] * e[34] * e[25]; + a[48] = e[19] * e[1] * e[7] + e[19] * e[0] * e[6] + e[19] * e[2] * e[8] + + e[4] * e[21] * e[6] + e[4] * e[3] * e[24] + e[4] * e[22] * e[7] + + e[4] * e[23] * e[8] + e[4] * e[5] * e[26] + e[22] * e[3] * e[6] + + e[22] * e[5] * e[8] + e[7] * e[24] * e[6] + e[7] * e[26] * e[8] + + e[1] * e[18] * e[6] + e[1] * e[0] * e[24] + e[1] * e[20] * e[8] + + e[1] * e[2] * e[26] - e[7] * e[21] * e[3] - e[7] * e[20] * e[2] - + e[7] * e[18] * e[0] - e[7] * e[23] * e[5] + 0.5 * e[25] * e2[4] - + 0.5 * e[25] * e2[0] + 0.5 * e[25] * e2[6] - 0.5 * e[25] * e2[5] + + 0.5 * e[25] * e2[1] + 1.5 * e[25] * e2[7] - 0.5 * e[25] * e2[3] - + 0.5 * e[25] * e2[2] + 0.5 * e[25] * e2[8]; + a[123] = e[5] * e[27] * e[30] + e[5] * e[29] * e[32] + e[5] * e[28] * e[31] + + e[32] * e[27] * e[3] + e[32] * e[0] * e[30] + e[32] * e[28] * e[4] + + e[32] * e[1] * e[31] + e[8] * e[27] * e[33] + e[8] * e[29] * e[35] + + e[8] * e[28] * e[34] + e[29] * e[27] * e[0] + e[29] * e[28] * e[1] + + e[35] * e[27] * e[6] + e[35] * e[0] * e[33] + e[35] * e[28] * e[7] + + e[35] * e[1] * e[34] - e[29] * e[34] * e[7] - e[29] * e[33] * e[6] - + e[29] * e[30] * e[3] - e[29] * e[31] * e[4] + 0.5 * e[2] * e2[27] + + 1.5 * e[2] * e2[29] + 0.5 * e[2] * e2[28] + 0.5 * e[2] * e2[32] - + 0.5 * e[2] * e2[31] - 0.5 * e[2] * e2[33] - 0.5 * e[2] * e2[30] - + 0.5 * e[2] * e2[34] + 0.5 * e[2] * e2[35]; + a[38] = e[13] * e[12] * e[6] + e[13] * e[3] * e[15] + e[13] * e[4] * e[16] + + e[13] * e[14] * e[8] + e[13] * e[5] * e[17] + e[16] * e[15] * e[6] + + e[16] * e[17] * e[8] + e[1] * e[11] * e[17] + e[1] * e[9] * e[15] + + e[1] * e[10] * e[16] + e[4] * e[14] * e[17] + e[4] * e[12] * e[15] + + e[10] * e[9] * e[6] + e[10] * e[0] * e[15] + e[10] * e[11] * e[8] + + e[10] * e[2] * e[17] - e[16] * e[11] * e[2] - e[16] * e[9] * e[0] - + e[16] * e[14] * e[5] - e[16] * e[12] * e[3] + 0.5 * e2[13] * e[7] + + 1.5 * e2[16] * e[7] + 0.5 * e[7] * e2[17] + 0.5 * e[7] * e2[15] - + 0.5 * e[7] * e2[9] - 0.5 * e[7] * e2[11] - 0.5 * e[7] * e2[12] + + 0.5 * e[7] * e2[10] - 0.5 * e[7] * e2[14]; + a[193] = 0.5 * e[29] * e2[32] + 0.5 * e[29] * e2[35] - 0.5 * e[29] * e2[31] - + 0.5 * e[29] * e2[33] - 0.5 * e[29] * e2[30] - 0.5 * e[29] * e2[34] + + e[32] * e[27] * e[30] + 0.5 * e3[29] + 0.5 * e[29] * e2[28] + + e[35] * e[28] * e[34] + 0.5 * e[29] * e2[27] + + e[35] * e[27] * e[33] + e[32] * e[28] * e[31]; + a[68] = + -e[16] * e[21] * e[12] + e[10] * e[18] * e[15] + e[10] * e[9] * e[24] + + e[10] * e[20] * e[17] + e[10] * e[11] * e[26] + e[19] * e[11] * e[17] + + e[19] * e[9] * e[15] + e[19] * e[10] * e[16] + e[13] * e[21] * e[15] + + e[13] * e[12] * e[24] + e[13] * e[23] * e[17] + e[13] * e[14] * e[26] + + e[13] * e[22] * e[16] + e[22] * e[14] * e[17] + e[22] * e[12] * e[15] + + e[16] * e[24] * e[15] + e[16] * e[26] * e[17] - e[16] * e[23] * e[14] - + e[16] * e[20] * e[11] - e[16] * e[18] * e[9] + 0.5 * e2[13] * e[25] + + 1.5 * e[25] * e2[16] + 0.5 * e[25] * e2[17] + 0.5 * e[25] * e2[15] + + 0.5 * e2[10] * e[25] - 0.5 * e[25] * e2[9] - 0.5 * e[25] * e2[11] - + 0.5 * e[25] * e2[12] - 0.5 * e[25] * e2[14]; + a[102] = e[19] * e[20] * e[2] + e[22] * e[18] * e[3] + e[22] * e[0] * e[21] + + e[22] * e[19] * e[4] + e[22] * e[20] * e[5] + e[22] * e[2] * e[23] - + e[19] * e[21] * e[3] - e[19] * e[26] * e[8] + e[19] * e[25] * e[7] - + e[19] * e[23] * e[5] - e[19] * e[24] * e[6] + e[4] * e[18] * e[21] + + e[4] * e[20] * e[23] + e[25] * e[18] * e[6] + e[25] * e[0] * e[24] + + e[25] * e[20] * e[8] + e[25] * e[2] * e[26] + e[7] * e[18] * e[24] + + e[7] * e[20] * e[26] + e[19] * e[18] * e[0] + 1.5 * e2[19] * e[1] + + 0.5 * e[1] * e2[22] + 0.5 * e[1] * e2[18] + 0.5 * e[1] * e2[20] + + 0.5 * e[1] * e2[25] - 0.5 * e[1] * e2[26] - 0.5 * e[1] * e2[23] - + 0.5 * e[1] * e2[24] - 0.5 * e[1] * e2[21]; + a[178] = + e[19] * e[27] * e[24] + e[19] * e[18] * e[33] + e[19] * e[28] * e[25] + + e[19] * e[29] * e[26] + e[19] * e[20] * e[35] + e[28] * e[18] * e[24] + + e[28] * e[20] * e[26] + e[22] * e[30] * e[24] + e[22] * e[21] * e[33] + + e[22] * e[31] * e[25] + e[22] * e[32] * e[26] + e[22] * e[23] * e[35] + + e[31] * e[21] * e[24] + e[31] * e[23] * e[26] + e[25] * e[33] * e[24] + + e[25] * e[35] * e[26] - e[25] * e[27] * e[18] - e[25] * e[30] * e[21] - + e[25] * e[29] * e[20] - e[25] * e[32] * e[23] - 0.5 * e[34] * e2[18] - + 0.5 * e[34] * e2[23] - 0.5 * e[34] * e2[20] - 0.5 * e[34] * e2[21] + + 0.5 * e2[19] * e[34] + 0.5 * e2[22] * e[34] + 1.5 * e[34] * e2[25] + + 0.5 * e[34] * e2[24] + 0.5 * e[34] * e2[26]; + a[22] = e[16] * e[0] * e[6] + e[16] * e[2] * e[8] + e[1] * e[11] * e[2] - + e[1] * e[15] * e[6] + e[1] * e[9] * e[0] - e[1] * e[14] * e[5] - + e[1] * e[12] * e[3] - e[1] * e[17] * e[8] + e[4] * e[9] * e[3] + + e[4] * e[0] * e[12] + e[4] * e[1] * e[13] + e[4] * e[11] * e[5] + + e[4] * e[2] * e[14] + e[13] * e[0] * e[3] + e[13] * e[2] * e[5] + + e[7] * e[9] * e[6] + e[7] * e[0] * e[15] + e[7] * e[1] * e[16] + + e[7] * e[11] * e[8] + e[7] * e[2] * e[17] - 0.5 * e[10] * e2[6] - + 0.5 * e[10] * e2[5] - 0.5 * e[10] * e2[3] - 0.5 * e[10] * e2[8] + + 1.5 * e[10] * e2[1] + 0.5 * e[10] * e2[0] + 0.5 * e[10] * e2[2] + + 0.5 * e[10] * e2[4] + 0.5 * e[10] * e2[7]; + a[18] = e[13] * e[14] * e[17] + e[13] * e[12] * e[15] + e[10] * e[9] * e[15] + + 0.5 * e[16] * e2[15] - 0.5 * e[16] * e2[11] - 0.5 * e[16] * e2[12] - + 0.5 * e[16] * e2[14] + e[10] * e[11] * e[17] + 0.5 * e2[10] * e[16] + + 0.5 * e3[16] - 0.5 * e[16] * e2[9] + 0.5 * e[16] * e2[17] + + 0.5 * e2[13] * e[16]; + a[142] = + e[10] * e[29] * e[20] + e[22] * e[27] * e[12] + e[22] * e[9] * e[30] + + e[22] * e[29] * e[14] + e[22] * e[11] * e[32] + e[22] * e[10] * e[31] + + e[31] * e[18] * e[12] + e[31] * e[9] * e[21] + e[31] * e[20] * e[14] + + e[31] * e[11] * e[23] - e[10] * e[33] * e[24] - e[10] * e[30] * e[21] - + e[10] * e[35] * e[26] - e[10] * e[32] * e[23] + e[10] * e[34] * e[25] + + e[19] * e[27] * e[9] + e[19] * e[29] * e[11] + e[28] * e[18] * e[9] + + e[28] * e[20] * e[11] + e[16] * e[27] * e[24] + e[16] * e[18] * e[33] + + e[16] * e[28] * e[25] + e[16] * e[19] * e[34] + e[16] * e[29] * e[26] + + e[16] * e[20] * e[35] - e[19] * e[30] * e[12] - e[19] * e[32] * e[14] - + e[19] * e[33] * e[15] - e[19] * e[35] * e[17] - e[28] * e[23] * e[14] - + e[28] * e[24] * e[15] - e[28] * e[26] * e[17] - e[28] * e[21] * e[12] + + e[25] * e[27] * e[15] + e[25] * e[9] * e[33] + e[25] * e[29] * e[17] + + e[25] * e[11] * e[35] + e[34] * e[18] * e[15] + e[34] * e[9] * e[24] + + e[34] * e[20] * e[17] + e[34] * e[11] * e[26] + e[13] * e[27] * e[21] + + e[13] * e[18] * e[30] + e[13] * e[28] * e[22] + e[13] * e[19] * e[31] + + e[13] * e[29] * e[23] + e[13] * e[20] * e[32] + e[10] * e[27] * e[18] + + 3. * e[10] * e[28] * e[19]; + a[98] = e[4] * e[30] * e[15] + e[4] * e[12] * e[33] + e[4] * e[32] * e[17] + + e[4] * e[14] * e[35] + e[4] * e[31] * e[16] + e[4] * e[13] * e[34] + + e[7] * e[33] * e[15] + e[7] * e[35] * e[17] + + 3. * e[7] * e[34] * e[16] + e[1] * e[27] * e[15] + + e[1] * e[9] * e[33] + e[1] * e[29] * e[17] + e[1] * e[11] * e[35] + + e[1] * e[28] * e[16] + e[1] * e[10] * e[34] - e[16] * e[27] * e[0] - + e[16] * e[32] * e[5] + e[16] * e[33] * e[6] - e[16] * e[30] * e[3] + + e[16] * e[35] * e[8] - e[16] * e[29] * e[2] + e[13] * e[30] * e[6] + + e[13] * e[3] * e[33] + e[13] * e[31] * e[7] + e[13] * e[32] * e[8] + + e[13] * e[5] * e[35] - e[34] * e[11] * e[2] + e[34] * e[15] * e[6] - + e[34] * e[9] * e[0] - e[34] * e[14] * e[5] - e[34] * e[12] * e[3] + + e[34] * e[17] * e[8] + e[31] * e[12] * e[6] + e[31] * e[3] * e[15] + + e[31] * e[14] * e[8] + e[31] * e[5] * e[17] - e[7] * e[27] * e[9] - + e[7] * e[30] * e[12] + e[7] * e[28] * e[10] - e[7] * e[32] * e[14] + + e[10] * e[27] * e[6] + e[10] * e[0] * e[33] + e[10] * e[29] * e[8] + + e[10] * e[2] * e[35] + e[28] * e[9] * e[6] + e[28] * e[0] * e[15] + + e[28] * e[11] * e[8] + e[28] * e[2] * e[17] - e[7] * e[29] * e[11]; + a[132] = + e[22] * e[18] * e[12] + e[22] * e[9] * e[21] + e[22] * e[20] * e[14] + + e[22] * e[11] * e[23] + e[22] * e[19] * e[13] + e[25] * e[18] * e[15] + + e[25] * e[9] * e[24] + e[25] * e[20] * e[17] + e[25] * e[11] * e[26] + + e[25] * e[19] * e[16] + e[16] * e[18] * e[24] + e[16] * e[20] * e[26] + + e[13] * e[18] * e[21] + e[13] * e[20] * e[23] + e[19] * e[18] * e[9] + + e[19] * e[20] * e[11] - e[19] * e[23] * e[14] - e[19] * e[24] * e[15] - + e[19] * e[26] * e[17] - e[19] * e[21] * e[12] + 0.5 * e[10] * e2[22] + + 0.5 * e[10] * e2[25] + 1.5 * e[10] * e2[19] + 0.5 * e[10] * e2[18] + + 0.5 * e[10] * e2[20] - 0.5 * e[10] * e2[26] - 0.5 * e[10] * e2[23] - + 0.5 * e[10] * e2[24] - 0.5 * e[10] * e2[21]; + a[168] = e[19] * e[20] * e[26] - 0.5 * e[25] * e2[20] + + e[22] * e[21] * e[24] + e[19] * e[18] * e[24] + + 0.5 * e2[22] * e[25] - 0.5 * e[25] * e2[21] - 0.5 * e[25] * e2[23] + + 0.5 * e2[19] * e[25] - 0.5 * e[25] * e2[18] + 0.5 * e[25] * e2[24] + + 0.5 * e[25] * e2[26] + 0.5 * e3[25] + e[22] * e[23] * e[26]; + a[113] = -e[20] * e[33] * e[6] - e[20] * e[30] * e[3] - e[20] * e[31] * e[4] - + e[29] * e[21] * e[3] - e[29] * e[22] * e[4] - e[29] * e[25] * e[7] - + e[29] * e[24] * e[6] + e[8] * e[27] * e[24] + e[8] * e[18] * e[33] + + e[8] * e[28] * e[25] + e[8] * e[19] * e[34] + e[23] * e[27] * e[3] + + e[23] * e[0] * e[30] + e[23] * e[28] * e[4] + e[23] * e[1] * e[31] + + e[32] * e[18] * e[3] + e[32] * e[0] * e[21] + e[32] * e[19] * e[4] + + e[32] * e[1] * e[22] + e[26] * e[27] * e[6] + e[26] * e[0] * e[33] + + e[26] * e[28] * e[7] + e[26] * e[1] * e[34] + e[26] * e[29] * e[8] + + e[26] * e[2] * e[35] + e[35] * e[18] * e[6] + e[35] * e[0] * e[24] + + e[35] * e[19] * e[7] + e[35] * e[1] * e[25] + e[35] * e[20] * e[8] + + e[2] * e[27] * e[18] + e[2] * e[28] * e[19] + + 3. * e[2] * e[29] * e[20] + e[20] * e[27] * e[0] + + e[20] * e[28] * e[1] + e[29] * e[18] * e[0] + e[29] * e[19] * e[1] + + e[5] * e[27] * e[21] + e[5] * e[18] * e[30] + e[5] * e[28] * e[22] + + e[5] * e[19] * e[31] + e[5] * e[29] * e[23] + e[5] * e[20] * e[32] - + e[2] * e[33] * e[24] - e[2] * e[30] * e[21] - e[2] * e[31] * e[22] + + e[2] * e[32] * e[23] - e[2] * e[34] * e[25] - e[20] * e[34] * e[7]; + a[43] = e[5] * e[18] * e[3] + e[5] * e[0] * e[21] + e[5] * e[19] * e[4] + + e[5] * e[1] * e[22] + e[5] * e[2] * e[23] + e[23] * e[1] * e[4] + + e[23] * e[0] * e[3] + e[8] * e[18] * e[6] + e[8] * e[0] * e[24] + + e[8] * e[19] * e[7] + e[8] * e[1] * e[25] + e[8] * e[2] * e[26] + + e[26] * e[1] * e[7] + e[26] * e[0] * e[6] + e[2] * e[18] * e[0] + + e[2] * e[19] * e[1] - e[2] * e[21] * e[3] - e[2] * e[22] * e[4] - + e[2] * e[25] * e[7] - e[2] * e[24] * e[6] - 0.5 * e[20] * e2[4] + + 0.5 * e[20] * e2[0] - 0.5 * e[20] * e2[6] + 0.5 * e[20] * e2[5] + + 0.5 * e[20] * e2[1] - 0.5 * e[20] * e2[7] - 0.5 * e[20] * e2[3] + + 1.5 * e[20] * e2[2] + 0.5 * e[20] * e2[8]; + a[33] = e[14] * e[9] * e[3] + e[14] * e[0] * e[12] + e[14] * e[10] * e[4] + + e[14] * e[1] * e[13] + e[14] * e[11] * e[5] + e[17] * e[9] * e[6] + + e[17] * e[0] * e[15] + e[17] * e[10] * e[7] + e[17] * e[1] * e[16] + + e[17] * e[11] * e[8] + e[8] * e[9] * e[15] + e[8] * e[10] * e[16] + + e[5] * e[9] * e[12] + e[5] * e[10] * e[13] + e[11] * e[9] * e[0] + + e[11] * e[10] * e[1] - e[11] * e[13] * e[4] - e[11] * e[16] * e[7] - + e[11] * e[15] * e[6] - e[11] * e[12] * e[3] + 0.5 * e[2] * e2[14] + + 0.5 * e[2] * e2[17] + 1.5 * e[2] * e2[11] + 0.5 * e[2] * e2[9] + + 0.5 * e[2] * e2[10] - 0.5 * e[2] * e2[16] - 0.5 * e[2] * e2[12] - + 0.5 * e[2] * e2[15] - 0.5 * e[2] * e2[13]; + a[63] = e[14] * e[18] * e[12] + e[14] * e[9] * e[21] + e[14] * e[11] * e[23] + + e[14] * e[19] * e[13] + e[14] * e[10] * e[22] + e[23] * e[9] * e[12] + + e[23] * e[10] * e[13] + e[17] * e[18] * e[15] + e[17] * e[9] * e[24] + + e[17] * e[11] * e[26] + e[17] * e[19] * e[16] + + e[17] * e[10] * e[25] + e[26] * e[9] * e[15] + e[26] * e[10] * e[16] - + e[11] * e[24] * e[15] - e[11] * e[25] * e[16] + e[11] * e[18] * e[9] - + e[11] * e[21] * e[12] + e[11] * e[19] * e[10] - + e[11] * e[22] * e[13] + 1.5 * e[20] * e2[11] + 0.5 * e[20] * e2[9] + + 0.5 * e[20] * e2[10] + 0.5 * e[20] * e2[14] + 0.5 * e[20] * e2[17] - + 0.5 * e[20] * e2[16] - 0.5 * e[20] * e2[12] - 0.5 * e[20] * e2[15] - + 0.5 * e[20] * e2[13]; + a[143] = + e[23] * e[10] * e[31] + e[32] * e[18] * e[12] + e[32] * e[9] * e[21] + + e[32] * e[19] * e[13] + e[32] * e[10] * e[22] - e[11] * e[33] * e[24] - + e[11] * e[30] * e[21] + e[11] * e[35] * e[26] - e[11] * e[31] * e[22] - + e[11] * e[34] * e[25] + e[20] * e[27] * e[9] + e[20] * e[28] * e[10] + + e[29] * e[18] * e[9] + e[29] * e[19] * e[10] + e[17] * e[27] * e[24] + + e[17] * e[18] * e[33] + e[17] * e[28] * e[25] + e[17] * e[19] * e[34] + + e[17] * e[29] * e[26] + e[17] * e[20] * e[35] - e[20] * e[30] * e[12] - + e[20] * e[31] * e[13] - e[20] * e[33] * e[15] - e[20] * e[34] * e[16] - + e[29] * e[24] * e[15] - e[29] * e[25] * e[16] - e[29] * e[21] * e[12] - + e[29] * e[22] * e[13] + e[26] * e[27] * e[15] + e[26] * e[9] * e[33] + + e[26] * e[28] * e[16] + e[26] * e[10] * e[34] + e[35] * e[18] * e[15] + + e[35] * e[9] * e[24] + e[35] * e[19] * e[16] + e[35] * e[10] * e[25] + + e[14] * e[27] * e[21] + e[14] * e[18] * e[30] + e[14] * e[28] * e[22] + + e[14] * e[19] * e[31] + e[14] * e[29] * e[23] + e[14] * e[20] * e[32] + + e[11] * e[27] * e[18] + e[11] * e[28] * e[19] + + 3. * e[11] * e[29] * e[20] + e[23] * e[27] * e[12] + + e[23] * e[9] * e[30] + e[23] * e[11] * e[32] + e[23] * e[28] * e[13]; + a[133] = + e[23] * e[18] * e[12] + e[23] * e[9] * e[21] + e[23] * e[20] * e[14] + + e[23] * e[19] * e[13] + e[23] * e[10] * e[22] + e[26] * e[18] * e[15] + + e[26] * e[9] * e[24] + e[26] * e[20] * e[17] + e[26] * e[19] * e[16] + + e[26] * e[10] * e[25] + e[17] * e[19] * e[25] + e[17] * e[18] * e[24] + + e[14] * e[19] * e[22] + e[14] * e[18] * e[21] + e[20] * e[18] * e[9] + + e[20] * e[19] * e[10] - e[20] * e[24] * e[15] - e[20] * e[25] * e[16] - + e[20] * e[21] * e[12] - e[20] * e[22] * e[13] + 0.5 * e[11] * e2[23] + + 0.5 * e[11] * e2[26] + 0.5 * e[11] * e2[19] + 0.5 * e[11] * e2[18] + + 1.5 * e[11] * e2[20] - 0.5 * e[11] * e2[22] - 0.5 * e[11] * e2[24] - + 0.5 * e[11] * e2[21] - 0.5 * e[11] * e2[25]; + a[103] = -e[20] * e[21] * e[3] + e[20] * e[26] * e[8] - e[20] * e[22] * e[4] - + e[20] * e[25] * e[7] - e[20] * e[24] * e[6] + e[5] * e[19] * e[22] + + e[5] * e[18] * e[21] + e[26] * e[18] * e[6] + e[26] * e[0] * e[24] + + e[26] * e[19] * e[7] + e[26] * e[1] * e[25] + e[8] * e[19] * e[25] + + e[8] * e[18] * e[24] + e[20] * e[18] * e[0] + e[20] * e[19] * e[1] + + e[23] * e[18] * e[3] + e[23] * e[0] * e[21] + e[23] * e[19] * e[4] + + e[23] * e[1] * e[22] + e[23] * e[20] * e[5] + 1.5 * e2[20] * e[2] + + 0.5 * e[2] * e2[23] + 0.5 * e[2] * e2[19] + 0.5 * e[2] * e2[18] + + 0.5 * e[2] * e2[26] - 0.5 * e[2] * e2[22] - 0.5 * e[2] * e2[24] - + 0.5 * e[2] * e2[21] - 0.5 * e[2] * e2[25]; + a[23] = -e[2] * e[15] * e[6] + e[2] * e[9] * e[0] - e[2] * e[12] * e[3] + + e[5] * e[9] * e[3] + e[5] * e[0] * e[12] + e[5] * e[10] * e[4] + + e[5] * e[1] * e[13] + e[5] * e[2] * e[14] + e[14] * e[1] * e[4] + + e[14] * e[0] * e[3] + e[8] * e[9] * e[6] + e[8] * e[0] * e[15] + + e[8] * e[10] * e[7] + e[8] * e[1] * e[16] + e[8] * e[2] * e[17] + + e[17] * e[1] * e[7] + e[17] * e[0] * e[6] + e[2] * e[10] * e[1] - + e[2] * e[13] * e[4] - e[2] * e[16] * e[7] + 0.5 * e[11] * e2[1] + + 0.5 * e[11] * e2[0] + 1.5 * e[11] * e2[2] + 0.5 * e[11] * e2[5] + + 0.5 * e[11] * e2[8] - 0.5 * e[11] * e2[4] - 0.5 * e[11] * e2[6] - + 0.5 * e[11] * e2[7] - 0.5 * e[11] * e2[3]; + a[83] = e[5] * e[19] * e[13] + e[5] * e[10] * e[22] + e[8] * e[18] * e[15] + + e[8] * e[9] * e[24] + e[8] * e[20] * e[17] + e[8] * e[11] * e[26] + + e[8] * e[19] * e[16] + e[8] * e[10] * e[25] + e[2] * e[18] * e[9] + + e[2] * e[19] * e[10] - e[11] * e[21] * e[3] - e[11] * e[22] * e[4] - + e[11] * e[25] * e[7] - e[11] * e[24] * e[6] + e[14] * e[18] * e[3] + + e[14] * e[0] * e[21] + e[14] * e[19] * e[4] + e[14] * e[1] * e[22] + + e[14] * e[2] * e[23] - e[20] * e[13] * e[4] - e[20] * e[16] * e[7] - + e[20] * e[15] * e[6] - e[20] * e[12] * e[3] + e[23] * e[9] * e[3] + + e[23] * e[0] * e[12] + e[23] * e[10] * e[4] + e[23] * e[1] * e[13] + + e[17] * e[18] * e[6] + e[17] * e[0] * e[24] + e[17] * e[19] * e[7] + + e[17] * e[1] * e[25] + e[17] * e[2] * e[26] - e[2] * e[24] * e[15] - + e[2] * e[25] * e[16] - e[2] * e[21] * e[12] - e[2] * e[22] * e[13] + + e[26] * e[9] * e[6] + e[26] * e[0] * e[15] + e[26] * e[10] * e[7] + + e[26] * e[1] * e[16] + e[11] * e[18] * e[0] + e[11] * e[19] * e[1] + + 3. * e[11] * e[20] * e[2] + e[20] * e[9] * e[0] + + e[20] * e[10] * e[1] + e[5] * e[18] * e[12] + e[5] * e[9] * e[21] + + e[5] * e[20] * e[14] + e[5] * e[11] * e[23]; + a[53] = e[32] * e[1] * e[4] + e[32] * e[0] * e[3] + e[8] * e[27] * e[6] + + e[8] * e[0] * e[33] + e[8] * e[28] * e[7] + e[8] * e[1] * e[34] + + e[35] * e[1] * e[7] + e[35] * e[0] * e[6] + e[2] * e[27] * e[0] + + e[2] * e[28] * e[1] - e[2] * e[34] * e[7] + e[2] * e[32] * e[5] - + e[2] * e[33] * e[6] - e[2] * e[30] * e[3] + e[2] * e[35] * e[8] - + e[2] * e[31] * e[4] + e[5] * e[27] * e[3] + e[5] * e[0] * e[30] + + e[5] * e[28] * e[4] + e[5] * e[1] * e[31] + 1.5 * e[29] * e2[2] - + 0.5 * e[29] * e2[4] + 0.5 * e[29] * e2[0] - 0.5 * e[29] * e2[6] + + 0.5 * e[29] * e2[5] + 0.5 * e[29] * e2[1] - 0.5 * e[29] * e2[7] - + 0.5 * e[29] * e2[3] + 0.5 * e[29] * e2[8]; + a[3] = e[5] * e[0] * e[3] + e[8] * e[1] * e[7] + e[8] * e[0] * e[6] + + e[5] * e[1] * e[4] - 0.5 * e[2] * e2[4] + 0.5 * e3[2] + + 0.5 * e[2] * e2[1] - 0.5 * e[2] * e2[3] + 0.5 * e[2] * e2[0] + + 0.5 * e[2] * e2[8] + 0.5 * e[2] * e2[5] - 0.5 * e[2] * e2[6] - + 0.5 * e[2] * e2[7]; + a[73] = e[35] * e[9] * e[15] + e[35] * e[10] * e[16] - e[11] * e[30] * e[12] - + e[11] * e[31] * e[13] - e[11] * e[33] * e[15] - + e[11] * e[34] * e[16] + e[11] * e[27] * e[9] + e[11] * e[28] * e[10] + + e[14] * e[27] * e[12] + e[14] * e[9] * e[30] + e[14] * e[11] * e[32] + + e[14] * e[28] * e[13] + e[14] * e[10] * e[31] + e[32] * e[9] * e[12] + + e[32] * e[10] * e[13] + e[17] * e[27] * e[15] + e[17] * e[9] * e[33] + + e[17] * e[11] * e[35] + e[17] * e[28] * e[16] + + e[17] * e[10] * e[34] + 1.5 * e[29] * e2[11] - 0.5 * e[29] * e2[16] + + 0.5 * e[29] * e2[9] - 0.5 * e[29] * e2[12] - 0.5 * e[29] * e2[15] + + 0.5 * e[29] * e2[17] + 0.5 * e[29] * e2[10] + 0.5 * e[29] * e2[14] - + 0.5 * e[29] * e2[13]; + a[13] = e[14] * e[9] * e[12] + e[17] * e[10] * e[16] + e[17] * e[9] * e[15] + + 0.5 * e3[11] + e[14] * e[10] * e[13] + 0.5 * e[11] * e2[10] - + 0.5 * e[11] * e2[15] + 0.5 * e[11] * e2[14] - 0.5 * e[11] * e2[13] - + 0.5 * e[11] * e2[12] + 0.5 * e[11] * e2[9] - 0.5 * e[11] * e2[16] + + 0.5 * e[11] * e2[17]; + a[173] = + e[20] * e[27] * e[18] + e[20] * e[28] * e[19] + e[23] * e[27] * e[21] + + e[23] * e[18] * e[30] + e[23] * e[28] * e[22] + e[23] * e[19] * e[31] + + e[23] * e[20] * e[32] + e[32] * e[19] * e[22] + e[32] * e[18] * e[21] + + e[26] * e[27] * e[24] + e[26] * e[18] * e[33] + e[26] * e[28] * e[25] + + e[26] * e[19] * e[34] + e[26] * e[20] * e[35] + e[35] * e[19] * e[25] + + e[35] * e[18] * e[24] - e[20] * e[33] * e[24] - e[20] * e[30] * e[21] - + e[20] * e[31] * e[22] - e[20] * e[34] * e[25] + 0.5 * e[29] * e2[23] + + 0.5 * e[29] * e2[26] - 0.5 * e[29] * e2[22] - 0.5 * e[29] * e2[24] - + 0.5 * e[29] * e2[21] - 0.5 * e[29] * e2[25] + 1.5 * e[29] * e2[20] + + 0.5 * e[29] * e2[19] + 0.5 * e[29] * e2[18]; + a[163] = 0.5 * e[20] * e2[26] + 0.5 * e[20] * e2[18] + 0.5 * e3[20] + + 0.5 * e[20] * e2[19] + e[26] * e[18] * e[24] + 0.5 * e[20] * e2[23] - + 0.5 * e[20] * e2[25] + e[23] * e[19] * e[22] - 0.5 * e[20] * e2[24] - + 0.5 * e[20] * e2[21] - 0.5 * e[20] * e2[22] + e[23] * e[18] * e[21] + + e[26] * e[19] * e[25]; + a[93] = e[8] * e[28] * e[16] + e[8] * e[10] * e[34] + e[2] * e[27] * e[9] + + 3. * e[2] * e[29] * e[11] + e[2] * e[28] * e[10] + + e[11] * e[27] * e[0] - e[11] * e[34] * e[7] - e[11] * e[33] * e[6] - + e[11] * e[30] * e[3] + e[11] * e[28] * e[1] - e[11] * e[31] * e[4] + + e[14] * e[27] * e[3] + e[14] * e[0] * e[30] + e[14] * e[28] * e[4] + + e[14] * e[1] * e[31] + e[14] * e[2] * e[32] + e[29] * e[10] * e[1] - + e[29] * e[13] * e[4] - e[29] * e[16] * e[7] - e[29] * e[15] * e[6] + + e[29] * e[9] * e[0] - e[29] * e[12] * e[3] + e[32] * e[9] * e[3] + + e[32] * e[0] * e[12] + e[32] * e[10] * e[4] + e[32] * e[1] * e[13] + + e[17] * e[27] * e[6] + e[17] * e[0] * e[33] + e[17] * e[28] * e[7] + + e[17] * e[1] * e[34] + e[17] * e[2] * e[35] - e[2] * e[30] * e[12] - + e[2] * e[31] * e[13] - e[2] * e[33] * e[15] - e[2] * e[34] * e[16] + + e[35] * e[9] * e[6] + e[35] * e[0] * e[15] + e[35] * e[10] * e[7] + + e[35] * e[1] * e[16] + e[5] * e[27] * e[12] + e[5] * e[9] * e[30] + + e[5] * e[29] * e[14] + e[5] * e[11] * e[32] + e[5] * e[28] * e[13] + + e[5] * e[10] * e[31] + e[8] * e[27] * e[15] + e[8] * e[9] * e[33] + + e[8] * e[29] * e[17] + e[8] * e[11] * e[35]; + a[94] = -e[12] * e[34] * e[7] + e[12] * e[32] * e[5] - e[12] * e[35] * e[8] - + e[12] * e[29] * e[2] - e[12] * e[28] * e[1] + e[12] * e[31] * e[4] - + e[30] * e[11] * e[2] - e[30] * e[10] * e[1] + e[30] * e[13] * e[4] - + e[30] * e[16] * e[7] + e[30] * e[14] * e[5] - e[30] * e[17] * e[8] + + e[15] * e[3] * e[33] + e[15] * e[31] * e[7] + e[15] * e[4] * e[34] + + e[15] * e[32] * e[8] + e[15] * e[5] * e[35] + e[3] * e[27] * e[9] - + e[3] * e[28] * e[10] - e[3] * e[34] * e[16] - e[3] * e[35] * e[17] - + e[3] * e[29] * e[11] + e[33] * e[13] * e[7] + e[33] * e[4] * e[16] + + e[33] * e[14] * e[8] + e[33] * e[5] * e[17] + e[9] * e[28] * e[4] + + e[9] * e[1] * e[31] + e[9] * e[29] * e[5] + e[9] * e[2] * e[32] + + e[27] * e[10] * e[4] + e[27] * e[1] * e[13] + e[27] * e[11] * e[5] + + e[27] * e[2] * e[14] + 3. * e[3] * e[30] * e[12] + + e[3] * e[32] * e[14] + e[3] * e[31] * e[13] + e[6] * e[30] * e[15] + + e[6] * e[12] * e[33] + e[6] * e[32] * e[17] + e[6] * e[14] * e[35] + + e[6] * e[31] * e[16] + e[6] * e[13] * e[34] + e[0] * e[27] * e[12] + + e[0] * e[9] * e[30] + e[0] * e[29] * e[14] + e[0] * e[11] * e[32] + + e[0] * e[28] * e[13] + e[0] * e[10] * e[31]; + a[164] = 0.5 * e[21] * e2[24] - 0.5 * e[21] * e2[25] + 0.5 * e[21] * e2[23] - + 0.5 * e[21] * e2[26] + 0.5 * e2[18] * e[21] + 0.5 * e[21] * e2[22] - + 0.5 * e[21] * e2[20] + e[24] * e[22] * e[25] + + e[24] * e[23] * e[26] - 0.5 * e[21] * e2[19] + + e[18] * e[19] * e[22] + e[18] * e[20] * e[23] + 0.5 * e3[21]; + a[174] = + -0.5 * e[30] * e2[26] - 0.5 * e[30] * e2[19] - 0.5 * e[30] * e2[20] - + 0.5 * e[30] * e2[25] + 0.5 * e2[18] * e[30] + 1.5 * e[30] * e2[21] + + 0.5 * e[30] * e2[22] + 0.5 * e[30] * e2[23] + 0.5 * e[30] * e2[24] + + e[18] * e[27] * e[21] + e[18] * e[28] * e[22] + e[18] * e[19] * e[31] + + e[18] * e[29] * e[23] + e[18] * e[20] * e[32] + e[27] * e[19] * e[22] + + e[27] * e[20] * e[23] + e[21] * e[31] * e[22] + e[21] * e[32] * e[23] + + e[24] * e[21] * e[33] + e[24] * e[31] * e[25] + e[24] * e[22] * e[34] + + e[24] * e[32] * e[26] + e[24] * e[23] * e[35] + e[33] * e[22] * e[25] + + e[33] * e[23] * e[26] - e[21] * e[29] * e[20] - e[21] * e[35] * e[26] - + e[21] * e[28] * e[19] - e[21] * e[34] * e[25]; + a[14] = 0.5 * e[12] * e2[15] - 0.5 * e[12] * e2[17] + e[15] * e[13] * e[16] - + 0.5 * e[12] * e2[10] + e[15] * e[14] * e[17] - 0.5 * e[12] * e2[16] - + 0.5 * e[12] * e2[11] + e[9] * e[10] * e[13] + 0.5 * e[12] * e2[13] + + 0.5 * e2[9] * e[12] + 0.5 * e3[12] + e[9] * e[11] * e[14] + + 0.5 * e[12] * e2[14]; + a[34] = e[12] * e[13] * e[4] + e[12] * e[14] * e[5] + e[15] * e[12] * e[6] + + e[15] * e[13] * e[7] + e[15] * e[4] * e[16] + e[15] * e[14] * e[8] + + e[15] * e[5] * e[17] + e[6] * e[14] * e[17] + e[6] * e[13] * e[16] + + e[0] * e[11] * e[14] + e[0] * e[9] * e[12] + e[0] * e[10] * e[13] + + e[9] * e[10] * e[4] + e[9] * e[1] * e[13] + e[9] * e[11] * e[5] + + e[9] * e[2] * e[14] - e[12] * e[11] * e[2] - e[12] * e[10] * e[1] - + e[12] * e[16] * e[7] - e[12] * e[17] * e[8] + 1.5 * e2[12] * e[3] + + 0.5 * e[3] * e2[15] - 0.5 * e[3] * e2[16] + 0.5 * e[3] * e2[9] - + 0.5 * e[3] * e2[11] - 0.5 * e[3] * e2[17] - 0.5 * e[3] * e2[10] + + 0.5 * e[3] * e2[14] + 0.5 * e[3] * e2[13]; + a[64] = + e[18] * e[11] * e[14] + e[18] * e[9] * e[12] + e[18] * e[10] * e[13] + + e[12] * e[23] * e[14] + e[12] * e[22] * e[13] + e[15] * e[12] * e[24] + + e[15] * e[23] * e[17] + e[15] * e[14] * e[26] + e[15] * e[22] * e[16] + + e[15] * e[13] * e[25] + e[24] * e[14] * e[17] + e[24] * e[13] * e[16] - + e[12] * e[25] * e[16] - e[12] * e[26] * e[17] - e[12] * e[20] * e[11] - + e[12] * e[19] * e[10] + e[9] * e[20] * e[14] + e[9] * e[11] * e[23] + + e[9] * e[19] * e[13] + e[9] * e[10] * e[22] + 0.5 * e2[9] * e[21] - + 0.5 * e[21] * e2[16] - 0.5 * e[21] * e2[11] - 0.5 * e[21] * e2[17] - + 0.5 * e[21] * e2[10] + 1.5 * e[21] * e2[12] + 0.5 * e[21] * e2[14] + + 0.5 * e[21] * e2[13] + 0.5 * e[21] * e2[15]; + a[114] = -e[21] * e[35] * e[8] - e[21] * e[29] * e[2] - e[21] * e[28] * e[1] + + e[21] * e[31] * e[4] - e[30] * e[26] * e[8] - e[30] * e[20] * e[2] - + e[30] * e[19] * e[1] + e[30] * e[22] * e[4] - e[30] * e[25] * e[7] + + e[30] * e[23] * e[5] + e[6] * e[31] * e[25] + e[6] * e[22] * e[34] + + e[6] * e[32] * e[26] + e[6] * e[23] * e[35] + e[24] * e[30] * e[6] + + e[24] * e[3] * e[33] + e[24] * e[31] * e[7] + e[24] * e[4] * e[34] + + e[24] * e[32] * e[8] + e[24] * e[5] * e[35] + e[33] * e[21] * e[6] + + e[33] * e[22] * e[7] + e[33] * e[4] * e[25] + e[33] * e[23] * e[8] + + e[33] * e[5] * e[26] + e[0] * e[27] * e[21] + e[0] * e[18] * e[30] + + e[0] * e[28] * e[22] + e[0] * e[19] * e[31] + e[0] * e[29] * e[23] + + e[0] * e[20] * e[32] + e[18] * e[27] * e[3] + e[18] * e[28] * e[4] + + e[18] * e[1] * e[31] + e[18] * e[29] * e[5] + e[18] * e[2] * e[32] + + e[27] * e[19] * e[4] + e[27] * e[1] * e[22] + e[27] * e[20] * e[5] + + e[27] * e[2] * e[23] + 3. * e[3] * e[30] * e[21] + + e[3] * e[31] * e[22] + e[3] * e[32] * e[23] - e[3] * e[29] * e[20] - + e[3] * e[35] * e[26] - e[3] * e[28] * e[19] - e[3] * e[34] * e[25] - + e[21] * e[34] * e[7] + e[21] * e[32] * e[5]; + a[44] = e[18] * e[1] * e[4] + e[18] * e[0] * e[3] + e[18] * e[2] * e[5] + + e[3] * e[22] * e[4] + e[3] * e[23] * e[5] + e[6] * e[3] * e[24] + + e[6] * e[22] * e[7] + e[6] * e[4] * e[25] + e[6] * e[23] * e[8] + + e[6] * e[5] * e[26] + e[24] * e[4] * e[7] + e[24] * e[5] * e[8] + + e[0] * e[19] * e[4] + e[0] * e[1] * e[22] + e[0] * e[20] * e[5] + + e[0] * e[2] * e[23] - e[3] * e[26] * e[8] - e[3] * e[20] * e[2] - + e[3] * e[19] * e[1] - e[3] * e[25] * e[7] + 0.5 * e[21] * e2[4] + + 0.5 * e[21] * e2[0] + 0.5 * e[21] * e2[6] + 0.5 * e[21] * e2[5] - + 0.5 * e[21] * e2[1] - 0.5 * e[21] * e2[7] + 1.5 * e[21] * e2[3] - + 0.5 * e[21] * e2[2] - 0.5 * e[21] * e2[8]; + a[184] = + 0.5 * e2[27] * e[21] + 1.5 * e[21] * e2[30] + 0.5 * e[21] * e2[32] + + 0.5 * e[21] * e2[31] + 0.5 * e[21] * e2[33] - 0.5 * e[21] * e2[28] - + 0.5 * e[21] * e2[29] - 0.5 * e[21] * e2[34] - 0.5 * e[21] * e2[35] + + e[18] * e[27] * e[30] + e[18] * e[29] * e[32] + e[18] * e[28] * e[31] + + e[27] * e[28] * e[22] + e[27] * e[19] * e[31] + e[27] * e[29] * e[23] + + e[27] * e[20] * e[32] + e[30] * e[31] * e[22] + e[30] * e[32] * e[23] + + e[24] * e[30] * e[33] + e[24] * e[32] * e[35] + e[24] * e[31] * e[34] + + e[33] * e[31] * e[25] + e[33] * e[22] * e[34] + e[33] * e[32] * e[26] + + e[33] * e[23] * e[35] - e[30] * e[29] * e[20] - e[30] * e[35] * e[26] - + e[30] * e[28] * e[19] - e[30] * e[34] * e[25]; + a[49] = -0.5 * e[26] * e2[4] - 0.5 * e[26] * e2[0] + 0.5 * e[26] * e2[6] + + 0.5 * e[26] * e2[5] - 0.5 * e[26] * e2[1] + 0.5 * e[26] * e2[7] - + 0.5 * e[26] * e2[3] + 0.5 * e[26] * e2[2] + 1.5 * e[26] * e2[8] + + e[20] * e[0] * e[6] + e[20] * e[2] * e[8] + e[5] * e[21] * e[6] + + e[5] * e[3] * e[24] + e[5] * e[22] * e[7] + e[5] * e[4] * e[25] + + e[5] * e[23] * e[8] + e[23] * e[4] * e[7] + e[23] * e[3] * e[6] + + e[8] * e[24] * e[6] + e[8] * e[25] * e[7] + e[2] * e[18] * e[6] + + e[2] * e[0] * e[24] + e[2] * e[19] * e[7] + e[2] * e[1] * e[25] - + e[8] * e[21] * e[3] - e[8] * e[19] * e[1] - e[8] * e[22] * e[4] - + e[8] * e[18] * e[0] + e[20] * e[1] * e[7]; + a[154] = + e[9] * e[27] * e[30] + e[9] * e[29] * e[32] + e[9] * e[28] * e[31] + + e[33] * e[30] * e[15] + e[33] * e[32] * e[17] + e[33] * e[14] * e[35] + + e[33] * e[31] * e[16] + e[33] * e[13] * e[34] + e[27] * e[29] * e[14] + + e[27] * e[11] * e[32] + e[27] * e[28] * e[13] + e[27] * e[10] * e[31] - + e[30] * e[28] * e[10] + e[30] * e[31] * e[13] + e[30] * e[32] * e[14] - + e[30] * e[34] * e[16] - e[30] * e[35] * e[17] - e[30] * e[29] * e[11] + + e[15] * e[32] * e[35] + e[15] * e[31] * e[34] - 0.5 * e[12] * e2[34] - + 0.5 * e[12] * e2[35] + 0.5 * e[12] * e2[27] + 0.5 * e[12] * e2[32] - + 0.5 * e[12] * e2[28] - 0.5 * e[12] * e2[29] + 0.5 * e[12] * e2[31] + + 0.5 * e[12] * e2[33] + 1.5 * e[12] * e2[30]; + a[119] = e[23] * e[30] * e[6] + e[23] * e[3] * e[33] + e[23] * e[31] * e[7] + + e[23] * e[4] * e[34] + e[32] * e[21] * e[6] + e[32] * e[3] * e[24] + + e[32] * e[22] * e[7] + e[32] * e[4] * e[25] + e[26] * e[33] * e[6] + + e[26] * e[34] * e[7] + 3. * e[26] * e[35] * e[8] + + e[35] * e[24] * e[6] + e[35] * e[25] * e[7] + e[2] * e[27] * e[24] + + e[2] * e[18] * e[33] + e[2] * e[28] * e[25] + e[2] * e[19] * e[34] + + e[2] * e[29] * e[26] + e[2] * e[20] * e[35] + e[20] * e[27] * e[6] + + e[20] * e[0] * e[33] + e[20] * e[28] * e[7] + e[20] * e[1] * e[34] + + e[20] * e[29] * e[8] + e[29] * e[18] * e[6] + e[29] * e[0] * e[24] + + e[29] * e[19] * e[7] + e[29] * e[1] * e[25] + e[5] * e[30] * e[24] + + e[5] * e[21] * e[33] + e[5] * e[31] * e[25] + e[5] * e[22] * e[34] + + e[5] * e[32] * e[26] + e[5] * e[23] * e[35] - e[8] * e[27] * e[18] + + e[8] * e[33] * e[24] - e[8] * e[30] * e[21] - e[8] * e[31] * e[22] + + e[8] * e[32] * e[23] - e[8] * e[28] * e[19] + e[8] * e[34] * e[25] - + e[26] * e[27] * e[0] - e[26] * e[30] * e[3] - e[26] * e[28] * e[1] - + e[26] * e[31] * e[4] - e[35] * e[21] * e[3] - e[35] * e[19] * e[1] - + e[35] * e[22] * e[4] - e[35] * e[18] * e[0]; + a[194] = e[27] * e[29] * e[32] + e[27] * e[28] * e[31] + + e[33] * e[32] * e[35] + e[33] * e[31] * e[34] + 0.5 * e3[30] - + 0.5 * e[30] * e2[28] - 0.5 * e[30] * e2[29] - 0.5 * e[30] * e2[34] + + 0.5 * e[30] * e2[33] + 0.5 * e2[27] * e[30] + 0.5 * e[30] * e2[32] + + 0.5 * e[30] * e2[31] - 0.5 * e[30] * e2[35]; + a[69] = + 0.5 * e2[14] * e[26] + 1.5 * e[26] * e2[17] + 0.5 * e[26] * e2[15] + + 0.5 * e[26] * e2[16] + 0.5 * e2[11] * e[26] - 0.5 * e[26] * e2[9] - + 0.5 * e[26] * e2[12] - 0.5 * e[26] * e2[10] - 0.5 * e[26] * e2[13] + + e[20] * e[11] * e[17] + e[20] * e[9] * e[15] + e[20] * e[10] * e[16] + + e[14] * e[21] * e[15] + e[14] * e[12] * e[24] + e[14] * e[23] * e[17] + + e[14] * e[22] * e[16] + e[14] * e[13] * e[25] + e[23] * e[12] * e[15] + + e[23] * e[13] * e[16] + e[17] * e[24] * e[15] + e[17] * e[25] * e[16] - + e[17] * e[18] * e[9] - e[17] * e[21] * e[12] - e[17] * e[19] * e[10] - + e[17] * e[22] * e[13] + e[11] * e[18] * e[15] + e[11] * e[9] * e[24] + + e[11] * e[19] * e[16] + e[11] * e[10] * e[25]; + a[124] = e[0] * e[27] * e[30] + e[0] * e[29] * e[32] + e[0] * e[28] * e[31] + + e[30] * e[31] * e[4] + e[30] * e[32] * e[5] + e[6] * e[30] * e[33] + + e[6] * e[32] * e[35] + e[6] * e[31] * e[34] + e[27] * e[28] * e[4] + + e[27] * e[1] * e[31] + e[27] * e[29] * e[5] + e[27] * e[2] * e[32] + + e[33] * e[31] * e[7] + e[33] * e[4] * e[34] + e[33] * e[32] * e[8] + + e[33] * e[5] * e[35] - e[30] * e[34] * e[7] - e[30] * e[35] * e[8] - + e[30] * e[29] * e[2] - e[30] * e[28] * e[1] + 1.5 * e[3] * e2[30] + + 0.5 * e[3] * e2[32] + 0.5 * e[3] * e2[31] + 0.5 * e[3] * e2[27] - + 0.5 * e[3] * e2[28] - 0.5 * e[3] * e2[29] + 0.5 * e[3] * e2[33] - + 0.5 * e[3] * e2[34] - 0.5 * e[3] * e2[35]; + a[39] = 0.5 * e2[14] * e[8] + 1.5 * e2[17] * e[8] + 0.5 * e[8] * e2[15] + + 0.5 * e[8] * e2[16] - 0.5 * e[8] * e2[9] + 0.5 * e[8] * e2[11] - + 0.5 * e[8] * e2[12] - 0.5 * e[8] * e2[10] - 0.5 * e[8] * e2[13] + + e[14] * e[12] * e[6] + e[14] * e[3] * e[15] + e[14] * e[13] * e[7] + + e[14] * e[4] * e[16] + e[14] * e[5] * e[17] + e[17] * e[15] * e[6] + + e[17] * e[16] * e[7] + e[2] * e[11] * e[17] + e[2] * e[9] * e[15] + + e[2] * e[10] * e[16] + e[5] * e[12] * e[15] + e[5] * e[13] * e[16] + + e[11] * e[9] * e[6] + e[11] * e[0] * e[15] + e[11] * e[10] * e[7] + + e[11] * e[1] * e[16] - e[17] * e[10] * e[1] - e[17] * e[13] * e[4] - + e[17] * e[9] * e[0] - e[17] * e[12] * e[3]; + a[4] = -0.5 * e[3] * e2[1] - 0.5 * e[3] * e2[7] + 0.5 * e3[3] - + 0.5 * e[3] * e2[8] + e[0] * e[2] * e[5] + 0.5 * e[3] * e2[6] + + 0.5 * e[3] * e2[4] - 0.5 * e[3] * e2[2] + e[0] * e[1] * e[4] + + e[6] * e[4] * e[7] + 0.5 * e2[0] * e[3] + 0.5 * e[3] * e2[5] + + e[6] * e[5] * e[8]; + a[139] = + 0.5 * e2[23] * e[17] + 1.5 * e2[26] * e[17] + 0.5 * e[17] * e2[25] + + 0.5 * e[17] * e2[24] - 0.5 * e[17] * e2[18] - 0.5 * e[17] * e2[19] + + 0.5 * e[17] * e2[20] - 0.5 * e[17] * e2[22] - 0.5 * e[17] * e2[21] + + e[23] * e[21] * e[15] + e[23] * e[12] * e[24] + e[23] * e[14] * e[26] + + e[23] * e[22] * e[16] + e[23] * e[13] * e[25] + e[26] * e[24] * e[15] + + e[26] * e[25] * e[16] + e[11] * e[19] * e[25] + e[11] * e[18] * e[24] + + e[11] * e[20] * e[26] + e[14] * e[22] * e[25] + e[14] * e[21] * e[24] + + e[20] * e[18] * e[15] + e[20] * e[9] * e[24] + e[20] * e[19] * e[16] + + e[20] * e[10] * e[25] - e[26] * e[18] * e[9] - e[26] * e[21] * e[12] - + e[26] * e[19] * e[10] - e[26] * e[22] * e[13]; + a[74] = + -e[12] * e[34] * e[16] - e[12] * e[35] * e[17] - e[12] * e[29] * e[11] + + e[9] * e[27] * e[12] + e[9] * e[29] * e[14] + e[9] * e[11] * e[32] + + e[9] * e[28] * e[13] + e[9] * e[10] * e[31] + e[27] * e[11] * e[14] + + e[27] * e[10] * e[13] + e[12] * e[32] * e[14] + e[12] * e[31] * e[13] + + e[15] * e[12] * e[33] + e[15] * e[32] * e[17] + e[15] * e[14] * e[35] + + e[15] * e[31] * e[16] + e[15] * e[13] * e[34] + e[33] * e[14] * e[17] + + e[33] * e[13] * e[16] - e[12] * e[28] * e[10] + 0.5 * e2[9] * e[30] - + 0.5 * e[30] * e2[16] - 0.5 * e[30] * e2[11] + 1.5 * e[30] * e2[12] + + 0.5 * e[30] * e2[15] - 0.5 * e[30] * e2[17] - 0.5 * e[30] * e2[10] + + 0.5 * e[30] * e2[14] + 0.5 * e[30] * e2[13]; + a[149] = + e[32] * e[22] * e[16] + e[32] * e[13] * e[25] - e[17] * e[27] * e[18] + + e[17] * e[33] * e[24] - e[17] * e[30] * e[21] + e[17] * e[29] * e[20] + + 3. * e[17] * e[35] * e[26] - e[17] * e[31] * e[22] - + e[17] * e[28] * e[19] + e[17] * e[34] * e[25] + e[20] * e[27] * e[15] + + e[20] * e[9] * e[33] + e[20] * e[28] * e[16] + e[20] * e[10] * e[34] + + e[29] * e[18] * e[15] + e[29] * e[9] * e[24] + e[29] * e[19] * e[16] + + e[29] * e[10] * e[25] - e[26] * e[27] * e[9] - e[26] * e[30] * e[12] - + e[26] * e[28] * e[10] - e[26] * e[31] * e[13] + e[26] * e[33] * e[15] + + e[26] * e[34] * e[16] + e[35] * e[24] * e[15] + e[35] * e[25] * e[16] - + e[35] * e[18] * e[9] - e[35] * e[21] * e[12] - e[35] * e[19] * e[10] - + e[35] * e[22] * e[13] + e[14] * e[30] * e[24] + e[14] * e[21] * e[33] + + e[14] * e[31] * e[25] + e[14] * e[22] * e[34] + e[14] * e[32] * e[26] + + e[14] * e[23] * e[35] + e[11] * e[27] * e[24] + e[11] * e[18] * e[33] + + e[11] * e[28] * e[25] + e[11] * e[19] * e[34] + e[11] * e[29] * e[26] + + e[11] * e[20] * e[35] + e[23] * e[30] * e[15] + e[23] * e[12] * e[33] + + e[23] * e[32] * e[17] + e[23] * e[31] * e[16] + e[23] * e[13] * e[34] + + e[32] * e[21] * e[15] + e[32] * e[12] * e[24]; + a[84] = e[6] * e[23] * e[17] + e[6] * e[14] * e[26] + e[6] * e[22] * e[16] + + e[6] * e[13] * e[25] + e[0] * e[20] * e[14] + e[0] * e[11] * e[23] + + e[0] * e[19] * e[13] + e[0] * e[10] * e[22] - e[12] * e[26] * e[8] - + e[12] * e[20] * e[2] - e[12] * e[19] * e[1] + e[12] * e[22] * e[4] - + e[12] * e[25] * e[7] + e[12] * e[23] * e[5] - e[21] * e[11] * e[2] - + e[21] * e[10] * e[1] + e[21] * e[13] * e[4] - e[21] * e[16] * e[7] + + e[21] * e[14] * e[5] - e[21] * e[17] * e[8] + e[15] * e[3] * e[24] + + e[15] * e[22] * e[7] + e[15] * e[4] * e[25] + e[15] * e[23] * e[8] + + e[15] * e[5] * e[26] - e[3] * e[25] * e[16] - e[3] * e[26] * e[17] - + e[3] * e[20] * e[11] - e[3] * e[19] * e[10] + e[24] * e[13] * e[7] + + e[24] * e[4] * e[16] + e[24] * e[14] * e[8] + e[24] * e[5] * e[17] + + e[9] * e[18] * e[3] + e[9] * e[0] * e[21] + e[9] * e[19] * e[4] + + e[9] * e[1] * e[22] + e[9] * e[20] * e[5] + e[9] * e[2] * e[23] + + e[18] * e[0] * e[12] + e[18] * e[10] * e[4] + e[18] * e[1] * e[13] + + e[18] * e[11] * e[5] + e[18] * e[2] * e[14] + + 3. * e[3] * e[21] * e[12] + e[3] * e[23] * e[14] + + e[3] * e[22] * e[13] + e[6] * e[21] * e[15] + e[6] * e[12] * e[24]; + a[29] = 0.5 * e2[5] * e[17] + 1.5 * e[17] * e2[8] + 0.5 * e[17] * e2[7] + + 0.5 * e[17] * e2[6] + 0.5 * e2[2] * e[17] - 0.5 * e[17] * e2[4] - + 0.5 * e[17] * e2[0] - 0.5 * e[17] * e2[1] - 0.5 * e[17] * e2[3] + + e[11] * e[1] * e[7] + e[11] * e[0] * e[6] + e[11] * e[2] * e[8] + + e[5] * e[12] * e[6] + e[5] * e[3] * e[15] + e[5] * e[13] * e[7] + + e[5] * e[4] * e[16] + e[5] * e[14] * e[8] + e[14] * e[4] * e[7] + + e[14] * e[3] * e[6] + e[8] * e[15] * e[6] + e[8] * e[16] * e[7] - + e[8] * e[10] * e[1] - e[8] * e[13] * e[4] - e[8] * e[9] * e[0] - + e[8] * e[12] * e[3] + e[2] * e[9] * e[6] + e[2] * e[0] * e[15] + + e[2] * e[10] * e[7] + e[2] * e[1] * e[16]; + a[54] = e[6] * e[4] * e[34] + e[6] * e[32] * e[8] + e[6] * e[5] * e[35] + + e[33] * e[4] * e[7] + e[33] * e[5] * e[8] + e[0] * e[27] * e[3] + + e[0] * e[28] * e[4] + e[0] * e[1] * e[31] + e[0] * e[29] * e[5] + + e[0] * e[2] * e[32] - e[3] * e[34] * e[7] + e[3] * e[32] * e[5] + + e[3] * e[33] * e[6] - e[3] * e[35] * e[8] - e[3] * e[29] * e[2] - + e[3] * e[28] * e[1] + e[3] * e[31] * e[4] + e[27] * e[1] * e[4] + + e[27] * e[2] * e[5] + e[6] * e[31] * e[7] + 0.5 * e[30] * e2[4] + + 0.5 * e[30] * e2[6] + 0.5 * e[30] * e2[5] - 0.5 * e[30] * e2[1] - + 0.5 * e[30] * e2[7] - 0.5 * e[30] * e2[2] - 0.5 * e[30] * e2[8] + + 0.5 * e2[0] * e[30] + 1.5 * e[30] * e2[3]; + a[109] = 0.5 * e2[23] * e[8] + 1.5 * e2[26] * e[8] - 0.5 * e[8] * e2[18] - + 0.5 * e[8] * e2[19] - 0.5 * e[8] * e2[22] + 0.5 * e[8] * e2[24] - + 0.5 * e[8] * e2[21] + 0.5 * e[8] * e2[25] + 0.5 * e2[20] * e[8] + + e[20] * e[18] * e[6] + e[20] * e[0] * e[24] + e[20] * e[19] * e[7] + + e[20] * e[1] * e[25] + e[20] * e[2] * e[26] + e[23] * e[21] * e[6] + + e[23] * e[3] * e[24] + e[23] * e[22] * e[7] + e[23] * e[4] * e[25] + + e[23] * e[5] * e[26] - e[26] * e[21] * e[3] - e[26] * e[19] * e[1] - + e[26] * e[22] * e[4] - e[26] * e[18] * e[0] + e[26] * e[25] * e[7] + + e[26] * e[24] * e[6] + e[2] * e[19] * e[25] + e[2] * e[18] * e[24] + + e[5] * e[22] * e[25] + e[5] * e[21] * e[24]; + a[175] = + e[19] * e[27] * e[21] + e[19] * e[18] * e[30] + e[19] * e[28] * e[22] + + e[19] * e[29] * e[23] + e[19] * e[20] * e[32] + e[28] * e[18] * e[21] + + e[28] * e[20] * e[23] + e[22] * e[30] * e[21] + e[22] * e[32] * e[23] + + e[25] * e[30] * e[24] + e[25] * e[21] * e[33] + e[25] * e[22] * e[34] + + e[25] * e[32] * e[26] + e[25] * e[23] * e[35] + e[34] * e[21] * e[24] + + e[34] * e[23] * e[26] - e[22] * e[27] * e[18] - e[22] * e[33] * e[24] - + e[22] * e[29] * e[20] - e[22] * e[35] * e[26] + 0.5 * e2[19] * e[31] + + 1.5 * e[31] * e2[22] + 0.5 * e[31] * e2[21] + 0.5 * e[31] * e2[23] + + 0.5 * e[31] * e2[25] - 0.5 * e[31] * e2[26] - 0.5 * e[31] * e2[18] - + 0.5 * e[31] * e2[20] - 0.5 * e[31] * e2[24]; + a[15] = -0.5 * e[13] * e2[15] + 0.5 * e[13] * e2[16] + 0.5 * e[13] * e2[12] + + e[16] * e[12] * e[15] + 0.5 * e3[13] + e[10] * e[11] * e[14] + + 0.5 * e[13] * e2[14] - 0.5 * e[13] * e2[17] - 0.5 * e[13] * e2[11] - + 0.5 * e[13] * e2[9] + 0.5 * e2[10] * e[13] + e[10] * e[9] * e[12] + + e[16] * e[14] * e[17]; + a[95] = -e[13] * e[29] * e[2] - e[31] * e[11] * e[2] - e[31] * e[15] * e[6] - + e[31] * e[9] * e[0] + e[31] * e[14] * e[5] + e[31] * e[12] * e[3] - + e[31] * e[17] * e[8] + e[16] * e[30] * e[6] + e[16] * e[3] * e[33] + + e[16] * e[4] * e[34] + e[16] * e[32] * e[8] + e[16] * e[5] * e[35] - + e[4] * e[27] * e[9] + e[4] * e[28] * e[10] - e[4] * e[33] * e[15] - + e[4] * e[35] * e[17] - e[4] * e[29] * e[11] + e[34] * e[12] * e[6] + + e[34] * e[3] * e[15] + e[34] * e[14] * e[8] + e[34] * e[5] * e[17] + + e[10] * e[27] * e[3] + e[10] * e[0] * e[30] + e[10] * e[29] * e[5] + + e[10] * e[2] * e[32] + e[28] * e[9] * e[3] + e[28] * e[0] * e[12] + + e[28] * e[11] * e[5] + e[28] * e[2] * e[14] + e[4] * e[30] * e[12] + + e[4] * e[32] * e[14] + 3. * e[4] * e[31] * e[13] + + e[7] * e[30] * e[15] + e[7] * e[12] * e[33] + e[7] * e[32] * e[17] + + e[7] * e[14] * e[35] + e[7] * e[31] * e[16] + e[7] * e[13] * e[34] + + e[1] * e[27] * e[12] + e[1] * e[9] * e[30] + e[1] * e[29] * e[14] + + e[1] * e[11] * e[32] + e[1] * e[28] * e[13] + e[1] * e[10] * e[31] - + e[13] * e[27] * e[0] + e[13] * e[32] * e[5] - e[13] * e[33] * e[6] + + e[13] * e[30] * e[3] - e[13] * e[35] * e[8]; + a[165] = e[25] * e[23] * e[26] + e[19] * e[20] * e[23] + + e[19] * e[18] * e[21] + e[25] * e[21] * e[24] + 0.5 * e3[22] + + 0.5 * e[22] * e2[23] + 0.5 * e2[19] * e[22] - 0.5 * e[22] * e2[18] - + 0.5 * e[22] * e2[24] + 0.5 * e[22] * e2[21] + 0.5 * e[22] * e2[25] - + 0.5 * e[22] * e2[20] - 0.5 * e[22] * e2[26]; + a[55] = e[34] * e[5] * e[8] + e[1] * e[27] * e[3] + e[1] * e[0] * e[30] + + e[1] * e[28] * e[4] + e[1] * e[29] * e[5] + e[1] * e[2] * e[32] - + e[4] * e[27] * e[0] + e[4] * e[34] * e[7] + e[4] * e[32] * e[5] - + e[4] * e[33] * e[6] + e[4] * e[30] * e[3] - e[4] * e[35] * e[8] - + e[4] * e[29] * e[2] + e[28] * e[0] * e[3] + e[28] * e[2] * e[5] + + e[7] * e[30] * e[6] + e[7] * e[3] * e[33] + e[7] * e[32] * e[8] + + e[7] * e[5] * e[35] + e[34] * e[3] * e[6] + 0.5 * e2[1] * e[31] + + 1.5 * e[31] * e2[4] - 0.5 * e[31] * e2[0] - 0.5 * e[31] * e2[6] + + 0.5 * e[31] * e2[5] + 0.5 * e[31] * e2[7] + 0.5 * e[31] * e2[3] - + 0.5 * e[31] * e2[2] - 0.5 * e[31] * e2[8]; + a[85] = e[1] * e[20] * e[14] + e[1] * e[11] * e[23] + e[13] * e[21] * e[3] - + e[13] * e[26] * e[8] - e[13] * e[20] * e[2] - e[13] * e[18] * e[0] + + e[13] * e[23] * e[5] - e[13] * e[24] * e[6] - e[22] * e[11] * e[2] - + e[22] * e[15] * e[6] - e[22] * e[9] * e[0] + e[22] * e[14] * e[5] + + e[22] * e[12] * e[3] - e[22] * e[17] * e[8] + e[16] * e[21] * e[6] + + e[16] * e[3] * e[24] + e[16] * e[4] * e[25] + e[16] * e[23] * e[8] + + e[16] * e[5] * e[26] - e[4] * e[24] * e[15] - e[4] * e[26] * e[17] - + e[4] * e[20] * e[11] - e[4] * e[18] * e[9] + e[25] * e[12] * e[6] + + e[25] * e[3] * e[15] + e[25] * e[14] * e[8] + e[25] * e[5] * e[17] + + e[10] * e[18] * e[3] + e[10] * e[0] * e[21] + e[10] * e[19] * e[4] + + e[10] * e[1] * e[22] + e[10] * e[20] * e[5] + e[10] * e[2] * e[23] + + e[19] * e[9] * e[3] + e[19] * e[0] * e[12] + e[19] * e[1] * e[13] + + e[19] * e[11] * e[5] + e[19] * e[2] * e[14] + e[4] * e[21] * e[12] + + e[4] * e[23] * e[14] + 3. * e[4] * e[22] * e[13] + + e[7] * e[21] * e[15] + e[7] * e[12] * e[24] + e[7] * e[23] * e[17] + + e[7] * e[14] * e[26] + e[7] * e[22] * e[16] + e[7] * e[13] * e[25] + + e[1] * e[18] * e[12] + e[1] * e[9] * e[21]; + a[75] = + e[10] * e[27] * e[12] + e[10] * e[9] * e[30] + e[10] * e[29] * e[14] + + e[10] * e[11] * e[32] + e[10] * e[28] * e[13] + e[28] * e[11] * e[14] + + e[28] * e[9] * e[12] + e[13] * e[30] * e[12] + e[13] * e[32] * e[14] + + e[16] * e[30] * e[15] + e[16] * e[12] * e[33] + e[16] * e[32] * e[17] + + e[16] * e[14] * e[35] + e[16] * e[13] * e[34] + e[34] * e[14] * e[17] + + e[34] * e[12] * e[15] - e[13] * e[27] * e[9] - e[13] * e[33] * e[15] - + e[13] * e[35] * e[17] - e[13] * e[29] * e[11] + 0.5 * e2[10] * e[31] + + 0.5 * e[31] * e2[16] - 0.5 * e[31] * e2[9] - 0.5 * e[31] * e2[11] + + 0.5 * e[31] * e2[12] - 0.5 * e[31] * e2[15] - 0.5 * e[31] * e2[17] + + 0.5 * e[31] * e2[14] + 1.5 * e[31] * e2[13]; + a[5] = -0.5 * e[4] * e2[6] - 0.5 * e[4] * e2[0] + e[1] * e[2] * e[5] + + 0.5 * e[4] * e2[7] + e[1] * e[0] * e[3] + e[7] * e[5] * e[8] - + 0.5 * e[4] * e2[8] + 0.5 * e[4] * e2[3] + 0.5 * e[4] * e2[5] + + e[7] * e[3] * e[6] - 0.5 * e[4] * e2[2] + 0.5 * e3[4] + + 0.5 * e2[1] * e[4]; + a[195] = e[34] * e[32] * e[35] - 0.5 * e[31] * e2[35] + 0.5 * e[31] * e2[34] + + 0.5 * e2[28] * e[31] + 0.5 * e3[31] + 0.5 * e[31] * e2[32] + + e[34] * e[30] * e[33] - 0.5 * e[31] * e2[27] + 0.5 * e[31] * e2[30] - + 0.5 * e[31] * e2[33] - 0.5 * e[31] * e2[29] + e[28] * e[29] * e[32] + + e[28] * e[27] * e[30]; + a[125] = e[1] * e[27] * e[30] + e[1] * e[29] * e[32] + e[1] * e[28] * e[31] + + e[31] * e[30] * e[3] + e[31] * e[32] * e[5] + e[7] * e[30] * e[33] + + e[7] * e[32] * e[35] + e[7] * e[31] * e[34] + e[28] * e[27] * e[3] + + e[28] * e[0] * e[30] + e[28] * e[29] * e[5] + e[28] * e[2] * e[32] + + e[34] * e[30] * e[6] + e[34] * e[3] * e[33] + e[34] * e[32] * e[8] + + e[34] * e[5] * e[35] - e[31] * e[27] * e[0] - e[31] * e[33] * e[6] - + e[31] * e[35] * e[8] - e[31] * e[29] * e[2] + 0.5 * e[4] * e2[30] + + 0.5 * e[4] * e2[32] + 1.5 * e[4] * e2[31] - 0.5 * e[4] * e2[27] + + 0.5 * e[4] * e2[28] - 0.5 * e[4] * e2[29] - 0.5 * e[4] * e2[33] + + 0.5 * e[4] * e2[34] - 0.5 * e[4] * e2[35]; + a[185] = + 0.5 * e[22] * e2[30] + 0.5 * e[22] * e2[32] + 1.5 * e[22] * e2[31] + + 0.5 * e[22] * e2[34] - 0.5 * e[22] * e2[27] - 0.5 * e[22] * e2[29] - + 0.5 * e[22] * e2[33] - 0.5 * e[22] * e2[35] + e[28] * e[18] * e[30] + + e[28] * e[29] * e[23] + e[28] * e[20] * e[32] + e[31] * e[30] * e[21] + + e[31] * e[32] * e[23] + e[25] * e[30] * e[33] + e[25] * e[32] * e[35] + + e[25] * e[31] * e[34] + e[34] * e[30] * e[24] + e[34] * e[21] * e[33] + + e[34] * e[32] * e[26] + e[34] * e[23] * e[35] - e[31] * e[27] * e[18] - + e[31] * e[33] * e[24] - e[31] * e[29] * e[20] - e[31] * e[35] * e[26] + + e[19] * e[27] * e[30] + e[19] * e[29] * e[32] + e[19] * e[28] * e[31] + + e[28] * e[27] * e[21] + 0.5 * e2[28] * e[22]; + a[155] = + e[16] * e[30] * e[33] + e[16] * e[32] * e[35] + e[10] * e[27] * e[30] + + e[10] * e[29] * e[32] + e[10] * e[28] * e[31] + e[34] * e[30] * e[15] + + e[34] * e[12] * e[33] + e[34] * e[32] * e[17] + e[34] * e[14] * e[35] + + e[34] * e[31] * e[16] + e[28] * e[27] * e[12] + e[28] * e[9] * e[30] + + e[28] * e[29] * e[14] + e[28] * e[11] * e[32] - e[31] * e[27] * e[9] + + e[31] * e[30] * e[12] + e[31] * e[32] * e[14] - e[31] * e[33] * e[15] - + e[31] * e[35] * e[17] - e[31] * e[29] * e[11] - 0.5 * e[13] * e2[27] + + 0.5 * e[13] * e2[32] + 0.5 * e[13] * e2[28] - 0.5 * e[13] * e2[29] + + 1.5 * e[13] * e2[31] - 0.5 * e[13] * e2[33] + 0.5 * e[13] * e2[30] + + 0.5 * e[13] * e2[34] - 0.5 * e[13] * e2[35]; + a[134] = + e[21] * e[23] * e[14] + e[21] * e[22] * e[13] + e[24] * e[21] * e[15] + + e[24] * e[23] * e[17] + e[24] * e[14] * e[26] + e[24] * e[22] * e[16] + + e[24] * e[13] * e[25] + e[15] * e[22] * e[25] + e[15] * e[23] * e[26] + + e[9] * e[19] * e[22] + e[9] * e[18] * e[21] + e[9] * e[20] * e[23] + + e[18] * e[20] * e[14] + e[18] * e[11] * e[23] + e[18] * e[19] * e[13] + + e[18] * e[10] * e[22] - e[21] * e[25] * e[16] - e[21] * e[26] * e[17] - + e[21] * e[20] * e[11] - e[21] * e[19] * e[10] + 1.5 * e2[21] * e[12] + + 0.5 * e[12] * e2[24] - 0.5 * e[12] * e2[26] + 0.5 * e[12] * e2[18] + + 0.5 * e[12] * e2[23] - 0.5 * e[12] * e2[19] - 0.5 * e[12] * e2[20] + + 0.5 * e[12] * e2[22] - 0.5 * e[12] * e2[25]; + a[144] = + -e[12] * e[29] * e[20] - e[12] * e[35] * e[26] - e[12] * e[28] * e[19] - + e[12] * e[34] * e[25] + e[18] * e[29] * e[14] + e[18] * e[11] * e[32] + + e[18] * e[28] * e[13] + e[18] * e[10] * e[31] + e[27] * e[20] * e[14] + + e[27] * e[11] * e[23] + e[27] * e[19] * e[13] + e[27] * e[10] * e[22] + + e[15] * e[30] * e[24] + e[15] * e[21] * e[33] + e[15] * e[31] * e[25] + + e[15] * e[22] * e[34] + e[15] * e[32] * e[26] + e[15] * e[23] * e[35] - + e[21] * e[28] * e[10] - e[21] * e[34] * e[16] - e[21] * e[35] * e[17] - + e[21] * e[29] * e[11] - e[30] * e[25] * e[16] - e[30] * e[26] * e[17] - + e[30] * e[20] * e[11] - e[30] * e[19] * e[10] + e[24] * e[32] * e[17] + + e[24] * e[14] * e[35] + e[24] * e[31] * e[16] + e[24] * e[13] * e[34] + + e[33] * e[23] * e[17] + e[33] * e[14] * e[26] + e[33] * e[22] * e[16] + + e[33] * e[13] * e[25] + 3. * e[12] * e[30] * e[21] + + e[12] * e[31] * e[22] + e[12] * e[32] * e[23] + e[9] * e[27] * e[21] + + e[9] * e[18] * e[30] + e[9] * e[28] * e[22] + e[9] * e[19] * e[31] + + e[9] * e[29] * e[23] + e[9] * e[20] * e[32] + e[21] * e[32] * e[14] + + e[21] * e[31] * e[13] + e[30] * e[23] * e[14] + e[30] * e[22] * e[13] + + e[12] * e[27] * e[18] + e[12] * e[33] * e[24]; + a[24] = e[0] * e[11] * e[5] + e[0] * e[2] * e[14] + e[9] * e[1] * e[4] + + e[9] * e[0] * e[3] + e[9] * e[2] * e[5] + e[3] * e[13] * e[4] + + e[3] * e[14] * e[5] + e[6] * e[3] * e[15] + e[6] * e[13] * e[7] + + e[6] * e[4] * e[16] + e[6] * e[14] * e[8] + e[6] * e[5] * e[17] + + e[15] * e[4] * e[7] + e[15] * e[5] * e[8] - e[3] * e[11] * e[2] - + e[3] * e[10] * e[1] - e[3] * e[16] * e[7] - e[3] * e[17] * e[8] + + e[0] * e[10] * e[4] + e[0] * e[1] * e[13] + 1.5 * e[12] * e2[3] + + 0.5 * e[12] * e2[4] + 0.5 * e[12] * e2[5] + 0.5 * e[12] * e2[6] + + 0.5 * e2[0] * e[12] - 0.5 * e[12] * e2[1] - 0.5 * e[12] * e2[7] - + 0.5 * e[12] * e2[2] - 0.5 * e[12] * e2[8]; + a[104] = e[21] * e[24] * e[6] + e[0] * e[19] * e[22] + e[0] * e[20] * e[23] + + e[24] * e[22] * e[7] + e[24] * e[4] * e[25] + e[24] * e[23] * e[8] + + e[24] * e[5] * e[26] + e[6] * e[22] * e[25] + e[6] * e[23] * e[26] + + e[18] * e[0] * e[21] + e[18] * e[19] * e[4] + e[18] * e[1] * e[22] + + e[18] * e[20] * e[5] + e[18] * e[2] * e[23] + e[21] * e[22] * e[4] + + e[21] * e[23] * e[5] - e[21] * e[26] * e[8] - e[21] * e[20] * e[2] - + e[21] * e[19] * e[1] - e[21] * e[25] * e[7] + 1.5 * e2[21] * e[3] + + 0.5 * e[3] * e2[22] + 0.5 * e[3] * e2[23] + 0.5 * e[3] * e2[24] - + 0.5 * e[3] * e2[26] - 0.5 * e[3] * e2[19] - 0.5 * e[3] * e2[20] - + 0.5 * e[3] * e2[25] + 0.5 * e2[18] * e[3]; + a[76] = + e[11] * e[27] * e[12] + e[11] * e[9] * e[30] + e[11] * e[29] * e[14] + + e[11] * e[28] * e[13] + e[11] * e[10] * e[31] + e[29] * e[9] * e[12] + + e[29] * e[10] * e[13] + e[14] * e[30] * e[12] + e[14] * e[31] * e[13] + + e[17] * e[30] * e[15] + e[17] * e[12] * e[33] + e[17] * e[14] * e[35] + + e[17] * e[31] * e[16] + e[17] * e[13] * e[34] + e[35] * e[12] * e[15] + + e[35] * e[13] * e[16] - e[14] * e[27] * e[9] - e[14] * e[28] * e[10] - + e[14] * e[33] * e[15] - e[14] * e[34] * e[16] + 0.5 * e2[11] * e[32] - + 0.5 * e[32] * e2[16] - 0.5 * e[32] * e2[9] + 0.5 * e[32] * e2[12] - + 0.5 * e[32] * e2[15] + 0.5 * e[32] * e2[17] - 0.5 * e[32] * e2[10] + + 1.5 * e[32] * e2[14] + 0.5 * e[32] * e2[13]; + a[6] = e[8] * e[3] * e[6] + 0.5 * e2[2] * e[5] - 0.5 * e[5] * e2[0] + + 0.5 * e[5] * e2[4] - 0.5 * e[5] * e2[6] + 0.5 * e[5] * e2[8] + + e[8] * e[4] * e[7] + 0.5 * e3[5] + e[2] * e[0] * e[3] + + 0.5 * e[5] * e2[3] - 0.5 * e[5] * e2[7] + e[2] * e[1] * e[4] - + 0.5 * e[5] * e2[1]; + a[56] = e[2] * e[27] * e[3] + e[2] * e[0] * e[30] + e[2] * e[28] * e[4] + + e[2] * e[1] * e[31] + e[2] * e[29] * e[5] - e[5] * e[27] * e[0] - + e[5] * e[34] * e[7] - e[5] * e[33] * e[6] + e[5] * e[30] * e[3] + + e[5] * e[35] * e[8] - e[5] * e[28] * e[1] + e[5] * e[31] * e[4] + + e[29] * e[1] * e[4] + e[29] * e[0] * e[3] + e[8] * e[30] * e[6] + + e[8] * e[3] * e[33] + e[8] * e[31] * e[7] + e[8] * e[4] * e[34] + + e[35] * e[4] * e[7] + e[35] * e[3] * e[6] + 0.5 * e2[2] * e[32] + + 1.5 * e[32] * e2[5] + 0.5 * e[32] * e2[4] - 0.5 * e[32] * e2[0] - + 0.5 * e[32] * e2[6] - 0.5 * e[32] * e2[1] - 0.5 * e[32] * e2[7] + + 0.5 * e[32] * e2[3] + 0.5 * e[32] * e2[8]; + a[86] = -e[14] * e[19] * e[1] + e[14] * e[22] * e[4] - e[14] * e[18] * e[0] - + e[14] * e[25] * e[7] - e[14] * e[24] * e[6] - e[23] * e[10] * e[1] + + e[23] * e[13] * e[4] - e[23] * e[16] * e[7] - e[23] * e[15] * e[6] - + e[23] * e[9] * e[0] + e[23] * e[12] * e[3] + e[17] * e[21] * e[6] + + e[17] * e[3] * e[24] + e[17] * e[22] * e[7] + e[17] * e[4] * e[25] + + e[17] * e[5] * e[26] - e[5] * e[24] * e[15] - e[5] * e[25] * e[16] - + e[5] * e[18] * e[9] - e[5] * e[19] * e[10] + e[26] * e[12] * e[6] + + e[26] * e[3] * e[15] + e[26] * e[13] * e[7] + e[26] * e[4] * e[16] + + e[11] * e[18] * e[3] + e[11] * e[0] * e[21] + e[11] * e[19] * e[4] + + e[11] * e[1] * e[22] + e[11] * e[20] * e[5] + e[11] * e[2] * e[23] + + e[20] * e[9] * e[3] + e[20] * e[0] * e[12] + e[20] * e[10] * e[4] + + e[20] * e[1] * e[13] + e[20] * e[2] * e[14] + e[5] * e[21] * e[12] + + 3. * e[5] * e[23] * e[14] + e[5] * e[22] * e[13] + + e[8] * e[21] * e[15] + e[8] * e[12] * e[24] + e[8] * e[23] * e[17] + + e[8] * e[14] * e[26] + e[8] * e[22] * e[16] + e[8] * e[13] * e[25] + + e[2] * e[18] * e[12] + e[2] * e[9] * e[21] + e[2] * e[19] * e[13] + + e[2] * e[10] * e[22] + e[14] * e[21] * e[3]; + a[156] = + -0.5 * e[14] * e2[27] + 1.5 * e[14] * e2[32] - 0.5 * e[14] * e2[28] + + 0.5 * e[14] * e2[29] + 0.5 * e[14] * e2[31] - 0.5 * e[14] * e2[33] + + 0.5 * e[14] * e2[30] - 0.5 * e[14] * e2[34] + 0.5 * e[14] * e2[35] + + e[11] * e[27] * e[30] + e[11] * e[29] * e[32] + e[11] * e[28] * e[31] + + e[35] * e[30] * e[15] + e[35] * e[12] * e[33] + e[35] * e[32] * e[17] + + e[35] * e[31] * e[16] + e[35] * e[13] * e[34] + e[29] * e[27] * e[12] + + e[29] * e[9] * e[30] + e[29] * e[28] * e[13] + e[29] * e[10] * e[31] - + e[32] * e[27] * e[9] + e[32] * e[30] * e[12] - e[32] * e[28] * e[10] + + e[32] * e[31] * e[13] - e[32] * e[33] * e[15] - e[32] * e[34] * e[16] + + e[17] * e[30] * e[33] + e[17] * e[31] * e[34]; + a[186] = + -0.5 * e[23] * e2[33] - 0.5 * e[23] * e2[34] + 0.5 * e2[29] * e[23] + + 0.5 * e[23] * e2[30] + 1.5 * e[23] * e2[32] + 0.5 * e[23] * e2[31] + + 0.5 * e[23] * e2[35] - 0.5 * e[23] * e2[27] - 0.5 * e[23] * e2[28] + + e[32] * e[30] * e[21] + e[32] * e[31] * e[22] + e[26] * e[30] * e[33] + + e[26] * e[32] * e[35] + e[26] * e[31] * e[34] + e[35] * e[30] * e[24] + + e[35] * e[21] * e[33] + e[35] * e[31] * e[25] + e[35] * e[22] * e[34] - + e[32] * e[27] * e[18] - e[32] * e[33] * e[24] - e[32] * e[28] * e[19] - + e[32] * e[34] * e[25] + e[20] * e[27] * e[30] + e[20] * e[29] * e[32] + + e[20] * e[28] * e[31] + e[29] * e[27] * e[21] + e[29] * e[18] * e[30] + + e[29] * e[28] * e[22] + e[29] * e[19] * e[31]; + a[126] = e[2] * e[27] * e[30] + e[2] * e[29] * e[32] + e[2] * e[28] * e[31] + + e[32] * e[30] * e[3] + e[32] * e[31] * e[4] + e[8] * e[30] * e[33] + + e[8] * e[32] * e[35] + e[8] * e[31] * e[34] + e[29] * e[27] * e[3] + + e[29] * e[0] * e[30] + e[29] * e[28] * e[4] + e[29] * e[1] * e[31] + + e[35] * e[30] * e[6] + e[35] * e[3] * e[33] + e[35] * e[31] * e[7] + + e[35] * e[4] * e[34] - e[32] * e[27] * e[0] - e[32] * e[34] * e[7] - + e[32] * e[33] * e[6] - e[32] * e[28] * e[1] + 0.5 * e[5] * e2[30] + + 1.5 * e[5] * e2[32] + 0.5 * e[5] * e2[31] - 0.5 * e[5] * e2[27] - + 0.5 * e[5] * e2[28] + 0.5 * e[5] * e2[29] - 0.5 * e[5] * e2[33] - + 0.5 * e[5] * e2[34] + 0.5 * e[5] * e2[35]; + a[196] = 0.5 * e[32] * e2[31] + 0.5 * e[32] * e2[35] - 0.5 * e[32] * e2[27] + + e[29] * e[27] * e[30] + e[29] * e[28] * e[31] + + e[35] * e[30] * e[33] + e[35] * e[31] * e[34] + + 0.5 * e2[29] * e[32] + 0.5 * e3[32] - 0.5 * e[32] * e2[33] - + 0.5 * e[32] * e2[34] + 0.5 * e[32] * e2[30] - 0.5 * e[32] * e2[28]; + a[25] = e[10] * e[1] * e[4] + e[10] * e[0] * e[3] + e[10] * e[2] * e[5] + + e[4] * e[12] * e[3] + e[4] * e[14] * e[5] + e[7] * e[12] * e[6] + + e[7] * e[3] * e[15] + e[7] * e[4] * e[16] + e[7] * e[14] * e[8] + + e[7] * e[5] * e[17] + e[16] * e[3] * e[6] + e[16] * e[5] * e[8] - + e[4] * e[11] * e[2] - e[4] * e[15] * e[6] - e[4] * e[9] * e[0] - + e[4] * e[17] * e[8] + e[1] * e[9] * e[3] + e[1] * e[0] * e[12] + + e[1] * e[11] * e[5] + e[1] * e[2] * e[14] + 1.5 * e[13] * e2[4] + + 0.5 * e[13] * e2[3] + 0.5 * e[13] * e2[5] + 0.5 * e[13] * e2[7] + + 0.5 * e2[1] * e[13] - 0.5 * e[13] * e2[0] - 0.5 * e[13] * e2[6] - + 0.5 * e[13] * e2[2] - 0.5 * e[13] * e2[8]; + a[105] = e[25] * e[21] * e[6] + e[25] * e[3] * e[24] + e[25] * e[23] * e[8] + + e[25] * e[5] * e[26] + e[7] * e[21] * e[24] + e[7] * e[23] * e[26] + + e[19] * e[18] * e[3] + e[19] * e[0] * e[21] + e[19] * e[1] * e[22] + + e[19] * e[20] * e[5] + e[19] * e[2] * e[23] + e[22] * e[21] * e[3] + + e[22] * e[23] * e[5] - e[22] * e[26] * e[8] - e[22] * e[20] * e[2] - + e[22] * e[18] * e[0] + e[22] * e[25] * e[7] - e[22] * e[24] * e[6] + + e[1] * e[18] * e[21] + e[1] * e[20] * e[23] + 0.5 * e[4] * e2[25] - + 0.5 * e[4] * e2[26] - 0.5 * e[4] * e2[18] - 0.5 * e[4] * e2[20] - + 0.5 * e[4] * e2[24] + 0.5 * e2[19] * e[4] + 1.5 * e2[22] * e[4] + + 0.5 * e[4] * e2[21] + 0.5 * e[4] * e2[23]; + a[135] = + e[22] * e[21] * e[12] + e[22] * e[23] * e[14] + e[25] * e[21] * e[15] + + e[25] * e[12] * e[24] + e[25] * e[23] * e[17] + e[25] * e[14] * e[26] + + e[25] * e[22] * e[16] + e[16] * e[21] * e[24] + e[16] * e[23] * e[26] + + e[10] * e[19] * e[22] + e[10] * e[18] * e[21] + e[10] * e[20] * e[23] + + e[19] * e[18] * e[12] + e[19] * e[9] * e[21] + e[19] * e[20] * e[14] + + e[19] * e[11] * e[23] - e[22] * e[24] * e[15] - e[22] * e[26] * e[17] - + e[22] * e[20] * e[11] - e[22] * e[18] * e[9] - 0.5 * e[13] * e2[26] - + 0.5 * e[13] * e2[18] + 0.5 * e[13] * e2[23] + 0.5 * e[13] * e2[19] - + 0.5 * e[13] * e2[20] - 0.5 * e[13] * e2[24] + 0.5 * e[13] * e2[21] + + 1.5 * e2[22] * e[13] + 0.5 * e[13] * e2[25]; + a[145] = + e[13] * e[30] * e[21] + 3. * e[13] * e[31] * e[22] + + e[13] * e[32] * e[23] + e[10] * e[27] * e[21] + e[10] * e[18] * e[30] + + e[10] * e[28] * e[22] + e[10] * e[19] * e[31] + e[10] * e[29] * e[23] + + e[10] * e[20] * e[32] + e[22] * e[30] * e[12] + e[22] * e[32] * e[14] + + e[31] * e[21] * e[12] + e[31] * e[23] * e[14] - e[13] * e[27] * e[18] - + e[13] * e[33] * e[24] - e[13] * e[29] * e[20] - e[13] * e[35] * e[26] + + e[13] * e[28] * e[19] + e[13] * e[34] * e[25] + e[19] * e[27] * e[12] + + e[19] * e[9] * e[30] + e[19] * e[29] * e[14] + e[19] * e[11] * e[32] + + e[28] * e[18] * e[12] + e[28] * e[9] * e[21] + e[28] * e[20] * e[14] + + e[28] * e[11] * e[23] + e[16] * e[30] * e[24] + e[16] * e[21] * e[33] + + e[16] * e[31] * e[25] + e[16] * e[22] * e[34] + e[16] * e[32] * e[26] + + e[16] * e[23] * e[35] - e[22] * e[27] * e[9] - e[22] * e[33] * e[15] - + e[22] * e[35] * e[17] - e[22] * e[29] * e[11] - e[31] * e[24] * e[15] - + e[31] * e[26] * e[17] - e[31] * e[20] * e[11] - e[31] * e[18] * e[9] + + e[25] * e[30] * e[15] + e[25] * e[12] * e[33] + e[25] * e[32] * e[17] + + e[25] * e[14] * e[35] + e[34] * e[21] * e[15] + e[34] * e[12] * e[24] + + e[34] * e[23] * e[17] + e[34] * e[14] * e[26]; + a[65] = + e[19] * e[11] * e[14] + e[19] * e[9] * e[12] + e[19] * e[10] * e[13] + + e[13] * e[21] * e[12] + e[13] * e[23] * e[14] + e[16] * e[21] * e[15] + + e[16] * e[12] * e[24] + e[16] * e[23] * e[17] + e[16] * e[14] * e[26] + + e[16] * e[13] * e[25] + e[25] * e[14] * e[17] + e[25] * e[12] * e[15] - + e[13] * e[24] * e[15] - e[13] * e[26] * e[17] - e[13] * e[20] * e[11] - + e[13] * e[18] * e[9] + e[10] * e[18] * e[12] + e[10] * e[9] * e[21] + + e[10] * e[20] * e[14] + e[10] * e[11] * e[23] + 1.5 * e[22] * e2[13] + + 0.5 * e[22] * e2[14] + 0.5 * e[22] * e2[12] + 0.5 * e[22] * e2[16] + + 0.5 * e2[10] * e[22] - 0.5 * e[22] * e2[9] - 0.5 * e[22] * e2[11] - + 0.5 * e[22] * e2[15] - 0.5 * e[22] * e2[17]; + a[35] = e[13] * e[12] * e[3] + e[13] * e[14] * e[5] + e[16] * e[12] * e[6] + + e[16] * e[3] * e[15] + e[16] * e[13] * e[7] + e[16] * e[14] * e[8] + + e[16] * e[5] * e[17] + e[7] * e[14] * e[17] + e[7] * e[12] * e[15] + + e[1] * e[11] * e[14] + e[1] * e[9] * e[12] + e[1] * e[10] * e[13] + + e[10] * e[9] * e[3] + e[10] * e[0] * e[12] + e[10] * e[11] * e[5] + + e[10] * e[2] * e[14] - e[13] * e[11] * e[2] - e[13] * e[15] * e[6] - + e[13] * e[9] * e[0] - e[13] * e[17] * e[8] + 1.5 * e2[13] * e[4] + + 0.5 * e[4] * e2[16] - 0.5 * e[4] * e2[9] - 0.5 * e[4] * e2[11] + + 0.5 * e[4] * e2[12] - 0.5 * e[4] * e2[15] - 0.5 * e[4] * e2[17] + + 0.5 * e[4] * e2[10] + 0.5 * e[4] * e2[14]; + a[45] = e[19] * e[1] * e[4] + e[19] * e[0] * e[3] + e[19] * e[2] * e[5] + + e[4] * e[21] * e[3] + e[4] * e[23] * e[5] + e[7] * e[21] * e[6] + + e[7] * e[3] * e[24] + e[7] * e[4] * e[25] + e[7] * e[23] * e[8] + + e[7] * e[5] * e[26] + e[25] * e[3] * e[6] + e[25] * e[5] * e[8] + + e[1] * e[18] * e[3] + e[1] * e[0] * e[21] + e[1] * e[20] * e[5] + + e[1] * e[2] * e[23] - e[4] * e[26] * e[8] - e[4] * e[20] * e[2] - + e[4] * e[18] * e[0] - e[4] * e[24] * e[6] + 1.5 * e[22] * e2[4] - + 0.5 * e[22] * e2[0] - 0.5 * e[22] * e2[6] + 0.5 * e[22] * e2[5] + + 0.5 * e[22] * e2[1] + 0.5 * e[22] * e2[7] + 0.5 * e[22] * e2[3] - + 0.5 * e[22] * e2[2] - 0.5 * e[22] * e2[8]; + a[115] = -e[31] * e[20] * e[2] - e[31] * e[18] * e[0] + e[31] * e[23] * e[5] - + e[31] * e[24] * e[6] + e[7] * e[30] * e[24] + e[7] * e[21] * e[33] + + e[7] * e[32] * e[26] + e[7] * e[23] * e[35] + e[25] * e[30] * e[6] + + e[25] * e[3] * e[33] + e[25] * e[31] * e[7] + e[25] * e[4] * e[34] + + e[25] * e[32] * e[8] + e[25] * e[5] * e[35] + e[34] * e[21] * e[6] + + e[34] * e[3] * e[24] + e[34] * e[22] * e[7] + e[34] * e[23] * e[8] + + e[34] * e[5] * e[26] + e[1] * e[27] * e[21] + e[1] * e[18] * e[30] + + e[1] * e[28] * e[22] + e[1] * e[19] * e[31] + e[1] * e[29] * e[23] + + e[1] * e[20] * e[32] + e[19] * e[27] * e[3] + e[19] * e[0] * e[30] + + e[19] * e[28] * e[4] + e[19] * e[29] * e[5] + e[19] * e[2] * e[32] + + e[28] * e[18] * e[3] + e[28] * e[0] * e[21] + e[28] * e[20] * e[5] + + e[28] * e[2] * e[23] + e[4] * e[30] * e[21] + + 3. * e[4] * e[31] * e[22] + e[4] * e[32] * e[23] - + e[4] * e[27] * e[18] - e[4] * e[33] * e[24] - e[4] * e[29] * e[20] - + e[4] * e[35] * e[26] - e[22] * e[27] * e[0] + e[22] * e[32] * e[5] - + e[22] * e[33] * e[6] + e[22] * e[30] * e[3] - e[22] * e[35] * e[8] - + e[22] * e[29] * e[2] + e[31] * e[21] * e[3] - e[31] * e[26] * e[8]; +} diff --git a/colmap/include/colmap/estimators/euclidean_transform.h b/colmap/include/colmap/estimators/euclidean_transform.h new file mode 100644 index 0000000000000000000000000000000000000000..a81b28776491b1c6a04b163b1723fbd4fbad3a51 --- /dev/null +++ b/colmap/include/colmap/estimators/euclidean_transform.h @@ -0,0 +1,56 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_ESTIMATORS_EUCLIDEAN_TRANSFORM_H_ +#define COLMAP_SRC_ESTIMATORS_EUCLIDEAN_TRANSFORM_H_ + +#include "base/similarity_transform.h" +#include "util/alignment.h" + +namespace colmap { + +// N-D Euclidean transform estimator from corresponding point pairs in the +// source and destination coordinate systems. +// +// This algorithm is based on the following paper: +// +// S. Umeyama. Least-Squares Estimation of Transformation Parameters +// Between Two Point Patterns. IEEE Transactions on Pattern Analysis and +// Machine Intelligence, Volume 13 Issue 4, Page 376-380, 1991. +// http://www.stanford.edu/class/cs273/refs/umeyama.pdf +// +// and uses the Eigen implementation. +template +using EuclideanTransformEstimator = SimilarityTransformEstimator; + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_EUCLIDEAN_TRANSFORM_H_ diff --git a/colmap/include/colmap/estimators/fundamental_matrix.h b/colmap/include/colmap/estimators/fundamental_matrix.h new file mode 100644 index 0000000000000000000000000000000000000000..395eb1c5771f28be164718f8b9442fce40ca13f6 --- /dev/null +++ b/colmap/include/colmap/estimators/fundamental_matrix.h @@ -0,0 +1,129 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_ESTIMATORS_FUNDAMENTAL_MATRIX_H_ +#define COLMAP_SRC_ESTIMATORS_FUNDAMENTAL_MATRIX_H_ + +#include + +#include + +#include "estimators/homography_matrix.h" +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Fundamental matrix estimator from corresponding point pairs. +// +// This algorithm solves the 7-Point problem and is based on the following +// paper: +// +// Zhengyou Zhang and T. Kanade, Determining the Epipolar Geometry and its +// Uncertainty: A Review, International Journal of Computer Vision, 1998. +// http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.33.4540 +class FundamentalMatrixSevenPointEstimator { + public: + typedef Eigen::Vector2d X_t; + typedef Eigen::Vector2d Y_t; + typedef Eigen::Matrix3d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 7; + + // Estimate either 1 or 3 possible fundamental matrix solutions from a set of + // corresponding points. + // + // The number of corresponding points must be exactly 7. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points + // + // @return Up to 4 solutions as a vector of 3x3 fundamental matrices. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Calculate the residuals of a set of corresponding points and a given + // fundamental matrix. + // + // Residuals are defined as the squared Sampson error. + // + // @param points1 First set of corresponding points as Nx2 matrix. + // @param points2 Second set of corresponding points as Nx2 matrix. + // @param F 3x3 fundamental matrix. + // @param residuals Output vector of residuals. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& F, + std::vector* residuals); +}; + +// Fundamental matrix estimator from corresponding point pairs. +// +// This algorithm solves the 8-Point problem based on the following paper: +// +// Hartley and Zisserman, Multiple View Geometry, algorithm 11.1, page 282. +class FundamentalMatrixEightPointEstimator { + public: + typedef Eigen::Vector2d X_t; + typedef Eigen::Vector2d Y_t; + typedef Eigen::Matrix3d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 8; + + // Estimate fundamental matrix solutions from a set of corresponding points. + // + // The number of corresponding points must be at least 8. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points + // + // @return Single solution as a vector of 3x3 fundamental matrices. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Calculate the residuals of a set of corresponding points and a given + // fundamental matrix. + // + // Residuals are defined as the squared Sampson error. + // + // @param points1 First set of corresponding points as Nx2 matrix. + // @param points2 Second set of corresponding points as Nx2 matrix. + // @param F 3x3 fundamental matrix. + // @param residuals Output vector of residuals. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& F, + std::vector* residuals); +}; + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_FUNDAMENTAL_MATRIX_H_ diff --git a/colmap/include/colmap/estimators/generalized_absolute_pose.h b/colmap/include/colmap/estimators/generalized_absolute_pose.h new file mode 100644 index 0000000000000000000000000000000000000000..168787bbdc7c7948fa92d91ff15b6e3ae389d029 --- /dev/null +++ b/colmap/include/colmap/estimators/generalized_absolute_pose.h @@ -0,0 +1,100 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_H_ +#define COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_H_ + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Solver for the Generalized P3P problem (NP3P or GP3P), based on: +// +// Lee, Gim Hee, et al. "Minimal solutions for pose estimation of a +// multi-camera system." Robotics Research. Springer International +// Publishing, 2016. 521-538. +// +// This class is based on an original implementation by Federico Camposeco. +class GP3PEstimator { + public: + // The generalized image observations, which is composed of the relative pose + // of the specific camera in the generalized camera and its image observation. + struct X_t { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // The relative transformation from the generalized camera to the camera + // frame of the observation. + Eigen::Matrix3x4d rel_tform; + // The 2D image feature observation. + Eigen::Vector2d xy; + }; + + // The observed 3D feature points in the world frame. + typedef Eigen::Vector3d Y_t; + // The transformation from the world to the generalized camera frame. + typedef Eigen::Matrix3x4d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 3; + + enum class ResidualType { + CosineDistance, + ReprojectionError, + }; + + // Whether to compute the cosine similarity or the reprojection error. + // [WARNING] The reprojection error being in normalized coordinates, + // the unique error threshold of RANSAC corresponds to different pixel values + // in the different cameras of the rig if they have different intrinsics. + ResidualType residual_type = ResidualType::CosineDistance; + + // Estimate the most probable solution of the GP3P problem from a set of + // three 2D-3D point correspondences. + static std::vector Estimate(const std::vector& points2D, + const std::vector& points3D); + + // Calculate the squared cosine distance error between the rays given a set of + // 2D-3D point correspondences and a projection matrix of the generalized + // camera. + void Residuals(const std::vector& points2D, + const std::vector& points3D, + const M_t& proj_matrix, std::vector* residuals); +}; + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::GP3PEstimator::X_t) + +#endif // COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_H_ diff --git a/colmap/include/colmap/estimators/generalized_absolute_pose_coeffs.h b/colmap/include/colmap/estimators/generalized_absolute_pose_coeffs.h new file mode 100644 index 0000000000000000000000000000000000000000..b1c401203e40508adeb00bf4735f6a175f4aa78c --- /dev/null +++ b/colmap/include/colmap/estimators/generalized_absolute_pose_coeffs.h @@ -0,0 +1,44 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_COEFFS_H_ +#define COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_COEFFS_H_ + +#include + +namespace colmap { + +Eigen::Matrix ComputeDepthsSylvesterCoeffs( + const Eigen::Matrix& K); + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_COEFFS_H_ diff --git a/colmap/include/colmap/estimators/generalized_relative_pose.h b/colmap/include/colmap/estimators/generalized_relative_pose.h new file mode 100644 index 0000000000000000000000000000000000000000..f950b40c34b9e030cd54b982c07b8cb1c7050c7a --- /dev/null +++ b/colmap/include/colmap/estimators/generalized_relative_pose.h @@ -0,0 +1,94 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_ESTIMATORS_GENERALIZED_RELATIVE_POSE_H_ +#define COLMAP_SRC_ESTIMATORS_GENERALIZED_RELATIVE_POSE_H_ + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Solver for the Generalized Relative Pose problem using a minimal of 8 2D-2D +// correspondences. This implementation is based on: +// +// "Efficient Computation of Relative Pose for Multi-Camera Systems", +// Kneip and Li. CVPR 2014. +// +// Note that the solution to this problem is degenerate in the case of pure +// translation and when all correspondences are observed from the same cameras. +// +// The implementation is a modified and improved version of Kneip's original +// implementation in OpenGV licensed under the BSD license. +class GR6PEstimator { + public: + // The generalized image observations of the left camera, which is composed of + // the relative pose of the specific camera in the generalized camera and its + // image observation. + struct X_t { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // The relative transformation from the generalized camera to the camera + // frame of the observation. + Eigen::Matrix3x4d rel_tform; + // The 2D image feature observation. + Eigen::Vector2d xy; + }; + + // The normalized image feature points in the left camera. + typedef X_t Y_t; + // The relative transformation between the two generalized cameras. + typedef Eigen::Matrix3x4d M_t; + + // The minimum number of samples needed to estimate a model. Note that in + // theory the minimum required number of samples is 6 but Laurent Kneip showed + // in his paper that using 8 samples is more stable. + static const int kMinNumSamples = 8; + + // Estimate the most probable solution of the GR6P problem from a set of + // six 2D-2D point correspondences. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Calculate the squared Sampson error between corresponding points. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& proj_matrix, + std::vector* residuals); +}; + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::GR6PEstimator::X_t) + +#endif // COLMAP_SRC_ESTIMATORS_GENERALIZED_RELATIVE_POSE_H_ diff --git a/colmap/include/colmap/estimators/homography_matrix.h b/colmap/include/colmap/estimators/homography_matrix.h new file mode 100644 index 0000000000000000000000000000000000000000..847eb4c6e3609276b57cc99f86265093f954caa4 --- /dev/null +++ b/colmap/include/colmap/estimators/homography_matrix.h @@ -0,0 +1,83 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_ESTIMATORS_HOMOGRAPHY_MATRIX_H_ +#define COLMAP_SRC_ESTIMATORS_HOMOGRAPHY_MATRIX_H_ + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Direct linear transformation algorithm to compute the homography between +// point pairs. This algorithm computes the least squares estimate for +// the homography from at least 4 correspondences. +class HomographyMatrixEstimator { + public: + typedef Eigen::Vector2d X_t; + typedef Eigen::Vector2d Y_t; + typedef Eigen::Matrix3d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 4; + + // Estimate the projective transformation (homography). + // + // The number of corresponding points must be at least 4. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points. + // + // @return 3x3 homogeneous transformation matrix. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Calculate the transformation error for each corresponding point pair. + // + // Residuals are defined as the squared transformation error when + // transforming the source to the destination coordinates. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points. + // @param H 3x3 projective matrix. + // @param residuals Output vector of residuals. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& H, + std::vector* residuals); +}; + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_HOMOGRAPHY_MATRIX_H_ diff --git a/colmap/include/colmap/estimators/pose.h b/colmap/include/colmap/estimators/pose.h new file mode 100644 index 0000000000000000000000000000000000000000..b848340a74e89f2500889350194aa3f224c6f42e --- /dev/null +++ b/colmap/include/colmap/estimators/pose.h @@ -0,0 +1,234 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_ESTIMATORS_POSE_H_ +#define COLMAP_SRC_ESTIMATORS_POSE_H_ + +#include + +#include + +#include + +#include "base/camera.h" +#include "base/camera_models.h" +#include "optim/loransac.h" +#include "util/alignment.h" +#include "util/logging.h" +#include "util/threading.h" +#include "util/types.h" + +namespace colmap { + +struct AbsolutePoseEstimationOptions { + // Whether to estimate the focal length. + bool estimate_focal_length = false; + + // Number of discrete samples for focal length estimation. + size_t num_focal_length_samples = 30; + + // Minimum focal length ratio for discrete focal length sampling + // around focal length of given camera. + double min_focal_length_ratio = 0.2; + + // Maximum focal length ratio for discrete focal length sampling + // around focal length of given camera. + double max_focal_length_ratio = 5; + + // Number of threads for parallel estimation of focal length. + int num_threads = ThreadPool::kMaxNumThreads; + + // Options used for P3P RANSAC. + RANSACOptions ransac_options; + + void Check() const { + CHECK_GT(num_focal_length_samples, 0); + CHECK_GT(min_focal_length_ratio, 0); + CHECK_GT(max_focal_length_ratio, 0); + CHECK_LT(min_focal_length_ratio, max_focal_length_ratio); + ransac_options.Check(); + } +}; + +struct AbsolutePoseRefinementOptions { + // Convergence criterion. + double gradient_tolerance = 1.0; + + // Maximum number of solver iterations. + int max_num_iterations = 100; + + // Scaling factor determines at which residual robustification takes place. + double loss_function_scale = 1.0; + + // Whether to refine the focal length parameter group. + bool refine_focal_length = true; + + // Whether to refine the extra parameter group. + bool refine_extra_params = true; + + // Whether to print final summary. + bool print_summary = true; + + void Check() const { + CHECK_GE(gradient_tolerance, 0.0); + CHECK_GE(max_num_iterations, 0); + CHECK_GE(loss_function_scale, 0.0); + } +}; + +// Estimate absolute pose (optionally focal length) from 2D-3D correspondences. +// +// Focal length estimation is performed using discrete sampling around the +// focal length of the given camera. The focal length that results in the +// maximal number of inliers is assigned to the given camera. +// +// @param options Absolute pose estimation options. +// @param points2D Corresponding 2D points. +// @param points3D Corresponding 3D points. +// @param qvec Estimated rotation component as +// unit Quaternion coefficients (w, x, y, z). +// @param tvec Estimated translation component. +// @param camera Camera for which to estimate pose. Modified +// in-place to store the estimated focal length. +// @param num_inliers Number of inliers in RANSAC. +// @param inlier_mask Inlier mask for 2D-3D correspondences. +// +// @return Whether pose is estimated successfully. +bool EstimateAbsolutePose(const AbsolutePoseEstimationOptions& options, + const std::vector& points2D, + const std::vector& points3D, + Eigen::Vector4d* qvec, Eigen::Vector3d* tvec, + Camera* camera, size_t* num_inliers, + std::vector* inlier_mask); + +// Estimate relative from 2D-2D correspondences. +// +// Pose of first camera is assumed to be at the origin without rotation. Pose +// of second camera is given as world-to-image transformation, +// i.e. `x2 = [R | t] * X2`. +// +// @param ransac_options RANSAC options. +// @param points1 Corresponding 2D points. +// @param points2 Corresponding 2D points. +// @param qvec Estimated rotation component as +// unit Quaternion coefficients (w, x, y, z). +// @param tvec Estimated translation component. +// +// @return Number of RANSAC inliers. +size_t EstimateRelativePose(const RANSACOptions& ransac_options, + const std::vector& points1, + const std::vector& points2, + Eigen::Vector4d* qvec, Eigen::Vector3d* tvec); + +// Refine absolute pose (optionally focal length) from 2D-3D correspondences. +// +// @param options Refinement options. +// @param inlier_mask Inlier mask for 2D-3D correspondences. +// @param points2D Corresponding 2D points. +// @param points3D Corresponding 3D points. +// @param qvec Estimated rotation component as +// unit Quaternion coefficients (w, x, y, z). +// @param tvec Estimated translation component. +// @param camera Camera for which to estimate pose. Modified +// in-place to store the estimated focal length. +// @param rot_tvec_covariance Estimated 6x6 covariance matrix of +// the rotation (as axis-angle, in tangent space) +// and translation terms (optional). +// +// @return Whether the solution is usable. +bool RefineAbsolutePose(const AbsolutePoseRefinementOptions& options, + const std::vector& inlier_mask, + const std::vector& points2D, + const std::vector& points3D, + Eigen::Vector4d* qvec, Eigen::Vector3d* tvec, + Camera* camera, + Eigen::Matrix6d* rot_tvec_covariance = nullptr); + +// Refine relative pose of two cameras. +// +// Minimizes the Sampson error between corresponding normalized points using +// a robust cost function, i.e. the corresponding points need not necessarily +// be inliers given a sufficient initial guess for the relative pose. +// +// Assumes that first camera pose has projection matrix P = [I | 0], and +// pose of second camera is given as transformation from world to camera system. +// +// Assumes that the given translation vector is normalized, and refines +// the translation up to an unknown scale (i.e. refined translation vector +// is a unit vector again). +// +// @param options Solver options. +// @param points1 First set of corresponding points. +// @param points2 Second set of corresponding points. +// @param qvec Unit Quaternion rotation coefficients (w, x, y, z). +// @param tvec 3x1 translation vector. +// +// @return Flag indicating if solution is usable. +bool RefineRelativePose(const ceres::Solver::Options& options, + const std::vector& points1, + const std::vector& points2, + Eigen::Vector4d* qvec, Eigen::Vector3d* tvec); + +// Refine generalized absolute pose (optionally focal lengths) +// from 2D-3D correspondences. +// +// @param options Refinement options. +// @param inlier_mask Inlier mask for 2D-3D correspondences. +// @param points2D Corresponding 2D points. +// @param points3D Corresponding 3D points. +// @param camera_idxs Index of the rig camera for each correspondence. +// @param rig_qvecs Relative rotations from rig to each camera frame +// @param rig_tvecs Relative translations from rig +// to each camera frame. +// @param qvec Estimated rotation component of the rig as +// unit Quaternion coefficients (w, x, y, z). +// @param tvec Estimated translation component of the rig. +// @param cameras Cameras for which to estimate pose. Modified +// in-place to store the estimated focal lengths. +// @param rot_tvec_covariance Estimated 6x6 covariance matrix of +// the rotation (as axis-angle, in tangent space) +// and translation terms (optional). +// +// @return Whether the solution is usable. +bool RefineGeneralizedAbsolutePose( + const AbsolutePoseRefinementOptions& options, + const std::vector& inlier_mask, + const std::vector& points2D, + const std::vector& points3D, + const std::vector& camera_idxs, + const std::vector& rig_qvecs, + const std::vector& rig_tvecs, Eigen::Vector4d* qvec, + Eigen::Vector3d* tvec, std::vector* cameras, + Eigen::Matrix6d* rot_tvec_covariance = nullptr); + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_POSE_H_ diff --git a/colmap/include/colmap/estimators/similarity_transform.h b/colmap/include/colmap/estimators/similarity_transform.h new file mode 100644 index 0000000000000000000000000000000000000000..6381b5cb12832daf3c9ee10c25969511a1c65e12 --- /dev/null +++ b/colmap/include/colmap/estimators/similarity_transform.h @@ -0,0 +1,138 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_ESTIMATORS_SIMILARITY_TRANSFORM_H_ +#define COLMAP_SRC_ESTIMATORS_SIMILARITY_TRANSFORM_H_ + +#include + +#include +#include + +#include "base/projection.h" +#include "util/alignment.h" +#include "util/logging.h" +#include "util/types.h" + +namespace colmap { + +// N-D similarity transform estimator from corresponding point pairs in the +// source and destination coordinate systems. +// +// This algorithm is based on the following paper: +// +// S. Umeyama. Least-Squares Estimation of Transformation Parameters +// Between Two Point Patterns. IEEE Transactions on Pattern Analysis and +// Machine Intelligence, Volume 13 Issue 4, Page 376-380, 1991. +// http://www.stanford.edu/class/cs273/refs/umeyama.pdf +// +// and uses the Eigen implementation. +template +class SimilarityTransformEstimator { + public: + typedef Eigen::Matrix X_t; + typedef Eigen::Matrix Y_t; + typedef Eigen::Matrix M_t; + + // The minimum number of samples needed to estimate a model. Note that + // this only returns the true minimal sample in the two-dimensional case. + // For higher dimensions, the system will alway be over-determined. + static const int kMinNumSamples = kDim; + + // Estimate the similarity transform. + // + // @param src Set of corresponding source points. + // @param dst Set of corresponding destination points. + // + // @return 4x4 homogeneous transformation matrix. + static std::vector Estimate(const std::vector& src, + const std::vector& dst); + + // Calculate the transformation error for each corresponding point pair. + // + // Residuals are defined as the squared transformation error when + // transforming the source to the destination coordinates. + // + // @param src Set of corresponding points in the source coordinate + // system as a Nx3 matrix. + // @param dst Set of corresponding points in the destination + // coordinate system as a Nx3 matrix. + // @param matrix 4x4 homogeneous transformation matrix. + // @param residuals Output vector of residuals for each point pair. + static void Residuals(const std::vector& src, + const std::vector& dst, const M_t& matrix, + std::vector* residuals); +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +std::vector::M_t> +SimilarityTransformEstimator::Estimate( + const std::vector& src, const std::vector& dst) { + CHECK_EQ(src.size(), dst.size()); + + Eigen::Matrix src_mat(kDim, src.size()); + Eigen::Matrix dst_mat(kDim, dst.size()); + for (size_t i = 0; i < src.size(); ++i) { + src_mat.col(i) = src[i]; + dst_mat.col(i) = dst[i]; + } + + const M_t model = Eigen::umeyama(src_mat, dst_mat, kEstimateScale) + .topLeftCorner(kDim, kDim + 1); + + if (model.array().isNaN().any()) { + return std::vector{}; + } + + return {model}; +} + +template +void SimilarityTransformEstimator::Residuals( + const std::vector& src, const std::vector& dst, const M_t& matrix, + std::vector* residuals) { + CHECK_EQ(src.size(), dst.size()); + + residuals->resize(src.size()); + + for (size_t i = 0; i < src.size(); ++i) { + const Y_t dst_transformed = matrix * src[i].homogeneous(); + (*residuals)[i] = (dst[i] - dst_transformed).squaredNorm(); + } +} + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_SIMILARITY_TRANSFORM_H_ diff --git a/colmap/include/colmap/estimators/translation_transform.h b/colmap/include/colmap/estimators/translation_transform.h new file mode 100644 index 0000000000000000000000000000000000000000..1c86d6264bdc72b649ddca34a542e5a469d483ca --- /dev/null +++ b/colmap/include/colmap/estimators/translation_transform.h @@ -0,0 +1,120 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_ +#define COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_ + +#include + +#include +#include + +#include "util/alignment.h" +#include "util/logging.h" +#include "util/types.h" + +namespace colmap { + +// Estimate a N-D translation transformation between point pairs. +template +class TranslationTransformEstimator { + public: + typedef Eigen::Matrix X_t; + typedef Eigen::Matrix Y_t; + typedef Eigen::Matrix M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 1; + + // Estimate the 2D translation transform. + // + // @param points1 Set of corresponding source 2D points. + // @param points2 Set of corresponding destination 2D points. + // + // @return Translation vector. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Calculate the squared translation error. + // + // @param points1 Set of corresponding source 2D points. + // @param points2 Set of corresponding destination 2D points. + // @param translation Translation vector. + // @param residuals Output vector of residuals for each point pair. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& translation, + std::vector* residuals); +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +std::vector::M_t> +TranslationTransformEstimator::Estimate(const std::vector& points1, + const std::vector& points2) { + CHECK_EQ(points1.size(), points2.size()); + + X_t mean_src = X_t::Zero(); + Y_t mean_dst = Y_t::Zero(); + + for (size_t i = 0; i < points1.size(); ++i) { + mean_src += points1[i]; + mean_dst += points2[i]; + } + + mean_src /= points1.size(); + mean_dst /= points2.size(); + + std::vector models(1); + models[0] = mean_dst - mean_src; + + return models; +} + +template +void TranslationTransformEstimator::Residuals( + const std::vector& points1, const std::vector& points2, + const M_t& translation, std::vector* residuals) { + CHECK_EQ(points1.size(), points2.size()); + + residuals->resize(points1.size()); + + for (size_t i = 0; i < points1.size(); ++i) { + const M_t diff = points2[i] - points1[i] - translation; + (*residuals)[i] = diff.squaredNorm(); + } +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_ diff --git a/colmap/include/colmap/estimators/triangulation.h b/colmap/include/colmap/estimators/triangulation.h new file mode 100644 index 0000000000000000000000000000000000000000..0f4f9f4eeae0e2501676c8479cb5fbc085250760 --- /dev/null +++ b/colmap/include/colmap/estimators/triangulation.h @@ -0,0 +1,156 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_ESTIMATORS_TRIANGULATION_H_ +#define COLMAP_SRC_ESTIMATORS_TRIANGULATION_H_ + +#include "base/camera.h" + +#include + +#include + +#include "optim/ransac.h" +#include "util/alignment.h" +#include "util/math.h" +#include "util/types.h" + +namespace colmap { + +// Triangulation estimator to estimate 3D point from multiple observations. +// The triangulation must satisfy the following constraints: +// - Sufficient triangulation angle between observation pairs. +// - All observations must satisfy cheirality constraint. +// +// An observation is composed of an image measurement and the corresponding +// camera pose and calibration. +class TriangulationEstimator { + public: + enum class ResidualType { + ANGULAR_ERROR, + REPROJECTION_ERROR, + }; + + struct PointData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PointData() {} + PointData(const Eigen::Vector2d& point_, const Eigen::Vector2d& point_N_) + : point(point_), point_normalized(point_N_) {} + // Image observation in pixels. Only needs to be set for REPROJECTION_ERROR. + Eigen::Vector2d point; + // Normalized image observation. Must always be set. + Eigen::Vector2d point_normalized; + }; + + struct PoseData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PoseData() : camera(nullptr) {} + PoseData(const Eigen::Matrix3x4d& proj_matrix_, + const Eigen::Vector3d& pose_, const Camera* camera_) + : proj_matrix(proj_matrix_), proj_center(pose_), camera(camera_) {} + // The projection matrix for the image of the observation. + Eigen::Matrix3x4d proj_matrix; + // The projection center for the image of the observation. + Eigen::Vector3d proj_center; + // The camera for the image of the observation. + const Camera* camera; + }; + + typedef PointData X_t; + typedef PoseData Y_t; + typedef Eigen::Vector3d M_t; + + // Specify settings for triangulation estimator. + void SetMinTriAngle(const double min_tri_angle); + void SetResidualType(const ResidualType residual_type); + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 2; + + // Estimate a 3D point from a two-view observation. + // + // @param point_data Image measurement. + // @param point_data Camera poses. + // + // @return Triangulated point if successful, otherwise none. + std::vector Estimate(const std::vector& point_data, + const std::vector& pose_data) const; + + // Calculate residuals in terms of squared reprojection or angular error. + // + // @param point_data Image measurements. + // @param point_data Camera poses. + // @param xyz 3D point. + // + // @return Residual for each observation. + void Residuals(const std::vector& point_data, + const std::vector& pose_data, const M_t& xyz, + std::vector* residuals) const; + + private: + ResidualType residual_type_ = ResidualType::REPROJECTION_ERROR; + double min_tri_angle_ = 0.0; +}; + +struct EstimateTriangulationOptions { + // Minimum triangulation angle in radians. + double min_tri_angle = 0.0; + + // The employed residual type. + TriangulationEstimator::ResidualType residual_type = + TriangulationEstimator::ResidualType::ANGULAR_ERROR; + + // RANSAC options for TriangulationEstimator. + RANSACOptions ransac_options; + + void Check() const { + CHECK_GE(min_tri_angle, 0.0); + ransac_options.Check(); + } +}; + +// Robustly estimate 3D point from observations in multiple views using RANSAC +// and a subsequent non-linear refinement using all inliers. Returns true +// if the estimated number of inliers has more than two views. +bool EstimateTriangulation( + const EstimateTriangulationOptions& options, + const std::vector& point_data, + const std::vector& pose_data, + std::vector* inlier_mask, Eigen::Vector3d* xyz); + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM( + colmap::TriangulationEstimator::PointData) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM( + colmap::TriangulationEstimator::PoseData) + +#endif // COLMAP_SRC_ESTIMATORS_TRIANGULATION_H_ diff --git a/colmap/include/colmap/estimators/two_view_geometry.h b/colmap/include/colmap/estimators/two_view_geometry.h new file mode 100644 index 0000000000000000000000000000000000000000..2641a2594a56fba29ea834f9e0516af8634cc412 --- /dev/null +++ b/colmap/include/colmap/estimators/two_view_geometry.h @@ -0,0 +1,275 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_ESTIMATORS_TWO_VIEW_GEOMETRY_H_ +#define COLMAP_SRC_ESTIMATORS_TWO_VIEW_GEOMETRY_H_ + +#include "base/camera.h" +#include "feature/types.h" +#include "optim/ransac.h" +#include "util/alignment.h" +#include "util/logging.h" + +namespace colmap { + +// Two-view geometry estimator. +struct TwoViewGeometry { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + // The configuration of the estimated two-view geometry. + enum ConfigurationType { + UNDEFINED = 0, + // Degenerate configuration (e.g., no overlap or not enough inliers). + DEGENERATE = 1, + // Essential matrix. + CALIBRATED = 2, + // Fundamental matrix. + UNCALIBRATED = 3, + // Homography, planar scene with baseline. + PLANAR = 4, + // Homography, pure rotation without baseline. + PANORAMIC = 5, + // Homography, planar or panoramic. + PLANAR_OR_PANORAMIC = 6, + // Watermark, pure 2D translation in image borders. + WATERMARK = 7, + // Multi-model configuration, i.e. the inlier matches result from multiple + // individual, non-degenerate configurations. + MULTIPLE = 8, + }; + + // Estimation options. + struct Options { + // Minimum number of inliers for non-degenerate two-view geometry. + size_t min_num_inliers = 15; + + // In case both cameras are calibrated, the calibration is verified by + // estimating an essential and fundamental matrix and comparing their + // fractions of number of inliers. If the essential matrix produces + // a similar number of inliers (`min_E_F_inlier_ratio * F_num_inliers`), + // the calibration is assumed to be correct. + double min_E_F_inlier_ratio = 0.95; + + // In case an epipolar geometry can be verified, it is checked whether + // the geometry describes a planar scene or panoramic view (pure rotation) + // described by a homography. This is a degenerate case, since epipolar + // geometry is only defined for a moving camera. If the inlier ratio of + // a homography comes close to the inlier ratio of the epipolar geometry, + // a planar or panoramic configuration is assumed. + double max_H_inlier_ratio = 0.8; + + // In case of valid two-view geometry, it is checked whether the geometry + // describes a pure translation in the border region of the image. If more + // than a certain ratio of inlier points conform with a pure image + // translation, a watermark is assumed. + double watermark_min_inlier_ratio = 0.7; + + // Watermark matches have to be in the border region of the image. The + // border region is defined as the area around the image borders and + // is defined as a fraction of the image diagonal. + double watermark_border_size = 0.1; + + // Whether to enable watermark detection. A watermark causes a pure + // translation in the image space with inliers in the border region. + bool detect_watermark = true; + + // Whether to ignore watermark models in multiple model estimation. + bool multiple_ignore_watermark = true; + + // In case the user asks for it, only going to estimate a Homography + // between both cameras. + bool force_H_use = false; + + // Whether to compute the relative pose between the two views. + bool compute_relative_pose = false; + + // Options used to robustly estimate the geometry. + RANSACOptions ransac_options; + + void Check() const { + CHECK_GE(min_num_inliers, 0); + CHECK_GE(min_E_F_inlier_ratio, 0); + CHECK_LE(min_E_F_inlier_ratio, 1); + CHECK_GE(max_H_inlier_ratio, 0); + CHECK_LE(max_H_inlier_ratio, 1); + CHECK_GE(watermark_min_inlier_ratio, 0); + CHECK_LE(watermark_min_inlier_ratio, 1); + CHECK_GE(watermark_border_size, 0); + CHECK_LE(watermark_border_size, 1); + ransac_options.Check(); + } + }; + + TwoViewGeometry() + : config(ConfigurationType::UNDEFINED), + E(Eigen::Matrix3d::Zero()), + F(Eigen::Matrix3d::Zero()), + H(Eigen::Matrix3d::Zero()), + qvec(Eigen::Vector4d::Zero()), + tvec(Eigen::Vector3d::Zero()), + tri_angle(0) {} + + // Invert the two-view geometry in-place. + void Invert(); + + // Estimate two-view geometry from calibrated or uncalibrated image pair, + // depending on whether a prior focal length is given or not. + // + // @param camera1 Camera of first image. + // @param points1 Feature points in first image. + // @param camera2 Camera of second image. + // @param points2 Feature points in second image. + // @param matches Feature matches between first and second image. + // @param options Two-view geometry estimation options. + void Estimate(const Camera& camera1, + const std::vector& points1, + const Camera& camera2, + const std::vector& points2, + const FeatureMatches& matches, const Options& options); + + // Recursively estimate multiple configurations by removing the previous set + // of inliers from the matches until not enough inliers are found. Inlier + // matches are concatenated and the configuration type is `MULTIPLE` if + // multiple models could be estimated. This is useful to estimate the two-view + // geometry for images with large distortion or multiple rigidly moving + // objects in the scene. + // + // Note that in case the model type is `MULTIPLE`, only the `inlier_matches` + // field will be initialized. + // + // @param camera1 Camera of first image. + // @param points1 Feature points in first image. + // @param camera2 Camera of second image. + // @param points2 Feature points in second image. + // @param matches Feature matches between first and second image. + // @param options Two-view geometry estimation options. + void EstimateMultiple(const Camera& camera1, + const std::vector& points1, + const Camera& camera2, + const std::vector& points2, + const FeatureMatches& matches, const Options& options); + + // Estimate two-view geometry and its relative pose from a calibrated or an + // uncalibrated image pair. + // + // @param camera1 Camera of first image. + // @param points1 Feature points in first image. + // @param camera2 Camera of second image. + // @param points2 Feature points in second image. + // @param matches Feature matches between first and second image. + // @param options Two-view geometry estimation options. + bool EstimateRelativePose(const Camera& camera1, + const std::vector& points1, + const Camera& camera2, + const std::vector& points2); + + // Estimate two-view geometry from calibrated image pair. + // + // @param camera1 Camera of first image. + // @param points1 Feature points in first image. + // @param camera2 Camera of second image. + // @param points2 Feature points in second image. + // @param matches Feature matches between first and second image. + // @param options Two-view geometry estimation options. + void EstimateCalibrated(const Camera& camera1, + const std::vector& points1, + const Camera& camera2, + const std::vector& points2, + const FeatureMatches& matches, + const Options& options); + + // Estimate two-view geometry from uncalibrated image pair. + // + // @param camera1 Camera of first image. + // @param points1 Feature points in first image. + // @param camera2 Camera of second image. + // @param points2 Feature points in second image. + // @param matches Feature matches between first and second image. + // @param options Two-view geometry estimation options. + void EstimateUncalibrated(const Camera& camera1, + const std::vector& points1, + const Camera& camera2, + const std::vector& points2, + const FeatureMatches& matches, + const Options& options); + + // Estimate two-view geometry using a Homography, + // depending on the option was user specified or not. + // + // @param camera1 Camera of first image. + // @param points1 Feature points in first image. + // @param camera2 Camera of second image. + // @param points2 Feature points in second image. + // @param matches Feature matches between first and second image. + // @param options Two-view geometry estimation options. + void EstimateHomography(const Camera& camera1, + const std::vector& points1, + const Camera& camera2, + const std::vector& points2, + const FeatureMatches& matches, + const Options& options); + + // Detect if inlier matches are caused by a watermark. + // A watermark causes a pure translation in the border are of the image. + static bool DetectWatermark(const Camera& camera1, + const std::vector& points1, + const Camera& camera2, + const std::vector& points2, + const size_t num_inliers, + const std::vector& inlier_mask, + const Options& options); + + // One of `ConfigurationType`. + int config; + + // Essential matrix. + Eigen::Matrix3d E; + // Fundamental matrix. + Eigen::Matrix3d F; + // Homography matrix. + Eigen::Matrix3d H; + + // Relative pose. + Eigen::Vector4d qvec; + Eigen::Vector3d tvec; + + // Inlier matches of the configuration. + FeatureMatches inlier_matches; + + // Median triangulation angle. + double tri_angle; +}; + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::TwoViewGeometry) + +#endif // COLMAP_SRC_ESTIMATORS_TWO_VIEW_GEOMETRY_H_ diff --git a/colmap/include/colmap/estimators/utils.h b/colmap/include/colmap/estimators/utils.h new file mode 100644 index 0000000000000000000000000000000000000000..56d0f962149c59f055b28f9a5d789fce1f40eaeb --- /dev/null +++ b/colmap/include/colmap/estimators/utils.h @@ -0,0 +1,92 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_ESTIMATORS_UTILS_H_ +#define COLMAP_SRC_ESTIMATORS_UTILS_H_ + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Center and normalize image points. +// +// The points are transformed in a two-step procedure that is expressed +// as a transformation matrix. The matrix of the resulting points is usually +// better conditioned than the matrix of the original points. +// +// Center the image points, such that the new coordinate system has its +// origin at the centroid of the image points. +// +// Normalize the image points, such that the mean distance from the points +// to the coordinate system is sqrt(2). +// +// @param points Image coordinates. +// @param normed_points Transformed image coordinates. +// @param matrix 3x3 transformation matrix. +void CenterAndNormalizeImagePoints(const std::vector& points, + std::vector* normed_points, + Eigen::Matrix3d* matrix); + +// Calculate the residuals of a set of corresponding points and a given +// fundamental or essential matrix. +// +// Residuals are defined as the squared Sampson error. +// +// @param points1 First set of corresponding points as Nx2 matrix. +// @param points2 Second set of corresponding points as Nx2 matrix. +// @param E 3x3 fundamental or essential matrix. +// @param residuals Output vector of residuals. +void ComputeSquaredSampsonError(const std::vector& points1, + const std::vector& points2, + const Eigen::Matrix3d& E, + std::vector* residuals); + +// Calculate the squared reprojection error given a set of 2D-3D point +// correspondences and a projection matrix. Returns DBL_MAX if a 3D point is +// behind the given camera. +// +// @param points2D Normalized 2D image points. +// @param points3D 3D world points. +// @param proj_matrix 3x4 projection matrix. +// @param residuals Output vector of residuals. +void ComputeSquaredReprojectionError( + const std::vector& points2D, + const std::vector& points3D, + const Eigen::Matrix3x4d& proj_matrix, std::vector* residuals); + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_UTILS_H_ diff --git a/colmap/include/colmap/exe/database.h b/colmap/include/colmap/exe/database.h new file mode 100644 index 0000000000000000000000000000000000000000..f64258cd93ef6201d5f5b813b39ea25528ad72b2 --- /dev/null +++ b/colmap/include/colmap/exe/database.h @@ -0,0 +1,38 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +namespace colmap { + +int RunDatabaseCleaner(int argc, char** argv); +int RunDatabaseCreator(int argc, char** argv); +int RunDatabaseMerger(int argc, char** argv); + +} // namespace colmap diff --git a/colmap/include/colmap/exe/feature.h b/colmap/include/colmap/exe/feature.h new file mode 100644 index 0000000000000000000000000000000000000000..851ea3121c0b9827e10253a38575cc2f726712cb --- /dev/null +++ b/colmap/include/colmap/exe/feature.h @@ -0,0 +1,86 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_EXE_FEATURE_H_ +#define COLMAP_SRC_EXE_FEATURE_H_ + +#include "base/image_reader.h" + +namespace colmap { + +// This enum can be used as optional input for feature_extractor and +// feature_importer to ensure that the camera flags of ImageReader are set in an +// exclusive and unambigous way. The table below explains the corespondence of +// each setting with the flags +// +// ----------------------------------------------------------------------------------- +// | | ImageReaderOptions | +// | CameraMode | single_camera | single_camera_per_folder | single_camera_per_image | +// |------------|---------------|--------------------------|-------------------------| +// | AUTO | false | false | false | +// | SINGLE | true | false | false | +// | PER_FOLDER | false | true | false | +// | PER_IMAGE | false | false | true | +// ----------------------------------------------------------------------------------- +// +// Note: When using AUTO mode a camera model will be uniquely identified by the +// following 5 parameters from EXIF tags: +// 1. Camera Make +// 2. Camera Model +// 3. Focal Length +// 4. Image Width +// 5. Image Height +// +// If any of the tags is missing then a camera model is considered invalid and a +// new camera is created similar to the PER_IMAGE mode. +// +// If these considered fields are not sufficient to uniquely identify a camera +// then using the AUTO mode will lead to incorrect setup for the cameras, e.g. +// the same camera is used with same focal length but different principal point +// between captures. In these cases it is recommended to either use the +// PER_FOLDER or PER_IMAGE settings. +enum class CameraMode { AUTO = 0, SINGLE = 1, PER_FOLDER = 2, PER_IMAGE = 3 }; + +void UpdateImageReaderOptionsFromCameraMode(ImageReaderOptions& options, + CameraMode mode); + +int RunFeatureExtractor(int argc, char** argv); +int RunFeatureImporter(int argc, char** argv); +int RunExhaustiveMatcher(int argc, char** argv); +int RunMatchesImporter(int argc, char** argv); +int RunSequentialMatcher(int argc, char** argv); +int RunSpatialMatcher(int argc, char** argv); +int RunTransitiveMatcher(int argc, char** argv); +int RunVocabTreeMatcher(int argc, char** argv); + +} // namespace colmap + +#endif // COLMAP_SRC_EXE_FEATURE_H_ diff --git a/colmap/include/colmap/exe/gui.h b/colmap/include/colmap/exe/gui.h new file mode 100644 index 0000000000000000000000000000000000000000..7ef49904ddbdd138ee4e1cada4c897979f1e8d53 --- /dev/null +++ b/colmap/include/colmap/exe/gui.h @@ -0,0 +1,56 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#include + +#ifdef GUI_ENABLED +#include +#include "ui/main_window.h" +#else +// Dummy QApplication class when GUI is disabled +class QApplication { + public: + QApplication(int argc, char** argv) {} +}; +#endif + +namespace colmap { + +#if defined(CUDA_ENABLED) || !defined(OPENGL_ENABLED) +const bool kUseOpenGL = false; +#else +const bool kUseOpenGL = true; +#endif + +int RunGraphicalUserInterface(int argc, char** argv); +int RunProjectGenerator(int argc, char** argv); + +} // namespace colmap diff --git a/colmap/include/colmap/exe/image.h b/colmap/include/colmap/exe/image.h new file mode 100644 index 0000000000000000000000000000000000000000..2d4d8566ebba346b4c7560efe22cf9772b771139 --- /dev/null +++ b/colmap/include/colmap/exe/image.h @@ -0,0 +1,41 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +namespace colmap { + +int RunImageDeleter(int argc, char** argv); +int RunImageFilterer(int argc, char** argv); +int RunImageRectifier(int argc, char** argv); +int RunImageRegistrator(int argc, char** argv); +int RunImageUndistorter(int argc, char** argv); +int RunImageUndistorterStandalone(int argc, char** argv); + +} // namespace colmap diff --git a/colmap/include/colmap/exe/model.h b/colmap/include/colmap/exe/model.h new file mode 100644 index 0000000000000000000000000000000000000000..e2bca3b7e39fc02ddc9bff1d2d5c00442275f146 --- /dev/null +++ b/colmap/include/colmap/exe/model.h @@ -0,0 +1,44 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +namespace colmap { + +int RunModelAligner(int argc, char** argv); +int RunModelAnalyzer(int argc, char** argv); +int RunModelComparer(int argc, char** argv); +int RunModelConverter(int argc, char** argv); +int RunModelCropper(int argc, char** argv); +int RunModelMerger(int argc, char** argv); +int RunModelOrientationAligner(int argc, char** argv); +int RunModelSplitter(int argc, char** argv); +int RunModelTransformer(int argc, char** argv); + +} // namespace colmap diff --git a/colmap/include/colmap/exe/mvs.h b/colmap/include/colmap/exe/mvs.h new file mode 100644 index 0000000000000000000000000000000000000000..8115825c2580e50ead0a67eac8c792c12ca60fc5 --- /dev/null +++ b/colmap/include/colmap/exe/mvs.h @@ -0,0 +1,39 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +namespace colmap { + +int RunDelaunayMesher(int argc, char** argv); +int RunPatchMatchStereo(int argc, char** argv); +int RunPoissonMesher(int argc, char** argv); +int RunStereoFuser(int argc, char** argv); + +} // namespace colmap diff --git a/colmap/include/colmap/exe/sfm.h b/colmap/include/colmap/exe/sfm.h new file mode 100644 index 0000000000000000000000000000000000000000..56bb07c5cfa52c3113761060d429bf30c57caf28 --- /dev/null +++ b/colmap/include/colmap/exe/sfm.h @@ -0,0 +1,58 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_EXE_SFM_H_ +#define COLMAP_SRC_EXE_SFM_H_ + +#include "base/reconstruction.h" +#include "controllers/incremental_mapper.h" + +namespace colmap { + +int RunPointTriangulatorImpl(Reconstruction& reconstruction, + const std::string database_path, + const std::string image_path, + const std::string output_path, + const IncrementalMapperOptions& mapper_options, + const bool clear_points); + +int RunAutomaticReconstructor(int argc, char** argv); +int RunBundleAdjuster(int argc, char** argv); +int RunColorExtractor(int argc, char** argv); +int RunMapper(int argc, char** argv); +int RunHierarchicalMapper(int argc, char** argv); +int RunPointFiltering(int argc, char** argv); +int RunPointTriangulator(int argc, char** argv); +int RunRigBundleAdjuster(int argc, char** argv); + +} // namespace colmap + +#endif // COLMAP_SRC_EXE_SFM_H_ diff --git a/colmap/include/colmap/exe/vocab_tree.h b/colmap/include/colmap/exe/vocab_tree.h new file mode 100644 index 0000000000000000000000000000000000000000..1e8a7c746893ea5bd4db7dcad2ee05307f3abcbd --- /dev/null +++ b/colmap/include/colmap/exe/vocab_tree.h @@ -0,0 +1,37 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +namespace colmap { + +int RunVocabTreeBuilder(int argc, char** argv); +int RunVocabTreeRetriever(int argc, char** argv); + +} // namespace colmap diff --git a/colmap/include/colmap/feature/extraction.h b/colmap/include/colmap/feature/extraction.h new file mode 100644 index 0000000000000000000000000000000000000000..133da255551831f494cf7ce495619eb394d4073c --- /dev/null +++ b/colmap/include/colmap/feature/extraction.h @@ -0,0 +1,155 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_FEATURE_EXTRACTION_H_ +#define COLMAP_SRC_FEATURE_EXTRACTION_H_ + +#include "base/database.h" +#include "base/image_reader.h" +#include "feature/sift.h" +#include "util/opengl_utils.h" +#include "util/threading.h" + +namespace colmap { + +namespace internal { + +struct ImageData; + +} // namespace internal + +// Feature extraction class to extract features for all images in a directory. +class SiftFeatureExtractor : public Thread { + public: + SiftFeatureExtractor(const ImageReaderOptions& reader_options, + const SiftExtractionOptions& sift_options); + + private: + void Run(); + + const ImageReaderOptions reader_options_; + const SiftExtractionOptions sift_options_; + + Database database_; + ImageReader image_reader_; + + std::vector> resizers_; + std::vector> extractors_; + std::unique_ptr writer_; + + std::unique_ptr> resizer_queue_; + std::unique_ptr> extractor_queue_; + std::unique_ptr> writer_queue_; +}; + +// Import features from text files. Each image must have a corresponding text +// file with the same name and an additional ".txt" suffix. +class FeatureImporter : public Thread { + public: + FeatureImporter(const ImageReaderOptions& reader_options, + const std::string& import_path); + + private: + void Run(); + + const ImageReaderOptions reader_options_; + const std::string import_path_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +namespace internal { + +struct ImageData { + ImageReader::Status status = ImageReader::Status::FAILURE; + + Camera camera; + Image image; + Bitmap bitmap; + Bitmap mask; + + FeatureKeypoints keypoints; + FeatureDescriptors descriptors; +}; + +class ImageResizerThread : public Thread { + public: + ImageResizerThread(const int max_image_size, JobQueue* input_queue, + JobQueue* output_queue); + + private: + void Run(); + + const int max_image_size_; + + JobQueue* input_queue_; + JobQueue* output_queue_; +}; + +class SiftFeatureExtractorThread : public Thread { + public: + SiftFeatureExtractorThread(const SiftExtractionOptions& sift_options, + const std::shared_ptr& camera_mask, + JobQueue* input_queue, + JobQueue* output_queue); + + private: + void Run(); + + const SiftExtractionOptions sift_options_; + std::shared_ptr camera_mask_; + + std::unique_ptr opengl_context_; + + JobQueue* input_queue_; + JobQueue* output_queue_; +}; + +class FeatureWriterThread : public Thread { + public: + FeatureWriterThread(const size_t num_images, Database* database, + JobQueue* input_queue); + + private: + void Run(); + + const size_t num_images_; + Database* database_; + JobQueue* input_queue_; +}; + +} // namespace internal + +} // namespace colmap + +#endif // COLMAP_SRC_FEATURE_EXTRACTION_H_ diff --git a/colmap/include/colmap/feature/matching.h b/colmap/include/colmap/feature/matching.h new file mode 100644 index 0000000000000000000000000000000000000000..23afc6ad0a8b154519de96dac155cec404ec3aca --- /dev/null +++ b/colmap/include/colmap/feature/matching.h @@ -0,0 +1,569 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_FEATURE_MATCHING_H_ +#define COLMAP_SRC_FEATURE_MATCHING_H_ + +#include +#include +#include + +#include "base/database.h" +#include "feature/sift.h" +#include "util/alignment.h" +#include "util/cache.h" +#include "util/opengl_utils.h" +#include "util/threading.h" +#include "util/timer.h" + +namespace colmap { + +struct ExhaustiveMatchingOptions { + // Block size, i.e. number of images to simultaneously load into memory. + int block_size = 50; + + bool Check() const; +}; + +struct SequentialMatchingOptions { + // Number of overlapping image pairs. + int overlap = 10; + + // Whether to match images against their quadratic neighbors. + bool quadratic_overlap = true; + + // Whether to enable vocabulary tree based loop detection. + bool loop_detection = false; + + // Loop detection is invoked every `loop_detection_period` images. + int loop_detection_period = 10; + + // The number of images to retrieve in loop detection. This number should + // be significantly bigger than the sequential matching overlap. + int loop_detection_num_images = 50; + + // Number of nearest neighbors to retrieve per query feature. + int loop_detection_num_nearest_neighbors = 1; + + // Number of nearest-neighbor checks to use in retrieval. + int loop_detection_num_checks = 256; + + // How many images to return after spatial verification. Set to 0 to turn off + // spatial verification. + int loop_detection_num_images_after_verification = 0; + + // The maximum number of features to use for indexing an image. If an + // image has more features, only the largest-scale features will be indexed. + int loop_detection_max_num_features = -1; + + // Path to the vocabulary tree. + std::string vocab_tree_path = ""; + + bool Check() const; +}; + +struct VocabTreeMatchingOptions { + // Number of images to retrieve for each query image. + int num_images = 100; + + // Number of nearest neighbors to retrieve per query feature. + int num_nearest_neighbors = 5; + + // Number of nearest-neighbor checks to use in retrieval. + int num_checks = 256; + + // How many images to return after spatial verification. Set to 0 to turn off + // spatial verification. + int num_images_after_verification = 0; + + // The maximum number of features to use for indexing an image. If an + // image has more features, only the largest-scale features will be indexed. + int max_num_features = -1; + + // Path to the vocabulary tree. + std::string vocab_tree_path = ""; + + // Optional path to file with specific image names to match. + std::string match_list_path = ""; + + bool Check() const; +}; + +struct SpatialMatchingOptions { + // Whether the location priors in the database are GPS coordinates in + // the form of longitude and latitude coordinates in degrees. + bool is_gps = true; + + // Whether to ignore the Z-component of the location prior. + bool ignore_z = true; + + // The maximum number of nearest neighbors to match. + int max_num_neighbors = 50; + + // The maximum distance between the query and nearest neighbor. For GPS + // coordinates the unit is Euclidean distance in meters. + double max_distance = 100; + + bool Check() const; +}; + +struct TransitiveMatchingOptions { + // The maximum number of image pairs to process in one batch. + int batch_size = 1000; + + // The number of transitive closure iterations. + int num_iterations = 3; + + bool Check() const; +}; + +struct ImagePairsMatchingOptions { + // Number of image pairs to match in one batch. + int block_size = 1225; + + // Path to the file with the matches. + std::string match_list_path = ""; + + bool Check() const; +}; + +struct FeaturePairsMatchingOptions { + // Whether to geometrically verify the given matches. + bool verify_matches = true; + + // Path to the file with the matches. + std::string match_list_path = ""; + + bool Check() const; +}; + +namespace internal { + +struct FeatureMatcherData { + image_t image_id1 = kInvalidImageId; + image_t image_id2 = kInvalidImageId; + FeatureMatches matches; + TwoViewGeometry two_view_geometry; +}; + +} // namespace internal + +using FeatureKeypointsPtr = std::shared_ptr; +using FeatureDescriptorsPtr = std::shared_ptr; + +// Cache for feature matching to minimize database access during matching. +class FeatureMatcherCache { + public: + FeatureMatcherCache(const size_t cache_size, const Database* database); + + void Setup(); + + const Camera& GetCamera(const camera_t camera_id) const; + const Image& GetImage(const image_t image_id) const; + FeatureKeypointsPtr GetKeypoints(const image_t image_id); + FeatureDescriptorsPtr GetDescriptors(const image_t image_id); + FeatureMatches GetMatches(const image_t image_id1, const image_t image_id2); + std::vector GetImageIds() const; + + bool ExistsKeypoints(const image_t image_id); + bool ExistsDescriptors(const image_t image_id); + + bool ExistsMatches(const image_t image_id1, const image_t image_id2); + bool ExistsInlierMatches(const image_t image_id1, const image_t image_id2); + + void WriteMatches(const image_t image_id1, const image_t image_id2, + const FeatureMatches& matches); + void WriteTwoViewGeometry(const image_t image_id1, const image_t image_id2, + const TwoViewGeometry& two_view_geometry); + + void DeleteMatches(const image_t image_id1, const image_t image_id2); + void DeleteInlierMatches(const image_t image_id1, const image_t image_id2); + + private: + const size_t cache_size_; + const Database* database_; + std::mutex database_mutex_; + EIGEN_STL_UMAP(camera_t, Camera) cameras_cache_; + EIGEN_STL_UMAP(image_t, Image) images_cache_; + std::unique_ptr> keypoints_cache_; + std::unique_ptr> descriptors_cache_; + std::unique_ptr> keypoints_exists_cache_; + std::unique_ptr> descriptors_exists_cache_; +}; + +class FeatureMatcherThread : public Thread { + public: + FeatureMatcherThread(const SiftMatchingOptions& options, + FeatureMatcherCache* cache); + + void SetMaxNumMatches(const int max_num_matches); + + protected: + SiftMatchingOptions options_; + FeatureMatcherCache* cache_; +}; + +class SiftCPUFeatureMatcher : public FeatureMatcherThread { + public: + typedef internal::FeatureMatcherData Input; + typedef internal::FeatureMatcherData Output; + + SiftCPUFeatureMatcher(const SiftMatchingOptions& options, + FeatureMatcherCache* cache, + JobQueue* input_queue, + JobQueue* output_queue); + + protected: + void Run() override; + + JobQueue* input_queue_; + JobQueue* output_queue_; +}; + +class SiftGPUFeatureMatcher : public FeatureMatcherThread { + public: + typedef internal::FeatureMatcherData Input; + typedef internal::FeatureMatcherData Output; + + SiftGPUFeatureMatcher(const SiftMatchingOptions& options, + FeatureMatcherCache* cache, + JobQueue* input_queue, + JobQueue* output_queue); + + protected: + void Run() override; + + void GetDescriptorData(const int index, const image_t image_id, + const FeatureDescriptors** descriptors_ptr); + + JobQueue* input_queue_; + JobQueue* output_queue_; + + std::unique_ptr opengl_context_; + + // The previously uploaded images to the GPU. + std::array prev_uploaded_image_ids_; + std::array prev_uploaded_descriptors_; +}; + +class GuidedSiftCPUFeatureMatcher : public FeatureMatcherThread { + public: + typedef internal::FeatureMatcherData Input; + typedef internal::FeatureMatcherData Output; + + GuidedSiftCPUFeatureMatcher(const SiftMatchingOptions& options, + FeatureMatcherCache* cache, + JobQueue* input_queue, + JobQueue* output_queue); + + private: + void Run() override; + + JobQueue* input_queue_; + JobQueue* output_queue_; +}; + +class GuidedSiftGPUFeatureMatcher : public FeatureMatcherThread { + public: + typedef internal::FeatureMatcherData Input; + typedef internal::FeatureMatcherData Output; + + GuidedSiftGPUFeatureMatcher(const SiftMatchingOptions& options, + FeatureMatcherCache* cache, + JobQueue* input_queue, + JobQueue* output_queue); + + private: + void Run() override; + + void GetFeatureData(const int index, const image_t image_id, + const FeatureKeypoints** keypoints_ptr, + const FeatureDescriptors** descriptors_ptr); + + JobQueue* input_queue_; + JobQueue* output_queue_; + + std::unique_ptr opengl_context_; + + // The previously uploaded images to the GPU. + std::array prev_uploaded_image_ids_; + std::array prev_uploaded_keypoints_; + std::array prev_uploaded_descriptors_; +}; + +class TwoViewGeometryVerifier : public Thread { + public: + typedef internal::FeatureMatcherData Input; + typedef internal::FeatureMatcherData Output; + + TwoViewGeometryVerifier(const SiftMatchingOptions& options, + FeatureMatcherCache* cache, + JobQueue* input_queue, + JobQueue* output_queue); + + protected: + void Run() override; + + const SiftMatchingOptions options_; + TwoViewGeometry::Options two_view_geometry_options_; + FeatureMatcherCache* cache_; + JobQueue* input_queue_; + JobQueue* output_queue_; +}; + +// Multi-threaded and multi-GPU SIFT feature matcher, which writes the computed +// results to the database and skips already matched image pairs. To improve +// performance of the matching by taking advantage of caching and database +// transactions, pass multiple images to the `Match` function. Note that the +// database should be in an active transaction while calling `Match`. +class SiftFeatureMatcher { + public: + SiftFeatureMatcher(const SiftMatchingOptions& options, Database* database, + FeatureMatcherCache* cache); + + ~SiftFeatureMatcher(); + + // Setup the matchers and return if successful. + bool Setup(); + + // Match one batch of multiple image pairs. + void Match(const std::vector>& image_pairs); + + private: + SiftMatchingOptions options_; + Database* database_; + FeatureMatcherCache* cache_; + + bool is_setup_; + + std::vector> matchers_; + std::vector> guided_matchers_; + std::vector> verifiers_; + std::unique_ptr thread_pool_; + + JobQueue matcher_queue_; + JobQueue verifier_queue_; + JobQueue guided_matcher_queue_; + JobQueue output_queue_; +}; + +// Exhaustively match images by processing each block in the exhaustive match +// matrix in one batch: +// +// +----+----+-----------------> images[i] +// |#000|0000| +// |1#00|1000| <- Above the main diagonal, the block diagonal is not matched +// |11#0|1100| ^ +// |111#|1110| | +// +----+----+ | +// |1000|#000|\ | +// |1100|1#00| \ One block | +// |1110|11#0| / of image pairs | +// |1111|111#|/ | +// +----+----+ | +// | ^ | +// | | | +// | Below the main diagonal, the block diagonal is matched <--------------+ +// | +// v +// images[i] +// +// Pairs will only be matched if 1, to avoid duplicate pairs. Pairs with # +// are on the main diagonal and denote pairs of the same image. +class ExhaustiveFeatureMatcher : public Thread { + public: + ExhaustiveFeatureMatcher(const ExhaustiveMatchingOptions& options, + const SiftMatchingOptions& match_options, + const std::string& database_path); + + private: + void Run() override; + + const ExhaustiveMatchingOptions options_; + const SiftMatchingOptions match_options_; + Database database_; + FeatureMatcherCache cache_; + SiftFeatureMatcher matcher_; +}; + +// Sequentially match images within neighborhood: +// +// +-------------------------------+-----------------------> images[i] +// ^ | ^ +// | Current image[i] | +// | | | +// +----------+-----------+ +// | +// Match image_i against +// +// image_[i - o, i + o] with o = [1 .. overlap] +// image_[i - 2^o, i + 2^o] (for quadratic overlap) +// +// Sequential order is determined based on the image names in ascending order. +// +// Invoke loop detection if `(i mod loop_detection_period) == 0`, retrieve +// most similar `loop_detection_num_images` images from vocabulary tree, +// and perform matching and verification. +class SequentialFeatureMatcher : public Thread { + public: + SequentialFeatureMatcher(const SequentialMatchingOptions& options, + const SiftMatchingOptions& match_options, + const std::string& database_path); + + private: + void Run() override; + + std::vector GetOrderedImageIds() const; + void RunSequentialMatching(const std::vector& image_ids); + void RunLoopDetection(const std::vector& image_ids); + + const SequentialMatchingOptions options_; + const SiftMatchingOptions match_options_; + Database database_; + FeatureMatcherCache cache_; + SiftFeatureMatcher matcher_; +}; + +// Match each image against its nearest neighbors using a vocabulary tree. +class VocabTreeFeatureMatcher : public Thread { + public: + VocabTreeFeatureMatcher(const VocabTreeMatchingOptions& options, + const SiftMatchingOptions& match_options, + const std::string& database_path); + + private: + void Run() override; + + const VocabTreeMatchingOptions options_; + const SiftMatchingOptions match_options_; + Database database_; + FeatureMatcherCache cache_; + SiftFeatureMatcher matcher_; +}; + +// Match images against spatial nearest neighbors using prior location +// information, e.g. provided manually or extracted from EXIF. +class SpatialFeatureMatcher : public Thread { + public: + SpatialFeatureMatcher(const SpatialMatchingOptions& options, + const SiftMatchingOptions& match_options, + const std::string& database_path); + + private: + void Run() override; + + const SpatialMatchingOptions options_; + const SiftMatchingOptions match_options_; + Database database_; + FeatureMatcherCache cache_; + SiftFeatureMatcher matcher_; +}; + +// Match transitive image pairs in a database with existing feature matches. +// This matcher transitively closes loops. For example, if image pairs A-B and +// B-C match but A-C has not been matched, then this matcher attempts to match +// A-C. This procedure is performed for multiple iterations. +class TransitiveFeatureMatcher : public Thread { + public: + TransitiveFeatureMatcher(const TransitiveMatchingOptions& options, + const SiftMatchingOptions& match_options, + const std::string& database_path); + + private: + void Run() override; + + const TransitiveMatchingOptions options_; + const SiftMatchingOptions match_options_; + Database database_; + FeatureMatcherCache cache_; + SiftFeatureMatcher matcher_; +}; + +// Match images manually specified in a list of image pairs. +// +// Read matches file with the following format: +// +// image_name1 image_name2 +// image_name1 image_name3 +// image_name2 image_name3 +// ... +// +class ImagePairsFeatureMatcher : public Thread { + public: + ImagePairsFeatureMatcher(const ImagePairsMatchingOptions& options, + const SiftMatchingOptions& match_options, + const std::string& database_path); + + private: + void Run() override; + + const ImagePairsMatchingOptions options_; + const SiftMatchingOptions match_options_; + Database database_; + FeatureMatcherCache cache_; + SiftFeatureMatcher matcher_; +}; + +// Import feature matches from a text file. +// +// Read matches file with the following format: +// +// image_name1 image_name2 +// 0 1 +// 1 2 +// 2 3 +// +// image_name1 image_name3 +// 0 1 +// 1 2 +// 2 3 +// ... +// +class FeaturePairsFeatureMatcher : public Thread { + public: + FeaturePairsFeatureMatcher(const FeaturePairsMatchingOptions& options, + const SiftMatchingOptions& match_options, + const std::string& database_path); + + private: + const static size_t kCacheSize = 100; + + void Run() override; + + const FeaturePairsMatchingOptions options_; + const SiftMatchingOptions match_options_; + Database database_; + FeatureMatcherCache cache_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_FEATURE_MATCHING_H_ diff --git a/colmap/include/colmap/feature/sift.h b/colmap/include/colmap/feature/sift.h new file mode 100644 index 0000000000000000000000000000000000000000..28ef3e43ba5cb15caa33e1cd70be32e63b3a0ba1 --- /dev/null +++ b/colmap/include/colmap/feature/sift.h @@ -0,0 +1,264 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_FEATURE_SIFT_H_ +#define COLMAP_SRC_FEATURE_SIFT_H_ + +#include "estimators/two_view_geometry.h" +#include "feature/types.h" +#include "util/bitmap.h" + +class SiftGPU; +class SiftMatchGPU; + +namespace colmap { + +struct SiftExtractionOptions { + // Number of threads for feature extraction. + int num_threads = -1; + + // Whether to use the GPU for feature extraction. + bool use_gpu = true; + + // Index of the GPU used for feature extraction. For multi-GPU extraction, + // you should separate multiple GPU indices by comma, e.g., "0,1,2,3". + std::string gpu_index = "-1"; + + // Maximum image size, otherwise image will be down-scaled. + int max_image_size = 3200; + + // Maximum number of features to detect, keeping larger-scale features. + int max_num_features = 8192; + + // First octave in the pyramid, i.e. -1 upsamples the image by one level. + int first_octave = -1; + + // Number of octaves. + int num_octaves = 4; + + // Number of levels per octave. + int octave_resolution = 3; + + // Peak threshold for detection. + double peak_threshold = 0.02 / octave_resolution; + + // Edge threshold for detection. + double edge_threshold = 10.0; + + // Estimate affine shape of SIFT features in the form of oriented ellipses as + // opposed to original SIFT which estimates oriented disks. + bool estimate_affine_shape = false; + + // Maximum number of orientations per keypoint if not estimate_affine_shape. + int max_num_orientations = 2; + + // Fix the orientation to 0 for upright features. + bool upright = false; + + // Whether to adapt the feature detection depending on the image darkness. + // Note that this feature is only available in the OpenGL SiftGPU version. + bool darkness_adaptivity = false; + + // Domain-size pooling parameters. Domain-size pooling computes an average + // SIFT descriptor across multiple scales around the detected scale. This was + // proposed in "Domain-Size Pooling in Local Descriptors and Network + // Architectures", J. Dong and S. Soatto, CVPR 2015. This has been shown to + // outperform other SIFT variants and learned descriptors in "Comparative + // Evaluation of Hand-Crafted and Learned Local Features", Schönberger, + // Hardmeier, Sattler, Pollefeys, CVPR 2016. + bool domain_size_pooling = false; + double dsp_min_scale = 1.0 / 6.0; + double dsp_max_scale = 3.0; + int dsp_num_scales = 10; + + enum class Normalization { + // L1-normalizes each descriptor followed by element-wise square rooting. + // This normalization is usually better than standard L2-normalization. + // See "Three things everyone should know to improve object retrieval", + // Relja Arandjelovic and Andrew Zisserman, CVPR 2012. + L1_ROOT, + // Each vector is L2-normalized. + L2, + }; + Normalization normalization = Normalization::L1_ROOT; + + bool Check() const; +}; + +struct SiftMatchingOptions { + // Number of threads for feature matching and geometric verification. + int num_threads = -1; + + // Whether to use the GPU for feature matching. + bool use_gpu = true; + + // Index of the GPU used for feature matching. For multi-GPU matching, + // you should separate multiple GPU indices by comma, e.g., "0,1,2,3". + std::string gpu_index = "-1"; + + // Maximum distance ratio between first and second best match. + double max_ratio = 0.8; + + // Maximum distance to best match. + double max_distance = 0.7; + + // Whether to enable cross checking in matching. + bool cross_check = true; + + // Maximum number of matches. + int max_num_matches = 32768; + + // Maximum epipolar error in pixels for geometric verification. + double max_error = 4.0; + + // Confidence threshold for geometric verification. + double confidence = 0.999; + + // Minimum/maximum number of RANSAC iterations. Note that this option + // overrules the min_inlier_ratio option. + int min_num_trials = 100; + int max_num_trials = 10000; + + // A priori assumed minimum inlier ratio, which determines the maximum + // number of iterations. + double min_inlier_ratio = 0.25; + + // Minimum number of inliers for an image pair to be considered as + // geometrically verified. + int min_num_inliers = 15; + + // Whether to attempt to estimate multiple geometric models per image pair. + bool multiple_models = false; + + // Whether to perform guided matching, if geometric verification succeeds. + bool guided_matching = false; + + // Force Homography use for Two-view Geometry (can help for planar scenes) + bool planar_scene = false; + + // Whether to estimate the relative pose between the two images and save them to the DB. + bool compute_relative_pose = false; + + bool Check() const; +}; + +// Extract SIFT features for the given image on the CPU. Only extract +// descriptors if the given input is not NULL. +bool ExtractSiftFeaturesCPU(const SiftExtractionOptions& options, + const Bitmap& bitmap, FeatureKeypoints* keypoints, + FeatureDescriptors* descriptors); +bool ExtractCovariantSiftFeaturesCPU(const SiftExtractionOptions& options, + const Bitmap& bitmap, + FeatureKeypoints* keypoints, + FeatureDescriptors* descriptors); + +// Create a SiftGPU feature extractor. The same SiftGPU instance can be used to +// extract features for multiple images. Note a OpenGL context must be made +// current in the thread of the caller. If the gpu_index is not -1, the CUDA +// version of SiftGPU is used, which produces slightly different results +// than the OpenGL implementation. +bool CreateSiftGPUExtractor(const SiftExtractionOptions& options, + SiftGPU* sift_gpu); + +// Extract SIFT features for the given image on the GPU. +// SiftGPU must already be initialized using `CreateSiftGPU`. +bool ExtractSiftFeaturesGPU(const SiftExtractionOptions& options, + const Bitmap& bitmap, SiftGPU* sift_gpu, + FeatureKeypoints* keypoints, + FeatureDescriptors* descriptors); + +// Load keypoints and descriptors from text file in the following format: +// +// LINE_0: NUM_FEATURES DIM +// LINE_1: X Y SCALE ORIENTATION D_1 D_2 D_3 ... D_DIM +// LINE_I: ... +// LINE_NUM_FEATURES: X Y SCALE ORIENTATION D_1 D_2 D_3 ... D_DIM +// +// where the first line specifies the number of features and the descriptor +// dimensionality followed by one line per feature: X, Y, SCALE, ORIENTATION are +// of type float and D_J represent the descriptor in the range [0, 255]. +// +// For example: +// +// 2 4 +// 0.32 0.12 1.23 1.0 1 2 3 4 +// 0.32 0.12 1.23 1.0 1 2 3 4 +// +void LoadSiftFeaturesFromTextFile(const std::string& path, + FeatureKeypoints* keypoints, + FeatureDescriptors* descriptors); + +// Match the given SIFT features on the CPU. +void MatchSiftFeaturesCPUBruteForce(const SiftMatchingOptions& match_options, + const FeatureDescriptors& descriptors1, + const FeatureDescriptors& descriptors2, + FeatureMatches* matches); +void MatchSiftFeaturesCPUFLANN(const SiftMatchingOptions& match_options, + const FeatureDescriptors& descriptors1, + const FeatureDescriptors& descriptors2, + FeatureMatches* matches); +void MatchSiftFeaturesCPU(const SiftMatchingOptions& match_options, + const FeatureDescriptors& descriptors1, + const FeatureDescriptors& descriptors2, + FeatureMatches* matches); +void MatchGuidedSiftFeaturesCPU(const SiftMatchingOptions& match_options, + const FeatureKeypoints& keypoints1, + const FeatureKeypoints& keypoints2, + const FeatureDescriptors& descriptors1, + const FeatureDescriptors& descriptors2, + TwoViewGeometry* two_view_geometry); + +// Create a SiftGPU feature matcher. Note that if CUDA is not available or the +// gpu_index is -1, the OpenGLContextManager must be created in the main thread +// of the Qt application before calling this function. The same SiftMatchGPU +// instance can be used to match features between multiple image pairs. +bool CreateSiftGPUMatcher(const SiftMatchingOptions& match_options, + SiftMatchGPU* sift_match_gpu); + +// Match the given SIFT features on the GPU. If either of the descriptors is +// NULL, the keypoints/descriptors will not be uploaded and the previously +// uploaded descriptors will be reused for the matching. +void MatchSiftFeaturesGPU(const SiftMatchingOptions& match_options, + const FeatureDescriptors* descriptors1, + const FeatureDescriptors* descriptors2, + SiftMatchGPU* sift_match_gpu, + FeatureMatches* matches); +void MatchGuidedSiftFeaturesGPU(const SiftMatchingOptions& match_options, + const FeatureKeypoints* keypoints1, + const FeatureKeypoints* keypoints2, + const FeatureDescriptors* descriptors1, + const FeatureDescriptors* descriptors2, + SiftMatchGPU* sift_match_gpu, + TwoViewGeometry* two_view_geometry); + +} // namespace colmap + +#endif // COLMAP_SRC_FEATURE_SIFT_H_ diff --git a/colmap/include/colmap/feature/types.h b/colmap/include/colmap/feature/types.h new file mode 100644 index 0000000000000000000000000000000000000000..1011f84ba0df3c45945ddba71e53320d49cb4cd7 --- /dev/null +++ b/colmap/include/colmap/feature/types.h @@ -0,0 +1,103 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_FEATURE_TYPES_H_ +#define COLMAP_SRC_FEATURE_TYPES_H_ + +#include + +#include + +#include "util/types.h" + +namespace colmap { + +struct FeatureKeypoint { + FeatureKeypoint(); + FeatureKeypoint(const float x, const float y); + FeatureKeypoint(const float x, const float y, const float scale, + const float orientation); + FeatureKeypoint(const float x, const float y, const float a11, + const float a12, const float a21, const float a22); + + static FeatureKeypoint FromParameters(const float x, const float y, + const float scale_x, + const float scale_y, + const float orientation, + const float shear); + + // Rescale the feature location and shape size by the given scale factor. + void Rescale(const float scale); + void Rescale(const float scale_x, const float scale_y); + + // Compute similarity shape parameters from affine shape. + float ComputeScale() const; + float ComputeScaleX() const; + float ComputeScaleY() const; + float ComputeOrientation() const; + float ComputeShear() const; + + // Location of the feature, with the origin at the upper left image corner, + // i.e. the upper left pixel has the coordinate (0.5, 0.5). + float x; + float y; + + // Affine shape of the feature. + float a11; + float a12; + float a21; + float a22; +}; + +typedef Eigen::Matrix + FeatureDescriptor; + +struct FeatureMatch { + FeatureMatch() + : point2D_idx1(kInvalidPoint2DIdx), point2D_idx2(kInvalidPoint2DIdx) {} + FeatureMatch(const point2D_t point2D_idx1, const point2D_t point2D_idx2) + : point2D_idx1(point2D_idx1), point2D_idx2(point2D_idx2) {} + + // Feature index in first image. + point2D_t point2D_idx1 = kInvalidPoint2DIdx; + + // Feature index in second image. + point2D_t point2D_idx2 = kInvalidPoint2DIdx; +}; + +typedef std::vector FeatureKeypoints; +typedef Eigen::Matrix + FeatureDescriptors; +typedef std::vector FeatureMatches; + +} // namespace colmap + +#endif // COLMAP_SRC_FEATURE_TYPES_H_ diff --git a/colmap/include/colmap/feature/utils.h b/colmap/include/colmap/feature/utils.h new file mode 100644 index 0000000000000000000000000000000000000000..671c597bebc7db3beb93f82a6f38d15d729daa80 --- /dev/null +++ b/colmap/include/colmap/feature/utils.h @@ -0,0 +1,67 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_FEATURE_UTILS_H_ +#define COLMAP_SRC_FEATURE_UTILS_H_ + +#include "feature/types.h" + +namespace colmap { + +// Convert feature keypoints to vector of points. +std::vector FeatureKeypointsToPointsVector( + const FeatureKeypoints& keypoints); + +// L2-normalize feature descriptor, where each row represents one feature. +Eigen::MatrixXf L2NormalizeFeatureDescriptors( + const Eigen::MatrixXf& descriptors); + +// L1-Root-normalize feature descriptors, where each row represents one feature. +// See "Three things everyone should know to improve object retrieval", +// Relja Arandjelovic and Andrew Zisserman, CVPR 2012. +Eigen::MatrixXf L1RootNormalizeFeatureDescriptors( + const Eigen::MatrixXf& descriptors); + +// Convert normalized floating point feature descriptor to unsigned byte +// representation by linear scaling from range [0, 0.5] to [0, 255]. Truncation +// to a maximum value of 0.5 is used to avoid precision loss and follows the +// common practice of representing SIFT vectors. +FeatureDescriptors FeatureDescriptorsToUnsignedByte( + const Eigen::MatrixXf& descriptors); + +// Extract the descriptors corresponding to the largest-scale features. +void ExtractTopScaleFeatures(FeatureKeypoints* keypoints, + FeatureDescriptors* descriptors, + const size_t num_features); + +} // namespace colmap + +#endif // COLMAP_SRC_FEATURE_UTILS_H_ diff --git a/colmap/include/colmap/lib/LSD/lsd.h b/colmap/include/colmap/lib/LSD/lsd.h new file mode 100644 index 0000000000000000000000000000000000000000..8b166fbed791442bc7d935fe9ba486fac7244bcb --- /dev/null +++ b/colmap/include/colmap/lib/LSD/lsd.h @@ -0,0 +1,281 @@ +/*---------------------------------------------------------------------------- + + LSD - Line Segment Detector on digital images + + This code is part of the following publication and was subject + to peer review: + + "LSD: a Line Segment Detector" by Rafael Grompone von Gioi, + Jeremie Jakubowicz, Jean-Michel Morel, and Gregory Randall, + Image Processing On Line, 2012. DOI:10.5201/ipol.2012.gjmr-lsd + http://dx.doi.org/10.5201/ipol.2012.gjmr-lsd + + Copyright (c) 2007-2011 rafael grompone von gioi + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + + ----------------------------------------------------------------------------*/ + +/*----------------------------------------------------------------------------*/ +/** @file lsd.h + LSD module header + @author rafael grompone von gioi + */ +/*----------------------------------------------------------------------------*/ +#ifndef LSD_HEADER +#define LSD_HEADER + +/*----------------------------------------------------------------------------*/ +/** LSD Full Interface + + @param n_out Pointer to an int where LSD will store the number of + line segments detected. + + @param img Pointer to input image data. It must be an array of + doubles of size X x Y, and the pixel at coordinates + (x,y) is obtained by img[x+y*X]. + + @param X X size of the image: the number of columns. + + @param Y Y size of the image: the number of rows. + + @param scale When different from 1.0, LSD will scale the input image + by 'scale' factor by Gaussian filtering, before detecting + line segments. + Example: if scale=0.8, the input image will be subsampled + to 80% of its size, before the line segment detector + is applied. + Suggested value: 0.8 + + @param sigma_scale When scale!=1.0, the sigma of the Gaussian filter is: + sigma = sigma_scale / scale, if scale < 1.0 + sigma = sigma_scale, if scale >= 1.0 + Suggested value: 0.6 + + @param quant Bound to the quantization error on the gradient norm. + Example: if gray levels are quantized to integer steps, + the gradient (computed by finite differences) error + due to quantization will be bounded by 2.0, as the + worst case is when the error are 1 and -1, that + gives an error of 2.0. + Suggested value: 2.0 + + @param ang_th Gradient angle tolerance in the region growing + algorithm, in degrees. + Suggested value: 22.5 + + @param log_eps Detection threshold, accept if -log10(NFA) > log_eps. + The larger the value, the more strict the detector is, + and will result in less detections. + (Note that the 'minus sign' makes that this + behavior is opposite to the one of NFA.) + The value -log10(NFA) is equivalent but more + intuitive than NFA: + - -1.0 gives an average of 10 false detections on noise + - 0.0 gives an average of 1 false detections on noise + - 1.0 gives an average of 0.1 false detections on nose + - 2.0 gives an average of 0.01 false detections on noise + . + Suggested value: 0.0 + + @param density_th Minimal proportion of 'supporting' points in a rectangle. + Suggested value: 0.7 + + @param n_bins Number of bins used in the pseudo-ordering of gradient + modulus. + Suggested value: 1024 + + @param reg_img Optional output: if desired, LSD will return an + int image where each pixel indicates the line segment + to which it belongs. Unused pixels have the value '0', + while the used ones have the number of the line segment, + numbered 1,2,3,..., in the same order as in the + output list. If desired, a non NULL int** pointer must + be assigned, and LSD will make that the pointer point + to an int array of size reg_x x reg_y, where the pixel + value at (x,y) is obtained with (*reg_img)[x+y*reg_x]. + Note that the resulting image has the size of the image + used for the processing, that is, the size of the input + image scaled by the given factor 'scale'. If scale!=1 + this size differs from XxY and that is the reason why + its value is given by reg_x and reg_y. + Suggested value: NULL + + @param reg_x Pointer to an int where LSD will put the X size + 'reg_img' image, when asked for. + Suggested value: NULL + + @param reg_y Pointer to an int where LSD will put the Y size + 'reg_img' image, when asked for. + Suggested value: NULL + + @return A double array of size 7 x n_out, containing the list + of line segments detected. The array contains first + 7 values of line segment number 1, then the 7 values + of line segment number 2, and so on, and it finish + by the 7 values of line segment number n_out. + The seven values are: + - x1,y1,x2,y2,width,p,-log10(NFA) + . + for a line segment from coordinates (x1,y1) to (x2,y2), + a width 'width', an angle precision of p in (0,1) given + by angle_tolerance/180 degree, and NFA value 'NFA'. + If 'out' is the returned pointer, the 7 values of + line segment number 'n+1' are obtained with + 'out[7*n+0]' to 'out[7*n+6]'. + */ +double * LineSegmentDetection( int * n_out, + double * img, int X, int Y, + double scale, double sigma_scale, double quant, + double ang_th, double log_eps, double density_th, + int n_bins, + int ** reg_img, int * reg_x, int * reg_y ); + +/*----------------------------------------------------------------------------*/ +/** LSD Simple Interface with Scale and Region output. + + @param n_out Pointer to an int where LSD will store the number of + line segments detected. + + @param img Pointer to input image data. It must be an array of + doubles of size X x Y, and the pixel at coordinates + (x,y) is obtained by img[x+y*X]. + + @param X X size of the image: the number of columns. + + @param Y Y size of the image: the number of rows. + + @param scale When different from 1.0, LSD will scale the input image + by 'scale' factor by Gaussian filtering, before detecting + line segments. + Example: if scale=0.8, the input image will be subsampled + to 80% of its size, before the line segment detector + is applied. + Suggested value: 0.8 + + @param reg_img Optional output: if desired, LSD will return an + int image where each pixel indicates the line segment + to which it belongs. Unused pixels have the value '0', + while the used ones have the number of the line segment, + numbered 1,2,3,..., in the same order as in the + output list. If desired, a non NULL int** pointer must + be assigned, and LSD will make that the pointer point + to an int array of size reg_x x reg_y, where the pixel + value at (x,y) is obtained with (*reg_img)[x+y*reg_x]. + Note that the resulting image has the size of the image + used for the processing, that is, the size of the input + image scaled by the given factor 'scale'. If scale!=1 + this size differs from XxY and that is the reason why + its value is given by reg_x and reg_y. + Suggested value: NULL + + @param reg_x Pointer to an int where LSD will put the X size + 'reg_img' image, when asked for. + Suggested value: NULL + + @param reg_y Pointer to an int where LSD will put the Y size + 'reg_img' image, when asked for. + Suggested value: NULL + + @return A double array of size 7 x n_out, containing the list + of line segments detected. The array contains first + 7 values of line segment number 1, then the 7 values + of line segment number 2, and so on, and it finish + by the 7 values of line segment number n_out. + The seven values are: + - x1,y1,x2,y2,width,p,-log10(NFA) + . + for a line segment from coordinates (x1,y1) to (x2,y2), + a width 'width', an angle precision of p in (0,1) given + by angle_tolerance/180 degree, and NFA value 'NFA'. + If 'out' is the returned pointer, the 7 values of + line segment number 'n+1' are obtained with + 'out[7*n+0]' to 'out[7*n+6]'. + */ +double * lsd_scale_region( int * n_out, + double * img, int X, int Y, double scale, + int ** reg_img, int * reg_x, int * reg_y ); + +/*----------------------------------------------------------------------------*/ +/** LSD Simple Interface with Scale + + @param n_out Pointer to an int where LSD will store the number of + line segments detected. + + @param img Pointer to input image data. It must be an array of + doubles of size X x Y, and the pixel at coordinates + (x,y) is obtained by img[x+y*X]. + + @param X X size of the image: the number of columns. + + @param Y Y size of the image: the number of rows. + + @param scale When different from 1.0, LSD will scale the input image + by 'scale' factor by Gaussian filtering, before detecting + line segments. + Example: if scale=0.8, the input image will be subsampled + to 80% of its size, before the line segment detector + is applied. + Suggested value: 0.8 + + @return A double array of size 7 x n_out, containing the list + of line segments detected. The array contains first + 7 values of line segment number 1, then the 7 values + of line segment number 2, and so on, and it finish + by the 7 values of line segment number n_out. + The seven values are: + - x1,y1,x2,y2,width,p,-log10(NFA) + . + for a line segment from coordinates (x1,y1) to (x2,y2), + a width 'width', an angle precision of p in (0,1) given + by angle_tolerance/180 degree, and NFA value 'NFA'. + If 'out' is the returned pointer, the 7 values of + line segment number 'n+1' are obtained with + 'out[7*n+0]' to 'out[7*n+6]'. + */ +double * lsd_scale(int * n_out, double * img, int X, int Y, double scale); + +/*----------------------------------------------------------------------------*/ +/** LSD Simple Interface + + @param n_out Pointer to an int where LSD will store the number of + line segments detected. + + @param img Pointer to input image data. It must be an array of + doubles of size X x Y, and the pixel at coordinates + (x,y) is obtained by img[x+y*X]. + + @param X X size of the image: the number of columns. + + @param Y Y size of the image: the number of rows. + + @return A double array of size 7 x n_out, containing the list + of line segments detected. The array contains first + 7 values of line segment number 1, then the 7 values + of line segment number 2, and so on, and it finish + by the 7 values of line segment number n_out. + The seven values are: + - x1,y1,x2,y2,width,p,-log10(NFA) + . + for a line segment from coordinates (x1,y1) to (x2,y2), + a width 'width', an angle precision of p in (0,1) given + by angle_tolerance/180 degree, and NFA value 'NFA'. + If 'out' is the returned pointer, the 7 values of + line segment number 'n+1' are obtained with + 'out[7*n+0]' to 'out[7*n+6]'. + */ +double * lsd(int * n_out, double * img, int X, int Y); + +#endif /* !LSD_HEADER */ +/*----------------------------------------------------------------------------*/ diff --git a/colmap/include/colmap/lib/PBA/ConfigBA.h b/colmap/include/colmap/lib/PBA/ConfigBA.h new file mode 100644 index 0000000000000000000000000000000000000000..74bd52439e75de4b95bf198ca61dd79a09f4949a --- /dev/null +++ b/colmap/include/colmap/lib/PBA/ConfigBA.h @@ -0,0 +1,226 @@ +//////////////////////////////////////////////////////////////////////////// +// File: ConfigBA.h +// Author: Changchang Wu (ccwu@cs.washington.edu) +// Description : configuration object class +// +// Copyright (c) 2011 Changchang Wu (ccwu@cs.washington.edu) +// and the University of Washington at Seattle +// +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public +// License as published by the Free Software Foundation; either +// Version 3 of the License, or (at your option) any later version. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef CONFIG_BA_H +#define CONFIG_BA_H +#include + +namespace pba { + +class ConfigBA { + protected: + enum { + TIMER_OVERALL = 0, + TIMER_OPTIMIZATION, + TIMER_GPU_ALLOCATION, + TIMER_GPU_UPLOAD, + TIMER_PREPROCESSING, + TIMER_GPU_DOWNLOAD, + TIMER_CG_ITERATION, + TIMER_LM_ITERATION, + TIMER_FUNCTION_JJ, + TIMER_FUNCTION_PJ, + TIMER_FUNCTION_DD, + TIMER_FUNCTION_JX, + TIMER_FUNCTION_JTE, + TIMER_FUNCTION_BC, + TIMER_FUNCTION_MP, + TIMER_FUNCTION_UP, + TIMER_PROFILE_STEP, + NUM_TIMER, + FUNC_JX = 0, + FUNC_JX_, + FUNC_JTEC_JCT, + FUNC_JTEC_JCO, + FUNC_JTEP, + FUNC_JTE_, + FUNC_JJ_JCO_JCT_JP, + FUNC_JJ_JCO_JP, + FUNC_JJ_JCT_JP, + FUNC_JJ_JP, + FUNC_PJ, + FUNC_BCC_JCT, + FUNC_BCC_JCO, + FUNC_BCP, + FUNC_MPC, + FUNC_MPP, + FUNC_VS, + FUNC_VV, + NUM_FUNC + }; + class TimerBA { + ConfigBA* _config; + int _timer; + + public: + TimerBA(ConfigBA* config, int timer) { + (_config = config)->BundleTimerStart(_timer = timer); + } + TimerBA(ConfigBA* config, int timer, bool) { + (_config = config)->BundleTimerSwitch(_timer = timer); + } + ~TimerBA() { _config->BundleTimerSwitch(_timer); } + }; + friend class TimerBA; + + public: + ////////////////////////////// + int __lm_max_iteration; //(default 50) + int __cg_max_iteration; //(default 100) + int __cg_min_iteration; //(default 10) + int __cg_recalculate_freq; //(default 0) + bool __accurate_gain_ratio; //(default true) accurate gain ratio for + //approximate solutions + + ////////////////////////////// + float __lm_delta_threshold; //(default 1e-6)|dx|_2, I use absolute (not + //relative) change + float __lm_gradient_threshold; //(default 1e-10)|Jt * e|_inf + float __lm_mse_threshold; //(default 0.25) quit if MSE is equal to or smaller + //than this + float __lm_initial_damp; //(default 0.001)initial damping factor + float __lm_minimum_damp; //(default 1e-10)minimum damping factor + float __lm_maximum_damp; + float __cg_norm_threshold; //(default 0.1)terminate CG if norm ratio is less + //than threshold + float __cg_norm_guard; //(default 1.0)abort cg when norm increases to + int __pba_experimental; + bool __cg_schur_complement; + + ////////////////////////////// + bool __lm_check_gradient; //(default false) check g_inf for convergence + float __lm_damping_auto_switch; + bool __lm_use_diagonal_damp; //(default true)use (Jt*J + lambda * diag(Jt*J)) + //= Jt * e + // or use (Jt*J + lambda * I) = Jt * e + bool __fixed_intrinsics; //(default false) set true for calibrated camera + //system + int __use_radial_distortion; //(default 0, 1 for projection distortion, 2 for + //measurement distortion) + bool __reset_initial_distortion; //(default false) reset the initial + //distortio to 0 + + //////////////////////////// + int __verbose_level; //(default 2) how many messages to print out + bool __abort_flag; //(default false)abort the bundle adjustment loop if set + //true + bool __verbose_cg_iteration; //(default false)print out details of Conjugate + //Gradients + bool __verbose_function_time; //(default false)print timing of some key + //functions + bool __save_gradient_norm; //(default false)save |Jt * e|_2 of each iteration + bool __verbose_allocation; //(default false)whether print out allocation + //details + bool __verbose_sse; //(default false) show mse or sse + + /////////////////////////////////// + bool __jc_store_transpose; //(default true) whether store transpose of JC + bool __no_jacobian_store; //(default false) whether use memory saving mode + bool __jc_store_original; //(default true) whether store original JC + + /////////////////////////////////// + bool __jacobian_normalize; //(default true) scaling the jacobians according + //to initial jacobians + bool __focal_normalize; //(default true) data normalization + bool __depth_normalize; //(default true) data normalization + bool __depth_degeneracy_fix; + float __data_normalize_median; + float __depth_check_epsilon; + ///////////////////////////// + + protected: + bool __multiply_jx_usenoj; // for debug purpose + protected: + ///////////////////////////// + int __selected_device; + int __cpu_data_precision; + int __bundle_time_budget; + int __bundle_mode_next; + int __bundle_current_mode; + ////////////////////////////// + float __initial_mse; + float __final_mse; + float __final_mse_x; + float __focal_scaling; + float __depth_scaling; + int __current_device; + int __current_iteration; + int __num_cg_iteration; + int __num_lm_success; + int __num_lm_iteration; + int __num_projection_eval; + int __num_jacobian_eval; + int __num_camera_modified; + int __num_point_behind; + int __pba_return_code; + int __recent_cg_status; + int __profile_pba; + bool __cpu_thread_profile; + bool __debug_pba; + bool __warmup_device; + size_t __memory_usage; + ///////////////////////////////////// + bool __matlab_format_stat; + char* __stat_filename; + const char* __driver_output; + std::vector __bundle_records; + double __timer_record[NUM_TIMER]; + int __num_cpu_thread_all; + int __num_cpu_thread[NUM_FUNC]; + + protected: + ConfigBA(); + /////////////////////////////// + void ResetTemporarySetting(); + void ResetBundleStatistics(); + void PrintBundleStatistics(); + void SaveBundleStatistics(int ncam, int npt, int nproj); + /////////////////////////////////////// + void BundleTimerStart(int timer); + void BundleTimerSwitch(int timer); + float BundleTimerGet(int timer); + void BundleTimerSwap(int timer1, int timer2); + float BundleTimerGetNow(int timer = TIMER_OPTIMIZATION); + ///////////////////////////////// + void SaveBundleRecord(int iter, float res, float damping, float gn, float gi); + bool IsTimeBudgetAvailable(); + double MyClock(); + + public: + void ParseParam(int argc, char** argv); + + public: + // the following are to be called after finishing BA + const char* GetOutputParam() { return __driver_output; } + float GetInitialMSE() { return __initial_mse; } + float GetFinalMSE() { return __final_mse; } + double GetBundleTiming(int timer = TIMER_OVERALL) { + return __timer_record[timer]; + } + int GetIterationsLM() { return __num_lm_iteration; } + int GetIterationsCG() { return __num_cg_iteration; } + int GetCurrentDevice() { return __current_device; } + int GetBundleReturnCode() { return __pba_return_code; } + int GetActiveDevice() { return __selected_device; } +}; + +} // namespace pba + +#endif diff --git a/colmap/include/colmap/lib/PBA/CuTexImage.h b/colmap/include/colmap/lib/PBA/CuTexImage.h new file mode 100644 index 0000000000000000000000000000000000000000..e53e566e7afdb0d63138e98dab598c14f9afd136 --- /dev/null +++ b/colmap/include/colmap/lib/PBA/CuTexImage.h @@ -0,0 +1,83 @@ +//////////////////////////////////////////////////////////////////////////// +// File: CuTexImage.h +// Author: Changchang Wu +// Description : interface for the CuTexImage class. +// class for storing data in CUDA. +// +// Copyright (c) 2011 Changchang Wu (ccwu@cs.washington.edu) +// and the University of Washington at Seattle +// +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public +// License as published by the Free Software Foundation; either +// Version 3 of the License, or (at your option) any later version. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef CU_TEX_IMAGE_H +#define CU_TEX_IMAGE_H + +struct textureReference; + +namespace pba { + +class CuTexImage { + protected: + bool _owner; + void* _cuData; + unsigned int _numChannel; + unsigned int _imgWidth; + unsigned int _imgHeight; + size_t _numBytes; + + public: + bool InitTexture(unsigned int width, unsigned int height, + unsigned int nchannel = 1); + void SetTexture(void* data, unsigned int width, unsigned int nchannel = 1); + void BindTexture(textureReference& texRef); + void BindTexture(textureReference& texRef, int offset, size_t size); + void BindTexture2(textureReference& texRef1, textureReference& texRef2); + void BindTexture4(textureReference& texRef1, textureReference& texRef2, + textureReference& texRef3, textureReference& texRef4); + int BindTextureX(textureReference& texRef1, textureReference& texRef2, + textureReference& texRef3, textureReference& texRef4, + bool force4); + void SwapData(CuTexImage& src); + void CopyToHost(void* buf); + void CopyFromDevice(const void* buf); + void CopyFromHost(const void* buf); + void SaveToFile(const char* name); + void ReleaseData(); + + public: + inline float* data() { return GetRequiredSize() ? ((float*)_cuData) : NULL; } + inline bool IsValid() { return _cuData != NULL && GetDataSize() > 0; } + inline unsigned int GetLength() { + return _imgWidth * _imgHeight * _numChannel; + } + inline unsigned int GetImgWidth() { return _imgWidth; } + inline unsigned int GetImgHeight() { return _imgHeight; } + inline size_t GetReservedWidth() { + return _numBytes == 0 + ? 0 + : (_numBytes / (_imgHeight * _numChannel * sizeof(float))); + } + inline size_t GetDataSize() { return _numBytes == 0 ? 0 : GetRequiredSize(); } + inline size_t GetRequiredSize() { + return sizeof(float) * _imgWidth * _imgHeight * _numChannel; + } + inline unsigned int IsHugeData() { return (GetLength() - 1) / (1 << 27); } + + public: + CuTexImage(); + virtual ~CuTexImage(); +}; + +} // namespace pba + +#endif // !defined(CU_TEX_IMAGE_H) diff --git a/colmap/include/colmap/lib/PBA/DataInterface.h b/colmap/include/colmap/lib/PBA/DataInterface.h new file mode 100644 index 0000000000000000000000000000000000000000..b465bd60a877ad13f461b794b142bc87192c6b5a --- /dev/null +++ b/colmap/include/colmap/lib/PBA/DataInterface.h @@ -0,0 +1,423 @@ +//////////////////////////////////////////////////////////////////////////// +// File: DataInterface.h +// Author: Changchang Wu (ccwu@cs.washington.edu) +// Description : data interface, the data format been uploaded to GPU +// +// Copyright (c) 2011 Changchang Wu (ccwu@cs.washington.edu) +// and the University of Washington at Seattle +// +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public +// License as published by the Free Software Foundation; either +// Version 3 of the License, or (at your option) any later version. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DATA_INTERFACE_GPU_H +#define DATA_INTERFACE_GPU_H + +#include + +// ----------------------------WARNING------------------------------ +// ----------------------------------------------------------------- +// ROTATION CONVERSION: +// The internal rotation representation is 3x3 float matrix. Reading +// back the rotations as quaternion or Rodrigues's representation will +// cause inaccuracy, IF you have wrongly reconstructed cameras with +// a very very large focal length (typically also very far away). +// In this case, any small change in the rotation matrix, will cause +// a large reprojection error. +// +// --------------------------------------------------------------------- +// RADIAL distortion is NOT enabled by default, use parameter "-md", -pd" +// or set ConfigBA::__use_radial_distortion to 1 or -1 to enable it. +// --------------------------------------------------------------------------- + +namespace pba { + +// transfer data type with 4-float alignment +#define CameraT CameraT_ +#define Point3D Point3D_ +template + +struct CameraT_ { + typedef FT float_t; + ////////////////////////////////////////////////////// + float_t f; // single focal length, K = [f, 0, 0; 0 f 0; 0 0 1] + float_t t[3]; // T in P = K[R T], T = - RC + float_t m[3][3]; // R in P = K[R T]. + float_t radial; // WARNING: BE careful with the radial distortion model. + int distortion_type; + float_t constant_camera; + + ////////////////////////////////////////////////////////// + CameraT_() { + radial = 0; + distortion_type = 0; + constant_camera = 0; + } + + ////////////////////////////////////////////// + template + void SetCameraT(const CameraX& cam) { + f = (float_t)cam.f; + t[0] = (float_t)cam.t[0]; + t[1] = (float_t)cam.t[1]; + t[2] = (float_t)cam.t[2]; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) m[i][j] = (float_t)cam.m[i][j]; + radial = (float_t)cam.radial; + distortion_type = cam.distortion_type; + constant_camera = cam.constant_camera; + } + + ////////////////////////////////////////// + void SetConstantCamera() { constant_camera = 1.0f; } + void SetVariableCamera() { constant_camera = 0.0f; } + void SetFixedIntrinsic() { constant_camera = 2.0f; } + // void SetFixedExtrinsic() {constant_camera = 3.0f;} + + ////////////////////////////////////// + template + void SetFocalLength(Float F) { + f = (float_t)F; + } + float_t GetFocalLength() const { return f; } + + template + void SetMeasurementDistortion(Float r) { + radial = (float_t)r; + distortion_type = -1; + } + float_t GetMeasurementDistortion() const { + return distortion_type == -1 ? radial : 0; + } + + // normalize radial distortion that applies to angle will be (radial * f * f); + template + void SetNormalizedMeasurementDistortion(Float r) { + SetMeasurementDistortion(r / (f * f)); + } + float_t GetNormalizedMeasurementDistortion() const { + return GetMeasurementDistortion() * (f * f); + } + + // use projection distortion + template + void SetProjectionDistortion(Float r) { + radial = float_t(r); + distortion_type = 1; + } + template + void SetProjectionDistortion(const Float* r) { + SetProjectionDistortion(r[0]); + } + float_t GetProjectionDistortion() const { + return distortion_type == 1 ? radial : 0; + } + + template + void SetRodriguesRotation(const Float r[3]) { + double a = sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]); + double ct = a == 0.0 ? 0.5 : (1.0 - cos(a)) / a / a; + double st = a == 0.0 ? 1 : sin(a) / a; + m[0][0] = float_t(1.0 - (r[1] * r[1] + r[2] * r[2]) * ct); + m[0][1] = float_t(r[0] * r[1] * ct - r[2] * st); + m[0][2] = float_t(r[2] * r[0] * ct + r[1] * st); + m[1][0] = float_t(r[0] * r[1] * ct + r[2] * st); + m[1][1] = float_t(1.0 - (r[2] * r[2] + r[0] * r[0]) * ct); + m[1][2] = float_t(r[1] * r[2] * ct - r[0] * st); + m[2][0] = float_t(r[2] * r[0] * ct - r[1] * st); + m[2][1] = float_t(r[1] * r[2] * ct + r[0] * st); + m[2][2] = float_t(1.0 - (r[0] * r[0] + r[1] * r[1]) * ct); + } + template + void GetRodriguesRotation(Float r[3]) const { + double a = (m[0][0] + m[1][1] + m[2][2] - 1.0) / 2.0; + const double epsilon = 0.01; + if (fabs(m[0][1] - m[1][0]) < epsilon && + fabs(m[1][2] - m[2][1]) < epsilon && + fabs(m[0][2] - m[2][0]) < epsilon) { + if (fabs(m[0][1] + m[1][0]) < 0.1 && fabs(m[1][2] + m[2][1]) < 0.1 && + fabs(m[0][2] + m[2][0]) < 0.1 && a > 0.9) { + r[0] = 0; + r[1] = 0; + r[2] = 0; + } else { + const Float ha = Float(sqrt(0.5) * 3.14159265358979323846); + double xx = (m[0][0] + 1.0) / 2.0; + double yy = (m[1][1] + 1.0) / 2.0; + double zz = (m[2][2] + 1.0) / 2.0; + double xy = (m[0][1] + m[1][0]) / 4.0; + double xz = (m[0][2] + m[2][0]) / 4.0; + double yz = (m[1][2] + m[2][1]) / 4.0; + + if ((xx > yy) && (xx > zz)) { + if (xx < epsilon) { + r[0] = 0; + r[1] = r[2] = ha; + } else { + double t = sqrt(xx); + r[0] = Float(t * 3.14159265358979323846); + r[1] = Float(xy / t * 3.14159265358979323846); + r[2] = Float(xz / t * 3.14159265358979323846); + } + } else if (yy > zz) { + if (yy < epsilon) { + r[0] = r[2] = ha; + r[1] = 0; + } else { + double t = sqrt(yy); + r[0] = Float(xy / t * 3.14159265358979323846); + r[1] = Float(t * 3.14159265358979323846); + r[2] = Float(yz / t * 3.14159265358979323846); + } + } else { + if (zz < epsilon) { + r[0] = r[1] = ha; + r[2] = 0; + } else { + double t = sqrt(zz); + r[0] = Float(xz / t * 3.14159265358979323846); + r[1] = Float(yz / t * 3.14159265358979323846); + r[2] = Float(t * 3.14159265358979323846); + } + } + } + } else { + a = acos(a); + double b = 0.5 * a / sin(a); + r[0] = Float(b * (m[2][1] - m[1][2])); + r[1] = Float(b * (m[0][2] - m[2][0])); + r[2] = Float(b * (m[1][0] - m[0][1])); + } + } + //////////////////////// + template + void SetQuaternionRotation(const Float q[4]) { + double qq = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + double qw, qx, qy, qz; + if (qq > 0) { + qw = q[0] / qq; + qx = q[1] / qq; + qy = q[2] / qq; + qz = q[3] / qq; + } else { + qw = 1; + qx = qy = qz = 0; + } + m[0][0] = float_t(qw * qw + qx * qx - qz * qz - qy * qy); + m[0][1] = float_t(2 * qx * qy - 2 * qz * qw); + m[0][2] = float_t(2 * qy * qw + 2 * qz * qx); + m[1][0] = float_t(2 * qx * qy + 2 * qw * qz); + m[1][1] = float_t(qy * qy + qw * qw - qz * qz - qx * qx); + m[1][2] = float_t(2 * qz * qy - 2 * qx * qw); + m[2][0] = float_t(2 * qx * qz - 2 * qy * qw); + m[2][1] = float_t(2 * qy * qz + 2 * qw * qx); + m[2][2] = float_t(qz * qz + qw * qw - qy * qy - qx * qx); + } + template + void GetQuaternionRotation(Float q[4]) const { + q[0] = 1 + m[0][0] + m[1][1] + m[2][2]; + if (q[0] > 0.000000001) { + q[0] = sqrt(q[0]) / 2.0; + q[1] = (m[2][1] - m[1][2]) / (4.0 * q[0]); + q[2] = (m[0][2] - m[2][0]) / (4.0 * q[0]); + q[3] = (m[1][0] - m[0][1]) / (4.0 * q[0]); + } else { + double s; + if (m[0][0] > m[1][1] && m[0][0] > m[2][2]) { + s = 2.0 * sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]); + q[1] = 0.25 * s; + q[2] = (m[0][1] + m[1][0]) / s; + q[3] = (m[0][2] + m[2][0]) / s; + q[0] = (m[1][2] - m[2][1]) / s; + } else if (m[1][1] > m[2][2]) { + s = 2.0 * sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]); + q[1] = (m[0][1] + m[1][0]) / s; + q[2] = 0.25 * s; + q[3] = (m[1][2] + m[2][1]) / s; + q[0] = (m[0][2] - m[2][0]) / s; + } else { + s = 2.0 * sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]); + q[1] = (m[0][2] + m[2][0]) / s; + q[2] = (m[1][2] + m[2][1]) / s; + q[3] = 0.25f * s; + q[0] = (m[0][1] - m[1][0]) / s; + } + } + } + //////////////////////////////////////////////// + template + void SetMatrixRotation(const Float* r) { + int k = 0; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + m[i][j] = float_t(r[k++]); + } + } + } + template + void GetMatrixRotation(Float* r) const { + int k = 0; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + r[k++] = Float(m[i][j]); + } + } + } + float GetRotationMatrixDeterminant() const { + return m[0][0] * m[1][1] * m[2][2] + m[0][1] * m[1][2] * m[2][0] + + m[0][2] * m[1][0] * m[2][1] - m[0][2] * m[1][1] * m[2][0] - + m[0][1] * m[1][0] * m[2][2] - m[0][0] * m[1][2] * m[2][1]; + } + /////////////////////////////////////// + template + void SetTranslation(const Float T[3]) { + t[0] = (float_t)T[0]; + t[1] = (float_t)T[1]; + t[2] = (float_t)T[2]; + } + template + void GetTranslation(Float T[3]) const { + T[0] = (Float)t[0]; + T[1] = (Float)t[1]; + T[2] = (Float)t[2]; + } + ///////////////////////////////////////////// + template + void SetCameraCenterAfterRotation(const Float c[3]) { + // t = - R * C + for (int j = 0; j < 3; ++j) + t[j] = -float_t(m[j][0] * c[0] + m[j][1] * c[1] + m[j][2] * c[2]); + } + template + void GetCameraCenter(Float c[3]) { + // C = - R' * t + for (int j = 0; j < 3; ++j) + c[j] = -float_t(m[0][j] * t[0] + m[1][j] * t[1] + m[2][j] * t[2]); + } + //////////////////////////////////////////// + template + void SetInvertedRT(const Float e[3], const Float T[3]) { + SetRodriguesRotation(e); + for (int i = 3; i < 9; ++i) m[0][i] = -m[0][i]; + SetTranslation(T); + t[1] = -t[1]; + t[2] = -t[2]; + } + + template + void GetInvertedRT(Float e[3], Float T[3]) const { + CameraT ci; + ci.SetMatrixRotation(m[0]); + for (int i = 3; i < 9; ++i) ci.m[0][i] = -ci.m[0][i]; + // for(int i = 1; i < 3; ++i) for(int j = 0; j < 3; ++j) ci.m[i][j] = - + // ci.m[i][j]; + ci.GetRodriguesRotation(e); + GetTranslation(T); + T[1] = -T[1]; + T[2] = -T[2]; + } + template + void SetInvertedR9T(const Float e[9], const Float T[3]) { + // for(int i = 0; i < 9; ++i) m[0][i] = (i < 3 ? e[i] : - e[i]); + // SetTranslation(T); t[1] = - t[1]; t[2] = -t[2]; + m[0][0] = e[0]; + m[0][1] = e[1]; + m[0][2] = e[2]; + m[1][0] = -e[3]; + m[1][1] = -e[4]; + m[1][2] = -e[5]; + m[2][0] = -e[6]; + m[2][1] = -e[7]; + m[2][2] = -e[8]; + t[0] = T[0]; + t[1] = -T[1]; + t[2] = -T[2]; + } + template + void GetInvertedR9T(Float e[9], Float T[3]) const { + e[0] = m[0][0]; + e[1] = m[0][1]; + e[2] = m[0][2]; + e[3] = -m[1][0]; + e[4] = -m[1][1]; + e[5] = -m[1][2]; + e[6] = -m[2][0]; + e[7] = -m[2][1]; + e[8] = -m[2][2]; + T[0] = t[0]; + T[1] = -t[1]; + T[2] = -t[2]; + } +}; + +template +struct Point3D { + typedef FT float_t; + float_t xyz[3]; // 3D point location + float_t reserved; // alignment + //////////////////////////////// + template + void SetPoint(Float x, Float y, Float z) { + xyz[0] = (float_t)x; + xyz[1] = (float_t)y; + xyz[2] = (float_t)z; + reserved = 0; + } + template + void SetPoint(const Float* p) { + xyz[0] = (float_t)p[0]; + xyz[1] = (float_t)p[1]; + xyz[2] = (float_t)p[2]; + reserved = 0; + } + template + void GetPoint(Float* p) const { + p[0] = (Float)xyz[0]; + p[1] = (Float)xyz[1]; + p[2] = (Float)xyz[2]; + } + template + void GetPoint(Float& x, Float& y, Float& z) const { + x = (Float)xyz[0]; + y = (Float)xyz[1]; + z = (Float)xyz[2]; + } +}; + +#undef CameraT +#undef Point3D + +typedef CameraT_ CameraT; +typedef Point3D_ Point3D; + +struct Point2D { + float x, y; + //////////////////////////////////////////////////////// + Point2D() {} + template + Point2D(Float X, Float Y) { + SetPoint2D(X, Y); + } + template + void SetPoint2D(Float X, Float Y) { + x = (float)X; + y = (float)Y; + } + template + void GetPoint2D(Float& X, Float& Y) const { + X = (Float)x; + Y = (Float)y; + } +}; + +} // namespace pba + +#endif diff --git a/colmap/include/colmap/lib/PBA/ProgramCU.h b/colmap/include/colmap/lib/PBA/ProgramCU.h new file mode 100644 index 0000000000000000000000000000000000000000..d3d8af60944e6296667a27710fb76d89cb890500 --- /dev/null +++ b/colmap/include/colmap/lib/PBA/ProgramCU.h @@ -0,0 +1,127 @@ +//////////////////////////////////////////////////////////////////////////// +// File: ProgramCU.h +// Author: Changchang Wu +// Description : interface for the ProgramCU classes. +// It is basically a wrapper around all the CUDA kernels +// +// Copyright (c) 2011 Changchang Wu (ccwu@cs.washington.edu) +// and the University of Washington at Seattle +// +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public +// License as published by the Free Software Foundation; either +// Version 3 of the License, or (at your option) any later version. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef _PROGRAM_CU_H +#define _PROGRAM_CU_H + +class CuTexImage; + +namespace pba { +namespace ProgramCU { + +int SetCudaDevice(int device); +size_t GetCudaMemoryCap(); +int CheckErrorCUDA(const char* location); +void FinishWorkCUDA(); +void ClearPreviousError(); +void ResetCurrentDevice(); +void GetBlockConfiguration(unsigned int nblock, unsigned int& bw, + unsigned int& bh); + +////////////////////////////////////////////////////////// +void ComputeSQRT(CuTexImage& tex); +void ComputeRSQRT(CuTexImage& tex); +void ComputeVXY(CuTexImage& texX, CuTexImage& texY, CuTexImage& result, + unsigned int part = 0, unsigned int skip = 0); +void ComputeSAXPY(float a, CuTexImage& texX, CuTexImage& texY, + CuTexImage& result); +void ComputeSAX(float a, CuTexImage& texX, CuTexImage& result); +void ComputeSXYPZ(float a, CuTexImage& texX, CuTexImage& texY, CuTexImage& texZ, + CuTexImage& result); +float ComputeVectorMax(CuTexImage& vector, CuTexImage& buf); +float ComputeVectorSum(CuTexImage& vector, CuTexImage& buf, int skip); +double ComputeVectorNorm(CuTexImage& vector, CuTexImage& buf); +double ComputeVectorNormW(CuTexImage& vector, CuTexImage& weight, + CuTexImage& buf); +double ComputeVectorDot(CuTexImage& vector1, CuTexImage& vector2, + CuTexImage& buf); + +////////////////////////////////////////////////////////////////////////// +void UncompressCamera(int ncam, CuTexImage& camera0, CuTexImage& result); +void CompressCamera(int ncam, CuTexImage& camera0, CuTexImage& result); +void UpdateCameraPoint(int ncam, CuTexImage& camera, CuTexImage& point, + CuTexImage& delta, CuTexImage& new_camera, + CuTexImage& new_point, int mode = 0); + +///////////////////////////////////////////////////////////////////////// +void ComputeJacobian(CuTexImage& camera, CuTexImage& point, CuTexImage& jc, + CuTexImage& jp, CuTexImage& proj_map, CuTexImage& sj, + CuTexImage& meas, CuTexImage& cmlist, bool intrinsic_fixed, + int radial_distortion, bool shuffle); +void ComputeProjection(CuTexImage& camera, CuTexImage& point, CuTexImage& meas, + CuTexImage& proj_map, CuTexImage& proj, int radial); +void ComputeProjectionX(CuTexImage& camera, CuTexImage& point, CuTexImage& meas, + CuTexImage& proj_map, CuTexImage& proj, int radial); + +bool ShuffleCameraJacobian(CuTexImage& jc, CuTexImage& map, CuTexImage& result); + +///////////////////////////////////////////////////////////// +void ComputeDiagonal(CuTexImage& jc, CuTexImage& cmap, CuTexImage& jp, + CuTexImage& pmap, CuTexImage& cmlist, CuTexImage& jtjd, + CuTexImage& jtjdi, bool jc_transpose, int radial, + bool add_existing_diagc); +void MultiplyBlockConditioner(int ncam, int npoint, CuTexImage& blocks, + CuTexImage& vector, CuTexImage& result, + int radial, int mode = 0); + +//////////////////////////////////////////////////////////////////////////////// +void ComputeProjectionQ(CuTexImage& camera, CuTexImage& qmap, CuTexImage& qw, + CuTexImage& proj, int offset); +void ComputeJQX(CuTexImage& x, CuTexImage& qmap, CuTexImage& wq, CuTexImage& sj, + CuTexImage& jx, int offset); +void ComputeJQtEC(CuTexImage& pe, CuTexImage& qlist, CuTexImage& wq, + CuTexImage& sj, CuTexImage& result); +void ComputeDiagonalQ(CuTexImage& qlistw, CuTexImage& sj, CuTexImage& diag); + +////////////////////////////////////////////////////////////////////////// +void ComputeJX(int point_offset, CuTexImage& x, CuTexImage& jc, CuTexImage& jp, + CuTexImage& jmap, CuTexImage& result, int mode = 0); +void ComputeJtE(CuTexImage& pe, CuTexImage& jc, CuTexImage& cmap, + CuTexImage& cmlist, CuTexImage& jp, CuTexImage& pmap, + CuTexImage& jte, bool jc_transpose, int mode = 0); +void ComputeDiagonalBlock(float lambda, bool dampd, CuTexImage& jc, + CuTexImage& cmap, CuTexImage& jp, CuTexImage& pmap, + CuTexImage& cmlist, CuTexImage& diag, + CuTexImage& blocks, int radial_distortion, + bool jc_transpose, bool add_existing_diagc, + int mode = 0); + +///////////////////////////////////////////////////////////////////// +void ComputeJX_(CuTexImage& x, CuTexImage& jx, CuTexImage& camera, + CuTexImage& point, CuTexImage& meas, CuTexImage& pjmap, + bool intrinsic_fixed, int radial_distortion, int mode = 0); +void ComputeJtE_(CuTexImage& e, CuTexImage& jte, CuTexImage& camera, + CuTexImage& point, CuTexImage& meas, CuTexImage& cmap, + CuTexImage& cmlist, CuTexImage& pmap, CuTexImage& jmap, + CuTexImage& jp, bool intrinsic_fixed, int radial_distortion, + int mode = 0); +void ComputeDiagonalBlock_(float lambda, bool dampd, CuTexImage& camera, + CuTexImage& point, CuTexImage& meas, + CuTexImage& cmap, CuTexImage& cmlist, + CuTexImage& pmap, CuTexImage& jmap, CuTexImage& jp, + CuTexImage& sj, CuTexImage& diag, CuTexImage& blocks, + bool intrinsic_fixed, int radial_distortion, + bool add_existing_diagc, int mode = 0); + +} // namespace ProgramCU +} // namespace pba + +#endif diff --git a/colmap/include/colmap/lib/PBA/SparseBundleCPU.h b/colmap/include/colmap/lib/PBA/SparseBundleCPU.h new file mode 100644 index 0000000000000000000000000000000000000000..73beb9e10c4e5a8cc71c8dc987ad4a6eff74a20e --- /dev/null +++ b/colmap/include/colmap/lib/PBA/SparseBundleCPU.h @@ -0,0 +1,286 @@ +//////////////////////////////////////////////////////////////////////////// +// File: SparseBundleCPU.h +// Author: Changchang Wu (ccwu@cs.washington.edu) +// Description : interface of the CPU-version of multi-core bundle adjustment +// +// Copyright (c) 2011 Changchang Wu (ccwu@cs.washington.edu) +// and the University of Washington at Seattle +// +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public +// License as published by the Free Software Foundation; either +// Version 3 of the License, or (at your option) any later version. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +//////////////////////////////////////////////////////////////////////////////// + +#if !defined(SPARSE_BUNDLE_CPU_H) +#define SPARSE_BUNDLE_CPU_H + +// BYTE-ALIGNMENT for data allocation (16 required for SSE, 32 required for AVX) +// PREVIOUS version uses only SSE. The new version will include AVX. +// SO the alignment is increased from 16 to 32 +#define VECTOR_ALIGNMENT 32 +#define FLOAT_ALIGN 8 +#define VECTOR_ALIGNMENT_MASK (VECTOR_ALIGNMENT - 1) +#define ALIGN_PTR(p) \ + ((((size_t)p) + VECTOR_ALIGNMENT_MASK) & (~VECTOR_ALIGNMENT_MASK)) + +namespace pba { + +template +class avec { + bool _owner; + Float* _data; + Float* _last; + size_t _size; + size_t _capacity; + + public: + static Float* allocate(size_t count) { + size_t size = count * sizeof(Float); +#ifdef _MSC_VER + Float* p = (Float*)_aligned_malloc(size, VECTOR_ALIGNMENT); + if (p == NULL) throw std::bad_alloc(); + return p; +#else + char* p = (char*)malloc(size + VECTOR_ALIGNMENT + 4); + if (p == NULL) throw std::bad_alloc(); + char* p1 = p + 1; + char* p2 = + (char*)ALIGN_PTR(p1); //(char*) (((((size_t)p1) + 15) >> 4) << 4); + char* p3 = (p2 - 1); + p3[0] = (p2 - p); + return (Float*)p2; +#endif + } + static void deallocate(void* p) { +#ifdef _MSC_VER + _aligned_free(p); +#else + char* p3 = ((char*)p) - 1; + free(((char*)p) - p3[0]); +#endif + } + + public: + avec() { + _owner = true; + _last = _data = NULL; + _size = _capacity = 0; + } + avec(size_t count) { + _data = allocate(count); + _size = _capacity = count; + _last = _data + count; + _owner = true; + } + ~avec() { + if (_data && _owner) deallocate(_data); + } + + inline void resize(size_t newcount) { + if (!_owner) { + _data = _last = NULL; + _capacity = _size = 0; + _owner = true; + } + if (newcount <= _capacity) { + _size = newcount; + _last = _data + newcount; + } else { + if (_data && _owner) deallocate(_data); + _data = allocate(newcount); + _size = _capacity = newcount; + _last = _data + newcount; + } + } + + inline void set(Float* data, size_t count) { + if (_data && _owner) deallocate(_data); + _data = data; + _owner = false; + _size = count; + _last = _data + _size; + _capacity = count; + } + inline void swap(avec& next) { + bool _owner_bak = _owner; + Float* _data_bak = _data; + Float* _last_bak = _last; + size_t _size_bak = _size; + size_t _capa_bak = _capacity; + + _owner = next._owner; + _data = next._data; + _last = next._last; + _size = next._size; + _capacity = next._capacity; + + next._owner = _owner_bak; + next._data = _data_bak; + next._last = _last_bak; + next._size = _size_bak; + next._capacity = _capa_bak; + } + + inline operator Float*() { return _size ? _data : NULL; } + inline operator Float* const() const { return _data; } + inline Float* begin() { return _size ? _data : NULL; } + inline Float* data() { return _size ? _data : NULL; } + inline Float* end() { return _last; } + inline const Float* begin() const { return _size ? _data : NULL; } + inline const Float* end() const { return _last; } + inline size_t size() const { return _size; } + inline size_t IsValid() const { return _size; } + void SaveToFile(const char* name); +}; + +template +class SparseBundleCPU : public ParallelBA, public ConfigBA { + public: + SparseBundleCPU(const int num_threads); + + typedef avec VectorF; + typedef std::vector VectorI; + typedef float float_t; + + protected: // cpu data + int _num_camera; + int _num_point; + int _num_imgpt; + CameraT* _camera_data; + float* _point_data; + + //////////////////////////////// + const float* _imgpt_data; + const int* _camera_idx; + const int* _point_idx; + const int* _focal_mask; + + ///////////sumed square error + float _projection_sse; + + protected: // cuda data + VectorF _cuCameraData; + VectorF _cuCameraDataEX; + VectorF _cuPointData; + VectorF _cuPointDataEX; + VectorF _cuMeasurements; + VectorF _cuImageProj; + VectorF _cuJacobianCamera; + VectorF _cuJacobianPoint; + VectorF _cuJacobianCameraT; + VectorI _cuProjectionMap; + VectorI _cuPointMeasurementMap; + VectorI _cuCameraMeasurementMap; + VectorI _cuCameraMeasurementList; + VectorI _cuCameraMeasurementListT; + + ////////////////////////// + VectorF _cuBlockPC; + VectorF _cuVectorSJ; + + /// LM normal equation + VectorF _cuVectorJtE; + VectorF _cuVectorJJ; + VectorF _cuVectorJX; + VectorF _cuVectorXK; + VectorF _cuVectorPK; + VectorF _cuVectorZK; + VectorF _cuVectorRK; + + ////////////////////////////////// + protected: + int _num_imgpt_q; + float _weight_q; + VectorI _cuCameraQList; + VectorI _cuCameraQMap; + VectorF _cuCameraQMapW; + VectorF _cuCameraQListW; + + protected: + bool ProcessIndexCameraQ(std::vector& qmap, std::vector& qlist); + void ProcessWeightCameraQ(std::vector& cpnum, std::vector& qmap, + Float* qmapw, Float* qlistw); + + protected: // internal functions + int ValidateInputData(); + int InitializeBundle(); + int GetParameterLength(); + void BundleAdjustment(); + void NormalizeData(); + void TransferDataToHost(); + void DenormalizeData(); + void NormalizeDataF(); + void NormalizeDataD(); + bool InitializeStorageForSFM(); + bool InitializeStorageForCG(); + + void SaveBundleRecord(int iter, float res, float damping, float& g_norm, + float& g_inf); + + protected: + void PrepareJacobianNormalization(); + void EvaluateJacobians(); + void ComputeJtE(VectorF& E, VectorF& JtE, int mode = 0); + void ComputeJX(VectorF& X, VectorF& JX, int mode = 0); + void ComputeDiagonal(VectorF& JJI); + void ComputeBlockPC(float lambda, bool dampd); + void ApplyBlockPC(VectorF& v, VectorF& pv, int mode = 0); + float UpdateCameraPoint(VectorF& dx, VectorF& cuImageTempProj); + float EvaluateProjection(VectorF& cam, VectorF& point, VectorF& proj); + float EvaluateProjectionX(VectorF& cam, VectorF& point, VectorF& proj); + float SaveUpdatedSystem(float residual_reduction, float dx_sqnorm, + float damping); + float EvaluateDeltaNorm(); + int SolveNormalEquationPCGB(float lambda); + int SolveNormalEquationPCGX(float lambda); + int SolveNormalEquation(float lambda); + void NonlinearOptimizeLM(); + void AdjustBundleAdjsutmentMode(); + void RunProfileSteps(); + void RunTestIterationLM(bool reduced); + void DumpCooJacobian(); + + private: + static int FindProcessorCoreNum(); + + public: + virtual void AbortBundleAdjustment() { __abort_flag = true; } + virtual int GetCurrentIteration() { return __current_iteration; } + virtual void SetNextTimeBudget(int seconds) { + __bundle_time_budget = seconds; + } + virtual void SetNextBundleMode(BundleModeT mode) { + __bundle_mode_next = mode; + } + virtual void SetFixedIntrinsics(bool fixed) { __fixed_intrinsics = fixed; } + virtual void EnableRadialDistortion(DistortionT type) { + __use_radial_distortion = type; + } + virtual void ParseParam(int narg, char** argv) { + ConfigBA::ParseParam(narg, argv); + } + virtual ConfigBA* GetInternalConfig() { return this; } + + public: + SparseBundleCPU(); + virtual void SetCameraData(size_t ncam, CameraT* cams); + virtual void SetPointData(size_t npoint, Point3D* pts); + virtual void SetProjection(size_t nproj, const Point2D* imgpts, + const int* point_idx, const int* cam_idx); + virtual void SetFocalMask(const int* fmask, float weight); + virtual float GetMeanSquaredError(); + virtual int RunBundleAdjustment(); +}; + +ParallelBA* NewSparseBundleCPU(bool dp, const int num_threads); + +} // namespace pba + +#endif diff --git a/colmap/include/colmap/lib/PBA/SparseBundleCU.h b/colmap/include/colmap/lib/PBA/SparseBundleCU.h new file mode 100644 index 0000000000000000000000000000000000000000..7183deb677fa5d548cf01d91e179f40310ea3f27 --- /dev/null +++ b/colmap/include/colmap/lib/PBA/SparseBundleCU.h @@ -0,0 +1,176 @@ +//////////////////////////////////////////////////////////////////////////// +// File: SparseBundleCU.h +// Author: Changchang Wu (ccwu@cs.washington.edu) +// Description : interface of the CUDA-version of multicore bundle +// adjustment +// +// Copyright (c) 2011 Changchang Wu (ccwu@cs.washington.edu) +// and the University of Washington at Seattle +// +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public +// License as published by the Free Software Foundation; either +// Version 3 of the License, or (at your option) any later version. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +//////////////////////////////////////////////////////////////////////////////// + +#if !defined(SPARSE_BUNDLE_CU_H) +#define SPARSE_BUNDLE_CU_H + +#include "ConfigBA.h" +#include "CuTexImage.h" +#include "DataInterface.h" + +namespace pba { + +class SparseBundleCU : public ParallelBA, public ConfigBA { + protected: // cpu data + int _num_camera; + int _num_point; + int _num_imgpt; + CameraT* _camera_data; + float* _point_data; + //////////////////////////////// + const float* _imgpt_data; + const int* _camera_idx; + const int* _point_idx; + const int* _focal_mask; + std::vector _imgpt_datax; + //////////////////////// + float _projection_sse; // sumed square error + protected: // cuda data + CuTexImage _cuCameraData; + CuTexImage _cuCameraDataEX; + CuTexImage _cuPointData; + CuTexImage _cuPointDataEX; + CuTexImage _cuMeasurements; + CuTexImage _cuImageProj; + CuTexImage _cuJacobianCamera; + CuTexImage _cuJacobianPoint; + CuTexImage _cuJacobianCameraT; + CuTexImage _cuProjectionMap; + CuTexImage _cuPointMeasurementMap; + CuTexImage _cuCameraMeasurementMap; + CuTexImage _cuCameraMeasurementList; + CuTexImage _cuCameraMeasurementListT; + + /////////////////////////////// + CuTexImage _cuBufferData; + //////////////////////////// + CuTexImage _cuBlockPC; + CuTexImage _cuVectorSJ; + + /// LM normal equation + CuTexImage _cuVectorJtE; + CuTexImage _cuVectorJJ; + CuTexImage _cuVectorJX; + CuTexImage _cuVectorXK; + CuTexImage _cuVectorPK; + CuTexImage _cuVectorZK; + CuTexImage _cuVectorRK; + + /////////////////////// + protected: + int _num_imgpt_q; + float _weight_q; + CuTexImage _cuCameraQList; + CuTexImage _cuCameraQMap; + CuTexImage _cuCameraQMapW; + CuTexImage _cuCameraQListW; + + protected: + bool ProcessIndexCameraQ(std::vector& qmap, std::vector& qlist); + void ProcessWeightCameraQ(std::vector& cpnum, std::vector& qmap, + std::vector& qmapw, + std::vector& qlistw); + + protected: // internal functions + int GetParameterLength(); + int InitializeBundle(); + int ValidateInputData(); + void ReleaseAllocatedData(); + bool InitializeStorageForCG(); + bool InitializeBundleGPU(); + bool TransferDataToGPU(); + void TransferDataToHost(); + void DenormalizeData(); + void NormalizeData(); + void NormalizeDataF(); + void NormalizeDataD(); + void DebugProjections(); + void RunDebugSteps(); + bool CheckRequiredMem(int fresh = 1); + bool CheckRequiredMemX(); + void ReserveStorage(size_t ncam, size_t npt, size_t nproj); + void ReserveStorageAuto(); + + protected: + float EvaluateProjection(CuTexImage& cam, CuTexImage& point, + CuTexImage& proj); + float EvaluateProjectionX(CuTexImage& cam, CuTexImage& point, + CuTexImage& proj); + float UpdateCameraPoint(CuTexImage& dx, CuTexImage& cuImageTempProj); + float SaveUpdatedSystem(float residual_reduction, float dx_sqnorm, + float damping); + float EvaluateDeltaNorm(); + void EvaluateJacobians(bool shuffle = true); + void PrepareJacobianNormalization(); + void ComputeJtE(CuTexImage& E, CuTexImage& JtE, int mode = 0); + void ComputeJX(CuTexImage& X, CuTexImage& JX, int mode = 0); + void ComputeDiagonal(CuTexImage& JJ, CuTexImage& JJI); + void ComputeBlockPC(float lambda, bool dampd = true); + void ApplyBlockPC(CuTexImage& v, CuTexImage& pv, int mode = 0); + int SolveNormalEquationPCGB(float lambda); + int SolveNormalEquationPCGX(float lambda); + int SolveNormalEquation(float lambda); + void AdjustBundleAdjsutmentMode(); + void NonlinearOptimizeLM(); + void BundleAdjustment(); + void RunTestIterationLM(bool reduced); + void SaveBundleRecord(int iter, float res, float damping, float& g_norm, + float& g_inf); + ///////////////////////////////// + void SaveNormalEquation(float lambda); + void RunProfileSteps(); + void WarmupDevice(); + + public: + virtual float GetMeanSquaredError(); + virtual void SetCameraData(size_t ncam, CameraT* cams); + virtual void SetPointData(size_t npoint, Point3D* pts); + virtual void SetProjection(size_t nproj, const Point2D* imgpts, + const int* point_idx, const int* cam_idx); + virtual void SetFocalMask(const int* fmask, float weight); + virtual int RunBundleAdjustment(); + + /// + virtual void AbortBundleAdjustment() { __abort_flag = true; } + virtual int GetCurrentIteration() { return __current_iteration; } + virtual void SetNextTimeBudget(int seconds) { + __bundle_time_budget = seconds; + } + virtual void SetNextBundleMode(BundleModeT mode) { + __bundle_mode_next = mode; + } + virtual void SetFixedIntrinsics(bool fixed) { __fixed_intrinsics = fixed; } + virtual void EnableRadialDistortion(DistortionT type) { + __use_radial_distortion = type; + } + virtual void ParseParam(int narg, char** argv) { + ConfigBA::ParseParam(narg, argv); + } + virtual ConfigBA* GetInternalConfig() { return this; } + + public: + SparseBundleCU(int device); + size_t GetMemCapacity(); +}; + +} // namespace pba + +#endif diff --git a/colmap/include/colmap/lib/PBA/pba.h b/colmap/include/colmap/lib/PBA/pba.h new file mode 100644 index 0000000000000000000000000000000000000000..3ebf5813f94fa0dc616f9968279e5b2bfd6cf54a --- /dev/null +++ b/colmap/include/colmap/lib/PBA/pba.h @@ -0,0 +1,156 @@ +//////////////////////////////////////////////////////////////////////////// +// File: pba.h +// Author: Changchang Wu (ccwu@cs.washington.edu) +// Description : interface of class ParallelBA, which has two +//implementations +// SparseBundleCU for CUDA-based version, and +// SparseBundleCPU for CPU multi-threading version +// +// Copyright (c) 2011 Changchang Wu (ccwu@cs.washington.edu) +// and the University of Washington at Seattle +// +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public +// License as published by the Free Software Foundation; either +// Version 3 of the License, or (at your option) any later version. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +//////////////////////////////////////////////////////////////////////////////// + +#ifndef PARALLEL_BA_H +#define PARALLEL_BA_H + +#if defined(_WIN32) +#ifdef PBA_DLL +#ifdef DLL_EXPORT +#define PBA_EXPORT __declspec(dllexport) +#else +#define PBA_EXPORT __declspec(dllimport) +#endif +#else +#define PBA_EXPORT +#endif + +#define PBA_EXPORT_EXTERN PBA_EXPORT + +#if _MSC_VER > 1000 +#pragma once +#endif +#else +#define PBA_EXPORT +#define PBA_EXPORT_EXTERN extern "C" +#endif + +// filetype definitions for points and camera +#include "DataInterface.h" +#include "ConfigBA.h" + +namespace pba { + +class ParallelBA { + public: + enum StatusT { + STATUS_SUCCESS = 0, + STATUS_CAMERA_MISSING = 1, + STATUS_POINT_MISSING, + STATUS_PROJECTION_MISSING, + STATUS_MEASURMENT_MISSING, + STATUS_ALLOCATION_FAIL + }; + enum DeviceT { + PBA_INVALID_DEVICE = -4, + PBA_CPU_DOUBLE = -3, + PBA_CPU_FLOAT = -2, + PBA_CUDA_DEVICE_DEFAULT = -1, + PBA_CUDA_DEVICE0 = 0 + }; + enum DistortionT { + PBA_MEASUREMENT_DISTORTION = -1, // single parameter, apply to measurements + PBA_NO_DISTORTION = 0, // no radial distortion + PBA_PROJECTION_DISTORTION = 1 // single parameter, apply to projectino + }; + enum BundleModeT { + BUNDLE_FULL = 0, + BUNDLE_ONLY_MOTION = 1, + BUNDLE_ONLY_STRUCTURE = 2, + }; + + private: + ParallelBA* _optimizer; + + public: + //////////////////////////////////////////////////// + // methods for changing bundle adjustment settings + PBA_EXPORT virtual void ParseParam(int narg, char** argv); // indirect method + PBA_EXPORT virtual ConfigBA* GetInternalConfig(); // direct method + PBA_EXPORT virtual void SetFixedIntrinsics( + bool fixed); // call this for calibrated system + PBA_EXPORT virtual void EnableRadialDistortion( + DistortionT type); // call this to enable radial distortion + PBA_EXPORT virtual void SetNextTimeBudget( + int seconds); //# of seconds for next run (0 = no limit) + PBA_EXPORT virtual void ReserveStorage(size_t ncam, size_t npt, size_t nproj); + + public: + // function name change; the old one is mapped as inline function + inline void SetFocalLengthFixed(bool fixed) { SetFixedIntrinsics(fixed); } + inline void ResetBundleStorage() { + ReserveStorage(0, 0, 0); /*Reset devide for CUDA*/ + } + + public: + ///////////////////////////////////////////////////// + // optimizer interface, input and run + PBA_EXPORT virtual void SetCameraData(size_t ncam, + CameraT* cams); // set camera data + PBA_EXPORT virtual void SetPointData(size_t npoint, + Point3D* pts); // set 3D point data + PBA_EXPORT virtual void SetProjection(size_t nproj, const Point2D* imgpts, + const int* point_idx, + const int* cam_idx); // set projections + PBA_EXPORT virtual void SetNextBundleMode( + BundleModeT + mode = BUNDLE_FULL); // mode of the next bundle adjustment call + PBA_EXPORT virtual int RunBundleAdjustment(); // start bundle adjustment, + // return number of successful + // LM iterations + public: + ////////////////////////////////////////////////// + // Query optimzer runing status for Multi-threading + // Three functions below can be called from a differnt thread while bundle is + // running + PBA_EXPORT virtual float + GetMeanSquaredError(); // read back results during/after BA + PBA_EXPORT virtual void + AbortBundleAdjustment(); // tell bundle adjustment to abort ASAP + PBA_EXPORT virtual int + GetCurrentIteration(); // which iteration is it working on? + public: + PBA_EXPORT ParallelBA(DeviceT device = PBA_CUDA_DEVICE_DEFAULT, + const int num_threads = -1); + // PBA_EXPORT void* operator new(size_t size); + PBA_EXPORT virtual ~ParallelBA(); + + public: + ////////////////////////////////////////////// + // Future functions will be added to the end for compatiability with old + // version. + PBA_EXPORT virtual void SetFocalMask(const int* fmask, float weight = 1.0f); +}; + +// function for dynamic loading of library +PBA_EXPORT_EXTERN ParallelBA* NewParallelBA( + ParallelBA::DeviceT device = ParallelBA::PBA_CUDA_DEVICE_DEFAULT); +typedef ParallelBA* (*NEWPARALLELBAPROC)(ParallelBA::DeviceT); + +/////////////////////////////////////////////// +// older versions do not have this function. +PBA_EXPORT_EXTERN int ParallelBA_GetVersion(); + +} // namespace pba + +#endif diff --git a/colmap/include/colmap/lib/PBA/util.h b/colmap/include/colmap/lib/PBA/util.h new file mode 100644 index 0000000000000000000000000000000000000000..a63c8bbceb03f72b78f3bf0fae1f24733804d8dc --- /dev/null +++ b/colmap/include/colmap/lib/PBA/util.h @@ -0,0 +1,753 @@ +//////////////////////////////////////////////////////////////////////////// +// File: util.h +// Author: Changchang Wu (ccwu@cs.washington.edu) +// Description : some utility functions for reading/writing SfM data +// +// Copyright (c) 2011 Changchang Wu (ccwu@cs.washington.edu) +// and the University of Washington at Seattle +// +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public +// License as published by the Free Software Foundation; either +// Version 3 of the License, or (at your option) any later version. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include +#include +#include +#include +#include +#include +using namespace std; +#include "DataInterface.h" + +namespace pba { + +// File loader supports .nvm format and bundler format +bool LoadModelFile(const char* name, vector& camera_data, + vector& point_data, vector& measurements, + vector& ptidx, vector& camidx, + vector& names, vector& ptc); +void SaveNVM(const char* filename, vector& camera_data, + vector& point_data, vector& measurements, + vector& ptidx, vector& camidx, vector& names, + vector& ptc); +void SaveBundlerModel(const char* filename, vector& camera_data, + vector& point_data, + vector& measurements, vector& ptidx, + vector& camidx); + +////////////////////////////////////////////////////////////////// +void AddNoise(vector& camera_data, vector& point_data, + float percent); +void AddStableNoise(vector& camera_data, vector& point_data, + const vector& ptidx, const vector& camidx, + float percent); +bool RemoveInvisiblePoints(vector& camera_data, + vector& point_data, vector& ptidx, + vector& camidx, vector& measurements, + vector& names, vector& ptc); + +///////////////////////////////////////////////////////////////////////////// +bool LoadNVM(ifstream& in, vector& camera_data, + vector& point_data, vector& measurements, + vector& ptidx, vector& camidx, vector& names, + vector& ptc) { + int rotation_parameter_num = 4; + bool format_r9t = false; + string token; + if (in.peek() == 'N') { + in >> token; // file header + if (strstr(token.c_str(), "R9T")) { + rotation_parameter_num = 9; // rotation as 3x3 matrix + format_r9t = true; + } + } + + int ncam = 0, npoint = 0, nproj = 0; + // read # of cameras + in >> ncam; + if (ncam <= 1) return false; + + // read the camera parameters + camera_data.resize(ncam); // allocate the camera data + names.resize(ncam); + for (int i = 0; i < ncam; ++i) { + double f, q[9], c[3], d[2]; + in >> token >> f; + for (int j = 0; j < rotation_parameter_num; ++j) in >> q[j]; + in >> c[0] >> c[1] >> c[2] >> d[0] >> d[1]; + + camera_data[i].SetFocalLength(f); + if (format_r9t) { + camera_data[i].SetMatrixRotation(q); + camera_data[i].SetTranslation(c); + } else { + // older format for compability + camera_data[i].SetQuaternionRotation(q); // quaternion from the file + camera_data[i].SetCameraCenterAfterRotation( + c); // camera center from the file + } + camera_data[i].SetNormalizedMeasurementDistortion(d[0]); + names[i] = token; + } + + ////////////////////////////////////// + in >> npoint; + if (npoint <= 0) return false; + + // read image projections and 3D points. + point_data.resize(npoint); + for (int i = 0; i < npoint; ++i) { + float pt[3]; + int cc[3], npj; + in >> pt[0] >> pt[1] >> pt[2] >> cc[0] >> cc[1] >> cc[2] >> npj; + for (int j = 0; j < npj; ++j) { + int cidx, fidx; + float imx, imy; + in >> cidx >> fidx >> imx >> imy; + + camidx.push_back(cidx); // camera index + ptidx.push_back(i); // point index + + // add a measurment to the vector + measurements.push_back(Point2D(imx, imy)); + nproj++; + } + point_data[i].SetPoint(pt); + ptc.insert(ptc.end(), cc, cc + 3); + } + /////////////////////////////////////////////////////////////////////////////// + std::cout << ncam << " cameras; " << npoint << " 3D points; " << nproj + << " projections\n"; + + return true; +} + +void SaveNVM(const char* filename, vector& camera_data, + vector& point_data, vector& measurements, + vector& ptidx, vector& camidx, vector& names, + vector& ptc) { + std::cout << "Saving model to " << filename << "...\n"; + ofstream out(filename); + + out << "NVM_V3_R9T\n" << camera_data.size() << '\n' << std::setprecision(12); + if (names.size() < camera_data.size()) + names.resize(camera_data.size(), string("unknown")); + if (ptc.size() < 3 * point_data.size()) ptc.resize(point_data.size() * 3, 0); + + //////////////////////////////////// + for (size_t i = 0; i < camera_data.size(); ++i) { + CameraT& cam = camera_data[i]; + out << names[i] << ' ' << cam.GetFocalLength() << ' '; + for (int j = 0; j < 9; ++j) out << cam.m[0][j] << ' '; + out << cam.t[0] << ' ' << cam.t[1] << ' ' << cam.t[2] << ' ' + << cam.GetNormalizedMeasurementDistortion() << " 0\n"; + } + + out << point_data.size() << '\n'; + + for (size_t i = 0, j = 0; i < point_data.size(); ++i) { + Point3D& pt = point_data[i]; + int* pc = &ptc[i * 3]; + out << pt.xyz[0] << ' ' << pt.xyz[1] << ' ' << pt.xyz[2] << ' ' << pc[0] + << ' ' << pc[1] << ' ' << pc[2] << ' '; + + size_t je = j; + while (je < ptidx.size() && ptidx[je] == (int)i) je++; + + out << (je - j) << ' '; + + for (; j < je; ++j) + out << camidx[j] << ' ' << " 0 " << measurements[j].x << ' ' + << measurements[j].y << ' '; + + out << '\n'; + } +} + +bool LoadBundlerOut(const char* name, ifstream& in, + vector& camera_data, vector& point_data, + vector& measurements, vector& ptidx, + vector& camidx, vector& names, + vector& ptc) { + int rotation_parameter_num = 9; + string token; + while (in.peek() == '#') std::getline(in, token); + + char listpath[1024], filepath[1024]; + strcpy(listpath, name); + char* ext = strstr(listpath, ".out"); + strcpy(ext, "-list.txt\0"); + + /////////////////////////////////// + ifstream listin(listpath); + if (!listin.is_open()) { + listin.close(); + listin.clear(); + char* slash = strrchr(listpath, '/'); + if (slash == NULL) slash = strrchr(listpath, '\\'); + slash = slash ? slash + 1 : listpath; + strcpy(slash, "image_list.txt"); + listin.open(listpath); + } + if (listin) std::cout << "Using image list: " << listpath << '\n'; + + // read # of cameras + int ncam = 0, npoint = 0, nproj = 0; + in >> ncam >> npoint; + if (ncam <= 1 || npoint <= 1) return false; + std::cout << ncam << " cameras; " << npoint << " 3D points;\n"; + + // read the camera parameters + camera_data.resize(ncam); // allocate the camera data + names.resize(ncam); + + bool det_checked = false; + for (int i = 0; i < ncam; ++i) { + float f, q[9], c[3], d[2]; + in >> f >> d[0] >> d[1]; + for (int j = 0; j < rotation_parameter_num; ++j) in >> q[j]; + in >> c[0] >> c[1] >> c[2]; + + camera_data[i].SetFocalLength(f); + camera_data[i].SetInvertedR9T(q, c); + camera_data[i].SetProjectionDistortion(d[0]); + + if (listin >> filepath && f != 0) { + char* slash = strrchr(filepath, '/'); + if (slash == NULL) slash = strchr(filepath, '\\'); + names[i] = (slash ? (slash + 1) : filepath); + std::getline(listin, token); + + if (!det_checked) { + float det = camera_data[i].GetRotationMatrixDeterminant(); + std::cout << "Check rotation matrix: " << det << '\n'; + det_checked = true; + } + } else { + names[i] = "unknown"; + } + } + + // read image projections and 3D points. + point_data.resize(npoint); + for (int i = 0; i < npoint; ++i) { + float pt[3]; + int cc[3], npj; + in >> pt[0] >> pt[1] >> pt[2] >> cc[0] >> cc[1] >> cc[2] >> npj; + for (int j = 0; j < npj; ++j) { + int cidx, fidx; + float imx, imy; + in >> cidx >> fidx >> imx >> imy; + + camidx.push_back(cidx); // camera index + ptidx.push_back(i); // point index + + // add a measurment to the vector + measurements.push_back(Point2D(imx, -imy)); + nproj++; + } + point_data[i].SetPoint(pt[0], pt[1], pt[2]); + ptc.insert(ptc.end(), cc, cc + 3); + } + /////////////////////////////////////////////////////////////////////////////// + std::cout << ncam << " cameras; " << npoint << " 3D points; " << nproj + << " projections\n"; + return true; +} + +void SaveBundlerOut(const char* filename, vector& camera_data, + vector& point_data, vector& measurements, + vector& ptidx, vector& camidx, + vector& names, vector& ptc) { + char listpath[1024]; + strcpy(listpath, filename); + char* ext = strstr(listpath, ".out"); + if (ext == NULL) return; + strcpy(ext, "-list.txt\0"); + + ofstream out(filename); + out << "# Bundle file v0.3\n"; + out << std::setprecision(12); // need enough precision + out << camera_data.size() << " " << point_data.size() << '\n'; + + // save camera data + for (size_t i = 0; i < camera_data.size(); ++i) { + float q[9], c[3]; + CameraT& ci = camera_data[i]; + out << ci.GetFocalLength() << ' ' << ci.GetProjectionDistortion() << " 0\n"; + ci.GetInvertedR9T(q, c); + for (int j = 0; j < 9; ++j) out << q[j] << (((j % 3) == 2) ? '\n' : ' '); + out << c[0] << ' ' << c[1] << ' ' << c[2] << '\n'; + } + /// + for (size_t i = 0, j = 0; i < point_data.size(); ++i) { + int npj = 0, *ci = &ptc[i * 3]; + Point3D& pt = point_data[i]; + while (j + npj < point_data.size() && ptidx[j + npj] == ptidx[j]) npj++; + /////////////////////////// + out << pt.xyz[0] << ' ' << pt.xyz[1] << ' ' << pt.xyz[2] << '\n'; + out << ci[0] << ' ' << ci[1] << ' ' << ci[2] << '\n'; + out << npj << ' '; + for (int k = 0; k < npj; ++k) + out << camidx[j + k] << " 0 " << measurements[j + k].x << ' ' + << -measurements[j + k].y << '\n'; + out << '\n'; + j += npj; + } + + ofstream listout(listpath); + for (size_t i = 0; i < names.size(); ++i) listout << names[i] << '\n'; +} + +template +bool LoadBundlerModel(ifstream& in, vector& camera_data, + vector& point_data, + vector& measurements, vector& ptidx, + vector& camidx) { + // read bundle data from a file + size_t ncam = 0, npt = 0, nproj = 0; + if (!(in >> ncam >> npt >> nproj)) return false; + /////////////////////////////////////////////////////////////////////////////// + std::cout << ncam << " cameras; " << npt << " 3D points; " << nproj + << " projections\n"; + + camera_data.resize(ncam); + point_data.resize(npt); + measurements.resize(nproj); + camidx.resize(nproj); + ptidx.resize(nproj); + + for (size_t i = 0; i < nproj; ++i) { + double x, y; + int cidx, pidx; + in >> cidx >> pidx >> x >> y; + if (((size_t)pidx) == npt && camidx.size() > i) { + camidx.resize(i); + ptidx.resize(i); + measurements.resize(i); + std::cout << "Truncate measurements to " << i << '\n'; + } else if (((size_t)pidx) >= npt) { + continue; + } else { + camidx[i] = cidx; + ptidx[i] = pidx; + measurements[i].SetPoint2D(x, -y); + } + } + + for (size_t i = 0; i < ncam; ++i) { + double p[9]; + for (int j = 0; j < 9; ++j) in >> p[j]; + CameraT& cam = camera_data[i]; + cam.SetFocalLength(p[6]); + cam.SetInvertedRT(p, p + 3); + cam.SetProjectionDistortion(p[7]); + } + + for (size_t i = 0; i < npt; ++i) { + double pt[3]; + in >> pt[0] >> pt[1] >> pt[2]; + point_data[i].SetPoint(pt); + } + return true; +} + +void SaveBundlerModel(const char* filename, vector& camera_data, + vector& point_data, + vector& measurements, vector& ptidx, + vector& camidx) { + std::cout << "Saving model to " << filename << "...\n"; + ofstream out(filename); + out << std::setprecision(12); // need enough precision + out << camera_data.size() << ' ' << point_data.size() << ' ' + << measurements.size() << '\n'; + for (size_t i = 0; i < measurements.size(); ++i) { + out << camidx[i] << ' ' << ptidx[i] << ' ' << measurements[i].x << ' ' + << -measurements[i].y << '\n'; + } + + for (size_t i = 0; i < camera_data.size(); ++i) { + CameraT& cam = camera_data[i]; + double r[3], t[3]; + cam.GetInvertedRT(r, t); + out << r[0] << ' ' << r[1] << ' ' << r[2] << ' ' << t[0] << ' ' << t[1] + << ' ' << t[2] << ' ' << cam.f << ' ' << cam.GetProjectionDistortion() + << " 0\n"; + } + + for (size_t i = 0; i < point_data.size(); ++i) { + Point3D& pt = point_data[i]; + out << pt.xyz[0] << ' ' << pt.xyz[1] << ' ' << pt.xyz[2] << '\n'; + } +} + +bool LoadModelFile(const char* name, vector& camera_data, + vector& point_data, vector& measurements, + vector& ptidx, vector& camidx, + vector& names, vector& ptc) { + if (name == NULL) return false; + ifstream in(name); + + std::cout << "Loading cameras/points: " << name << "\n"; + if (!in.is_open()) return false; + + if (strstr(name, ".nvm")) + return LoadNVM(in, camera_data, point_data, measurements, ptidx, camidx, + names, ptc); + else if (strstr(name, ".out")) + return LoadBundlerOut(name, in, camera_data, point_data, measurements, + ptidx, camidx, names, ptc); + else + return LoadBundlerModel(in, camera_data, point_data, measurements, ptidx, + camidx); +} + +float random_ratio(float percent) { + return (rand() % 101 - 50) * 0.02f * percent + 1.0f; +} + +void AddNoise(vector& camera_data, vector& point_data, + float percent) { + std::srand((unsigned int)time(NULL)); + for (size_t i = 0; i < camera_data.size(); ++i) { + camera_data[i].f *= random_ratio(percent); + camera_data[i].t[0] *= random_ratio(percent); + camera_data[i].t[1] *= random_ratio(percent); + camera_data[i].t[2] *= random_ratio(percent); + double e[3]; + camera_data[i].GetRodriguesRotation(e); + e[0] *= random_ratio(percent); + e[1] *= random_ratio(percent); + e[2] *= random_ratio(percent); + camera_data[i].SetRodriguesRotation(e); + } + + for (size_t i = 0; i < point_data.size(); ++i) { + point_data[i].xyz[0] *= random_ratio(percent); + point_data[i].xyz[1] *= random_ratio(percent); + point_data[i].xyz[2] *= random_ratio(percent); + } +} + +void AddStableNoise(vector& camera_data, vector& point_data, + const vector& ptidx, const vector& camidx, + float percent) { + /// + std::srand((unsigned int)time(NULL)); + // do not modify the visibility status.. + vector zz0(ptidx.size()); + vector backup = camera_data; + vector vx(point_data.size()), vy(point_data.size()), + vz(point_data.size()); + for (size_t i = 0; i < point_data.size(); ++i) { + Point3D& pt = point_data[i]; + vx[i] = pt.xyz[0]; + vy[i] = pt.xyz[1]; + vz[i] = pt.xyz[2]; + } + + // find out the median location of all the 3D points. + size_t median_idx = point_data.size() / 2; + + std::nth_element(vx.begin(), vx.begin() + median_idx, vx.end()); + std::nth_element(vy.begin(), vy.begin() + median_idx, vy.end()); + std::nth_element(vz.begin(), vz.begin() + median_idx, vz.end()); + float cx = vx[median_idx], cy = vy[median_idx], cz = vz[median_idx]; + + for (size_t i = 0; i < ptidx.size(); ++i) { + CameraT& cam = camera_data[camidx[i]]; + Point3D& pt = point_data[ptidx[i]]; + zz0[i] = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] + + cam.m[2][2] * pt.xyz[2] + cam.t[2]; + } + + vector z2 = zz0; + median_idx = ptidx.size() / 2; + std::nth_element(z2.begin(), z2.begin() + median_idx, z2.end()); + float mz = z2[median_idx]; // median depth + float dist_noise_base = mz * 0.2f; + + ///////////////////////////////////////////////// + // modify points first.. + for (size_t i = 0; i < point_data.size(); ++i) { + Point3D& pt = point_data[i]; + pt.xyz[0] = pt.xyz[0] - cx + dist_noise_base * random_ratio(percent); + pt.xyz[1] = pt.xyz[1] - cy + dist_noise_base * random_ratio(percent); + pt.xyz[2] = pt.xyz[2] - cz + dist_noise_base * random_ratio(percent); + } + + vector need_modification(camera_data.size(), true); + int invalid_count = 0, modify_iteration = 1; + + do { + if (invalid_count) + std::cout << "NOTE" << std::setw(2) << modify_iteration << ": modify " + << invalid_count << " camera to fix visibility\n"; + + ////////////////////////////////////////////////////// + for (size_t i = 0; i < camera_data.size(); ++i) { + if (!need_modification[i]) continue; + CameraT& cam = camera_data[i]; + double e[3], c[3]; + cam = backup[i]; + cam.f *= random_ratio(percent); + + /////////////////////////////////////////////////////////// + cam.GetCameraCenter(c); + c[0] = c[0] - cx + dist_noise_base * random_ratio(percent); + c[1] = c[1] - cy + dist_noise_base * random_ratio(percent); + c[2] = c[2] - cz + dist_noise_base * random_ratio(percent); + + /////////////////////////////////////////////////////////// + cam.GetRodriguesRotation(e); + e[0] *= random_ratio(percent); + e[1] *= random_ratio(percent); + e[2] *= random_ratio(percent); + + /////////////////////////////////////////////////////////// + cam.SetRodriguesRotation(e); + cam.SetCameraCenterAfterRotation(c); + } + vector invalidc(camera_data.size(), false); + + invalid_count = 0; + for (size_t i = 0; i < ptidx.size(); ++i) { + int cid = camidx[i]; + if (need_modification[cid] == false) continue; + if (invalidc[cid]) continue; + CameraT& cam = camera_data[cid]; + Point3D& pt = point_data[ptidx[i]]; + float z = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] + + cam.m[2][2] * pt.xyz[2] + cam.t[2]; + if (z * zz0[i] > 0) continue; + if (zz0[i] == 0 && z > 0) continue; + invalid_count++; + invalidc[cid] = true; + } + + need_modification = invalidc; + modify_iteration++; + + } while (invalid_count && modify_iteration < 20); +} + +void ExamineVisiblity(const char* input_filename) { + ////////////// + vector camera_data; + vector point_data; + vector ptidx, camidx; + vector measurements; + ifstream in(input_filename); + LoadBundlerModel(in, camera_data, point_data, measurements, ptidx, camidx); + + //////////////// + int count = 0; + double d1 = 100, d2 = 100; + std::cout << "checking visibility...\n"; + vector zz(ptidx.size()); + for (size_t i = 0; i < ptidx.size(); ++i) { + CameraD& cam = camera_data[camidx[i]]; + Point3B& pt = point_data[ptidx[i]]; + double dz = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] + + cam.m[2][2] * pt.xyz[2] + cam.t[2]; + // double dx = cam.m[0][0] * pt.xyz[0] + cam.m[0][1] * pt.xyz[1] + + // cam.m[0][2] * pt.xyz[2] + cam.t[0]; + // double dy = cam.m[1][0] * pt.xyz[0] + cam.m[1][1] * pt.xyz[1] + + // cam.m[1][2] * pt.xyz[2] + cam.t[1]; + + //////////////////////////////////////// + float c[3]; + cam.GetCameraCenter(c); + + CameraT camt; + camt.SetCameraT(cam); + Point3D ptt; + ptt.SetPoint(pt.xyz); + double fz = camt.m[2][0] * ptt.xyz[0] + camt.m[2][1] * ptt.xyz[1] + + camt.m[2][2] * ptt.xyz[2] + camt.t[2]; + double fz2 = camt.m[2][0] * (ptt.xyz[0] - c[0]) + + camt.m[2][1] * (ptt.xyz[1] - c[1]) + + camt.m[2][2] * (ptt.xyz[2] - c[2]); + + // if(dz == 0 && fz == 0) continue; + + if (dz * fz <= 0 || fz == 0) { + std::cout << "cam " + << camidx[i] //<& camera_data, + vector& point_data, vector& ptidx, + vector& camidx, vector& measurements, + vector& names, vector& ptc) { + vector zz(ptidx.size()); + for (size_t i = 0; i < ptidx.size(); ++i) { + CameraT& cam = camera_data[camidx[i]]; + Point3D& pt = point_data[ptidx[i]]; + zz[i] = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] + + cam.m[2][2] * pt.xyz[2] + cam.t[2]; + } + size_t median_idx = ptidx.size() / 2; + std::nth_element(zz.begin(), zz.begin() + median_idx, zz.end()); + float dist_threshold = zz[median_idx] * 0.001f; + + // keep removing 3D points. until all of them are infront of the cameras.. + vector pmask(point_data.size(), true); + int points_removed = 0; + for (size_t i = 0; i < ptidx.size(); ++i) { + int cid = camidx[i], pid = ptidx[i]; + if (!pmask[pid]) continue; + CameraT& cam = camera_data[cid]; + Point3D& pt = point_data[pid]; + bool visible = (cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] + + cam.m[2][2] * pt.xyz[2] + cam.t[2] > + dist_threshold); + pmask[pid] = visible; // this point should be removed + if (!visible) points_removed++; + } + if (points_removed == 0) return false; + vector cv(camera_data.size(), 0); + // should any cameras be removed ? + int min_observation = 20; // cameras should see at leat 20 points + + do { + // count visible points for each camera + std::fill(cv.begin(), cv.end(), 0); + for (size_t i = 0; i < ptidx.size(); ++i) { + int cid = camidx[i], pid = ptidx[i]; + if (pmask[pid]) cv[cid]++; + } + + // check if any more points should be removed + vector pv(point_data.size(), 0); + for (size_t i = 0; i < ptidx.size(); ++i) { + int cid = camidx[i], pid = ptidx[i]; + if (!pmask[pid]) continue; // point already removed + if (cv[cid] < min_observation) // this camera shall be removed. + { + /// + } else { + pv[pid]++; + } + } + + points_removed = 0; + for (size_t i = 0; i < point_data.size(); ++i) { + if (pmask[i] == false) continue; + if (pv[i] >= 2) continue; + pmask[i] = false; + points_removed++; + } + } while (points_removed > 0); + + //////////////////////////////////// + vector cmask(camera_data.size(), true); + for (size_t i = 0; i < camera_data.size(); ++i) + cmask[i] = cv[i] >= min_observation; + //////////////////////////////////////////////////////// + + vector cidx(camera_data.size()); + vector pidx(point_data.size()); + + /// modified model. + vector camera_data2; + vector point_data2; + vector ptidx2; + vector camidx2; + vector measurements2; + vector names2; + vector ptc2; + + // + if (names.size() < camera_data.size()) + names.resize(camera_data.size(), string("unknown")); + if (ptc.size() < 3 * point_data.size()) ptc.resize(point_data.size() * 3, 0); + + ////////////////////////////// + int new_camera_count = 0, new_point_count = 0; + for (size_t i = 0; i < camera_data.size(); ++i) { + if (!cmask[i]) continue; + camera_data2.push_back(camera_data[i]); + names2.push_back(names[i]); + cidx[i] = new_camera_count++; + } + + for (size_t i = 0; i < point_data.size(); ++i) { + if (!pmask[i]) continue; + point_data2.push_back(point_data[i]); + ptc.push_back(ptc[i]); + pidx[i] = new_point_count++; + } + + int new_observation_count = 0; + for (size_t i = 0; i < ptidx.size(); ++i) { + int pid = ptidx[i], cid = camidx[i]; + if (!pmask[pid] || !cmask[cid]) continue; + ptidx2.push_back(pidx[pid]); + camidx2.push_back(cidx[cid]); + measurements2.push_back(measurements[i]); + new_observation_count++; + } + + std::cout << "NOTE: removing " << (camera_data.size() - new_camera_count) + << " cameras; " << (point_data.size() - new_point_count) + << " 3D Points; " << (measurements.size() - new_observation_count) + << " Observations;\n"; + + camera_data2.swap(camera_data); + names2.swap(names); + point_data2.swap(point_data); + ptc2.swap(ptc); + ptidx2.swap(ptidx); + camidx2.swap(camidx); + measurements2.swap(measurements); + + return true; +} + +void SaveModelFile(const char* outpath, vector& camera_data, + vector& point_data, vector& measurements, + vector& ptidx, vector& camidx, + vector& names, vector& ptc) { + if (outpath == NULL) return; + if (strstr(outpath, ".nvm")) + SaveNVM(outpath, camera_data, point_data, measurements, ptidx, camidx, + names, ptc); + else if (strstr(outpath, ".out")) + SaveBundlerOut(outpath, camera_data, point_data, measurements, ptidx, + camidx, names, ptc); + else + SaveBundlerModel(outpath, camera_data, point_data, measurements, ptidx, + camidx); +} + +} // namespace pba diff --git a/colmap/include/colmap/lib/PoissonRecon/Allocator.h b/colmap/include/colmap/lib/PoissonRecon/Allocator.h new file mode 100644 index 0000000000000000000000000000000000000000..e2723b988fcfbaae4480ce366e84b730b0e2a6cb --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/Allocator.h @@ -0,0 +1,163 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +#ifndef ALLOCATOR_INCLUDED +#define ALLOCATOR_INCLUDED +#include + +class AllocatorState{ +public: + int index,remains; +}; +/** This templated class assists in memory allocation and is well suited for instances + * when it is known that the sequence of memory allocations is performed in a stack-based + * manner, so that memory allocated last is released first. It also preallocates memory + * in chunks so that multiple requests for small chunks of memory do not require separate + * system calls to the memory manager. + * The allocator is templated off of the class of objects that we would like it to allocate, + * ensuring that appropriate constructors and destructors are called as necessary. + */ +template +class Allocator +{ + int blockSize; + int index , remains; + std::vector< T* > memory; +public: + Allocator( void ){ blockSize = index = remains = 0; } + ~Allocator( void ){ reset(); } + + /** This method is the allocators destructor. It frees up any of the memory that + * it has allocated. */ + void reset( void ) + { + for(size_t i=0;iblockSize = blockSize; + index=-1; + remains=0; + } + + /** This method returns a pointer to an array of elements objects. If there is left over pre-allocated + * memory, this method simply returns a pointer to the next free piece of memory, otherwise it pre-allocates + * more memory. Note that if the number of objects requested is larger than the value blockSize with which + * the allocator was initialized, the request for memory will fail. + */ + T* newElements( int elements=1 ) + { + T* mem; + if( !elements ) return NULL; + if( elements>blockSize ) fprintf( stderr , "[ERROR] Allocator: elements bigger than block-size: %d>%d\n" , elements , blockSize ) , exit( 0 ); + if( remains + +#define ARRAY_DEBUG 0 +#ifdef _WIN64 +#define ASSERT( x ) { if( !( x ) ) __debugbreak(); } +#else // !_WIN64 +#ifdef _WIN32 +#define ASSERT( x ) { if( !( x ) ) _asm{ int 0x03 } } +#else // !_WIN32 +#define ASSERT( x ) { if( !( x ) ) exit(0); } +#endif // _WIN32 +#endif // _WIN64 + +// Code from http://stackoverflow.com +void* aligned_malloc( size_t size , size_t align ) +{ + // Align enough for the data, the alignment padding, and room to store a pointer to the actual start of the memory + void* mem = malloc( size + align + sizeof( void* ) ); + // The position at which we could potentially start addressing + char* amem = ( (char*)mem ) + sizeof( void* ); + // Add align-1 to the start of the address and then zero out at most of the first align-1 bits. + amem = ( char* )( ( (size_t)( ( (char*)amem ) + (align-1) ) ) & ~( align-1 ) ); + // Pre-write the actual address + ( ( void** ) amem )[-1] = mem; + return amem; +} +void aligned_free( void* mem ) { free( ( ( void** )mem )[-1] ); } + +#if ARRAY_DEBUG +#pragma message ( "[WARNING] Array debugging is enabled" ) +#include "Array.inl" +#define Pointer( ... ) Array< __VA_ARGS__ > +#define ConstPointer( ... ) ConstArray< __VA_ARGS__ > +#define NullPointer( ... ) Array< __VA_ARGS__ >() +template< class C > void FreePointer( Array< C >& a ){ a.Free( ); } +template< class C > void AlignedFreePointer( Array< C >& a ){ a.Free( ); } +template< class C > void VFreePointer( Array< C >& a ){ a.Free( ); } +template< class C > void DeletePointer( Array< C >& a ){ a.Delete( ); } + +template< class C > Array< C > NewPointer( size_t size , const char* name=NULL ){ return Array< C >::New ( size , name ); } +template< class C > Array< C > AllocPointer( size_t size , const char* name=NULL ){ return Array< C >::Alloc ( size , false , name ); } +template< class C > Array< C > AlignedAllocPointer( size_t size , size_t alignment , const char* name=NULL ){ return Array< C >::AlignedAlloc( size , alignment , false , name ); } +template< class C > Array< C > ReAllocPointer( Array< C >& a , size_t size , const char* name=NULL ){ return Array< C >::ReAlloc ( a , size , false , name ); } + +template< class C > C* PointerAddress( Array< C >& a ) { return a.pointer(); } +template< class C > const C* PointerAddress( ConstArray< C >& a ) { return a.pointer(); } +template< class C > Array< C > GetPointer( C& c ) { return Array< C >::FromPointer( &c , 1 ); } +template< class C > ConstArray< C > GetPointer( const C& c ) { return ConstArray< C >::FromPointer( &c , 1 ); } +template< class C > Array< C > GetPointer( std::vector< C >& v ){ return Array< C >::FromPointer( &v[0] , v.size() ); } +template< class C > ConstArray< C > GetPointer( const std::vector< C >& v ){ return ConstArray< C >::FromPointer( &v[0] , v.size() ); } + +#else // !ARRAY_DEBUG +#define Pointer( ... ) __VA_ARGS__* +#define ConstPointer( ... ) const __VA_ARGS__* +#define NullPointer( ... ) NULL + +#define FreePointer( ... ) { if( __VA_ARGS__ ) free( __VA_ARGS__ ) , __VA_ARGS__ = NULL; } +#define AlignedFreePointer( ... ) { if( __VA_ARGS__ ) aligned_free( __VA_ARGS__ ) , __VA_ARGS__ = NULL; } +#define DeletePointer( ... ) { if( __VA_ARGS__ ) delete[] __VA_ARGS__ , __VA_ARGS__ = NULL; } + +template< class C > C* NewPointer( size_t size , const char* name=NULL ){ return new C[size]; } +template< class C > C* AllocPointer( size_t size , const char* name=NULL ){ return (C*) malloc( sizeof(C) * size ); } +template< class C > C* AlignedAllocPointer( size_t size , size_t alignment , const char* name=NULL ){ return (C*)aligned_malloc( sizeof(C) * size , alignment ); } +template< class C > C* ReAllocPointer( C* c , size_t size , const char* name=NULL ){ return (C*) realloc( c , sizeof(C) * size ); } + +//template< class C > C* NullPointer( void ){ return NULL; } + +template< class C > C* PointerAddress( C* c ){ return c; } +template< class C > const C* PointerAddress( const C* c ){ return c; } +template< class C > C* GetPointer( C& c ){ return &c; } +template< class C > const C* GetPointer( const C& c ){ return &c; } +template< class C > C* GetPointer( std::vector< C >& v ){ return &v[0]; } +template< class C > const C* GetPointer( const std::vector< C >& v ){ return &v[0]; } +#endif // ARRAY_DEBUG +#endif // ARRAY_INCLUDED diff --git a/colmap/include/colmap/lib/PoissonRecon/Array.inl b/colmap/include/colmap/lib/PoissonRecon/Array.inl new file mode 100644 index 0000000000000000000000000000000000000000..6247f5282f312c55a923221b61649354e343811d --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/Array.inl @@ -0,0 +1,662 @@ +/* +Copyright (c) 2011, Michael Kazhdan and Ming Chuang +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ +#define FULL_ARRAY_DEBUG 0 // Note that this is not thread-safe + +#include +#include +#include +#ifdef _WIN32 +#include +#endif // _WIN32 +#include + +inline bool isfinitef( float fp ){ float f=fp; return ((*(unsigned *)&f)&0x7f800000)!=0x7f800000; } + + +template< class C > bool IsValid( const C& c ); +#if _DEBUG +template< > inline bool IsValid< float >( const float& f ) { return isfinitef( f ) && ( f==0.f || abs(f)>1e-31f ); } +#else // !_DEBUG +template< > inline bool IsValid< float >( const float& f ) { return isfinitef( f ); } +#endif // _DEBUG +template< > inline bool IsValid< __m128 >( const __m128& m ) +{ + const __m128* addr = &m; + if( size_t(addr) & 15 ) return false; + else return true; +} +template< class C > inline bool IsValid( const C& c ){ return true; } + + +#if FULL_ARRAY_DEBUG +class DebugMemoryInfo +{ +public: + const void* address; + char name[512]; +}; +static std::vector< DebugMemoryInfo > memoryInfo; +#endif // FULL_ARRAY_DEBUG + +template< class C > +class Array +{ + void _assertBounds( long long idx ) const + { + if( idx=max ) + { + fprintf( stderr , "Array index out-of-bounds: %lld <= %lld < %lld\n" , min , idx , max ); + ASSERT( 0 ); + exit( 0 ); + } + } +protected: + C *data , *_data; + long long min , max; +#if FULL_ARRAY_DEBUG + static void _AddMemoryInfo( const void* ptr , const char* name ) + { + size_t sz = memoryInfo.size(); + memoryInfo.resize( sz + 1 ); + memoryInfo[sz].address = ptr; + if( name ) strcpy( memoryInfo[sz].name , name ); + else memoryInfo[sz].name[0] = 0; + } + static void _RemoveMemoryInfo( const void* ptr ) + { + { + size_t idx; + for( idx=0 ; idx + Array( Array< D >& a ) + { + _data = NULL; + if( !a ) + { + data = NULL; + min = max = 0; + } + else + { + // [WARNING] Chaning szC and szD to size_t causes some really strange behavior. + long long szC = sizeof( C ); + long long szD = sizeof( D ); + data = (C*)a.data; + min = ( a.minimum() * szD ) / szC; + max = ( a.maximum() * szD ) / szC; + if( min*szC!=a.minimum()*szD || max*szC!=a.maximum()*szD ) + { + fprintf( stderr , "Could not convert array [ %lld , %lld ] * %lld => [ %lld , %lld ] * %lld\n" , a.minimum() , a.maximum() , szD , min , max , szC ); + ASSERT( 0 ); + exit( 0 ); + } + } + } + static Array FromPointer( C* data , long long max ) + { + Array a; + a._data = NULL; + a.data = data; + a.min = 0; + a.max = max; + return a; + } + static Array FromPointer( C* data , long long min , long long max ) + { + Array a; + a._data = NULL; + a.data = data; + a.min = min; + a.max = max; + return a; + } + inline bool operator == ( const Array< C >& a ) const { return data==a.data; } + inline bool operator != ( const Array< C >& a ) const { return data!=a.data; } + inline bool operator == ( const C* c ) const { return data==c; } + inline bool operator != ( const C* c ) const { return data!=c; } + inline C* operator -> ( void ) + { + _assertBounds( 0 ); + return data; + } + inline const C* operator -> ( ) const + { + _assertBounds( 0 ); + return data; + } + inline C& operator[]( long long idx ) + { + _assertBounds( idx ); + return data[idx]; + } + inline const C& operator[]( long long idx ) const + { + _assertBounds( idx ); + return data[idx]; + } + inline Array operator + ( int idx ) const + { + Array a; + a._data = _data; + a.data = data+idx; + a.min = min-idx; + a.max = max-idx; + return a; + } + inline Array operator + ( long long idx ) const + { + Array a; + a._data = _data; + a.data = data+idx; + a.min = min-idx; + a.max = max-idx; + return a; + } + inline Array operator + ( unsigned int idx ) const + { + Array a; + a._data = _data; + a.data = data+idx; + a.min = min-idx; + a.max = max-idx; + return a; + } + inline Array operator + ( unsigned long long idx ) const + { + Array a; + a._data = _data; + a.data = data+idx; + a.min = min-idx; + a.max = max-idx; + return a; + } + inline Array& operator += ( int idx ) + { + min -= idx; + max -= idx; + data += idx; + return (*this); + } + inline Array& operator += ( long long idx ) + { + min -= idx; + max -= idx; + data += idx; + return (*this); + } + inline Array& operator += ( unsigned int idx ) + { + min -= idx; + max -= idx; + data += idx; + return (*this); + } + inline Array& operator += ( unsigned long long idx ) + { + min -= idx; + max -= idx; + data += idx; + return (*this); + } + inline Array& operator ++ ( void ) { return (*this) += 1; } + inline Array operator++( int ){ Array< C > temp = (*this) ; (*this) +=1 ; return temp; } + Array operator - ( int idx ) const { return (*this) + (-idx); } + Array operator - ( long long idx ) const { return (*this) + (-idx); } + Array operator - ( unsigned int idx ) const { return (*this) + (-idx); } + Array operator - ( unsigned long long idx ) const { return (*this) + (-idx); } + Array& operator -= ( int idx ) { return (*this) += (-idx); } + Array& operator -= ( long long idx ) { return (*this) += (-idx); } + Array& operator -= ( unsigned int idx ) { return (*this) += (-idx); } + Array& operator -= ( unsigned long long idx ) { return (*this) += (-idx); } + Array& operator -- ( void ) { return (*this) -= 1; } + inline Array operator--( int ){ Array< C > temp = (*this) ; (*this) -=1 ; return temp; } + long long operator - ( const Array& a ) const { return ( long long )( data - a.data ); } + + void Free( void ) + { + if( _data ) + { + free( _data ); +#if FULL_ARRAY_DEBUG + _RemoveMemoryInfo( _data ); +#endif // FULL_ARRAY_DEBUG + } + (*this) = Array( ); + } + void Delete( void ) + { + if( _data ) + { + delete[] _data; +#if FULL_ARRAY_DEBUG + _RemoveMemoryInfo( _data ); +#endif // FULL_ARRAY_DEBUG + } + (*this) = Array( ); + } + C* pointer( void ){ return data; } + const C* pointer( void ) const { return data; } + bool operator !( void ) const { return data==NULL; } + operator bool( ) const { return data!=NULL; } +}; + +template< class C > +class ConstArray +{ + void _assertBounds( long long idx ) const + { + if( idx=max ) + { + fprintf( stderr , "ConstArray index out-of-bounds: %lld <= %lld < %lld\n" , min , idx , max ); + ASSERT( 0 ); + exit( 0 ); + } + } +protected: + const C *data; + long long min , max; +public: + long long minimum( void ) const { return min; } + long long maximum( void ) const { return max; } + + inline ConstArray( void ) + { + data = NULL; + min = max = 0; + } + inline ConstArray( const Array< C >& a ) + { + // [WARNING] Changing szC and szD to size_t causes some really strange behavior. + data = ( const C* )a.pointer( ); + min = a.minimum(); + max = a.maximum(); + } + template< class D > + inline ConstArray( const Array< D >& a ) + { + // [WARNING] Changing szC and szD to size_t causes some really strange behavior. + long long szC = ( long long ) sizeof( C ); + long long szD = ( long long ) sizeof( D ); + data = ( const C* )a.pointer( ); + min = ( a.minimum() * szD ) / szC; + max = ( a.maximum() * szD ) / szC; + if( min*szC!=a.minimum()*szD || max*szC!=a.maximum()*szD ) + { +// fprintf( stderr , "Could not convert const array [ %lld , %lld ] * %lld => [ %lld , %lld ] * %lld\n" , a.minimum() , a.maximum() , szD , min , max , szC ); + fprintf( stderr , "Could not convert const array [ %lld , %lld ] * %lld => [ %lld , %lld ] * %lld\n %lld %lld %lld\n" , a.minimum() , a.maximum() , szD , min , max , szC , a.minimum() , a.minimum()*szD , (a.minimum()*szD)/szC ); + ASSERT( 0 ); + exit( 0 ); + } + } + template< class D > + inline ConstArray( const ConstArray< D >& a ) + { + // [WARNING] Chaning szC and szD to size_t causes some really strange behavior. + long long szC = sizeof( C ); + long long szD = sizeof( D ); + data = ( const C*)a.pointer( ); + min = ( a.minimum() * szD ) / szC; + max = ( a.maximum() * szD ) / szC; + if( min*szC!=a.minimum()*szD || max*szC!=a.maximum()*szD ) + { + fprintf( stderr , "Could not convert array [ %lld , %lld ] * %lld => [ %lld , %lld ] * %lld\n" , a.minimum() , a.maximum() , szD , min , max , szC ); + ASSERT( 0 ); + exit( 0 ); + } + } + static ConstArray FromPointer( const C* data , long long max ) + { + ConstArray a; + a.data = data; + a.min = 0; + a.max = max; + return a; + } + static ConstArray FromPointer( const C* data , long long min , long long max ) + { + ConstArray a; + a.data = data; + a.min = min; + a.max = max; + return a; + } + + inline bool operator == ( const ConstArray< C >& a ) const { return data==a.data; } + inline bool operator != ( const ConstArray< C >& a ) const { return data!=a.data; } + inline bool operator == ( const C* c ) const { return data==c; } + inline bool operator != ( const C* c ) const { return data!=c; } + inline const C* operator -> ( void ) + { + _assertBounds( 0 ); + return data; + } + inline const C& operator[]( long long idx ) const + { + _assertBounds( idx ); + return data[idx]; + } + inline ConstArray operator + ( int idx ) const + { + ConstArray a; + a.data = data+idx; + a.min = min-idx; + a.max = max-idx; + return a; + } + inline ConstArray operator + ( long long idx ) const + { + ConstArray a; + a.data = data+idx; + a.min = min-idx; + a.max = max-idx; + return a; + } + inline ConstArray operator + ( unsigned int idx ) const + { + ConstArray a; + a.data = data+idx; + a.min = min-idx; + a.max = max-idx; + return a; + } + inline ConstArray operator + ( unsigned long long idx ) const + { + ConstArray a; + a.data = data+idx; + a.min = min-idx; + a.max = max-idx; + return a; + } + inline ConstArray& operator += ( int idx ) + { + min -= idx; + max -= idx; + data += idx; + return (*this); + } + inline ConstArray& operator += ( long long idx ) + { + min -= idx; + max -= idx; + data += idx; + return (*this); + } + inline ConstArray& operator += ( unsigned int idx ) + { + min -= idx; + max -= idx; + data += idx; + return (*this); + } + inline ConstArray& operator += ( unsigned long long idx ) + { + min -= idx; + max -= idx; + data += idx; + return (*this); + } + inline ConstArray& operator ++ ( void ) { return (*this) += 1; } + inline ConstArray operator++( int ){ ConstArray< C > temp = (*this) ; (*this) +=1 ; return temp; } + ConstArray operator - ( int idx ) const { return (*this) + (-idx); } + ConstArray operator - ( long long idx ) const { return (*this) + (-idx); } + ConstArray operator - ( unsigned int idx ) const { return (*this) + (-idx); } + ConstArray operator - ( unsigned long long idx ) const { return (*this) + (-idx); } + ConstArray& operator -= ( int idx ) { return (*this) += (-idx); } + ConstArray& operator -= ( long long idx ) { return (*this) += (-idx); } + ConstArray& operator -= ( unsigned int idx ) { return (*this) += (-idx); } + ConstArray& operator -= ( unsigned long long idx ) { return (*this) += (-idx); } + ConstArray& operator -- ( void ) { return (*this) -= 1; } + inline ConstArray operator--( int ){ ConstArray< C > temp = (*this) ; (*this) -=1 ; return temp; } + long long operator - ( const ConstArray& a ) const { return ( long long )( data - a.data ); } + long long operator - ( const Array< C >& a ) const { return ( long long )( data - a.pointer() ); } + + const C* pointer( void ) const { return data; } + bool operator !( void ) { return data==NULL; } + operator bool( ) { return data!=NULL; } +}; + +#if FULL_ARRAY_DEBUG +inline void PrintMemoryInfo( void ){ for( size_t i=0 ; i +Array< C > memcpy( Array< C > destination , const void* source , size_t size ) +{ + if( size>destination.maximum()*sizeof(C) ) + { + fprintf( stderr , "Size of copy exceeds destination maximum: %lld > %lld\n" , ( long long )( size ) , ( long long )( destination.maximum()*sizeof( C ) ) ); + ASSERT( 0 ); + exit( 0 ); + } + if( size ) memcpy( &destination[0] , source , size ); + return destination; +} +template< class C , class D > +Array< C > memcpy( Array< C > destination , Array< D > source , size_t size ) +{ + if( size>destination.maximum()*sizeof( C ) ) + { + fprintf( stderr , "Size of copy exceeds destination maximum: %lld > %lld\n" , ( long long )( size ) , ( long long )( destination.maximum()*sizeof( C ) ) ); + ASSERT( 0 ); + exit( 0 ); + } + if( size>source.maximum()*sizeof( D ) ) + { + fprintf( stderr , "Size of copy exceeds source maximum: %lld > %lld\n" , ( long long )( size ) , ( long long )( source.maximum()*sizeof( D ) ) ); + ASSERT( 0 ); + exit( 0 ); + } + if( size ) memcpy( &destination[0] , &source[0] , size ); + return destination; +} +template< class C , class D > +Array< C > memcpy( Array< C > destination , ConstArray< D > source , size_t size ) +{ + if( size>destination.maximum()*sizeof( C ) ) + { + fprintf( stderr , "Size of copy exceeds destination maximum: %lld > %lld\n" , ( long long )( size ) , ( long long )( destination.maximum()*sizeof( C ) ) ); + ASSERT( 0 ); + exit( 0 ); + } + if( size>source.maximum()*sizeof( D ) ) + { + fprintf( stderr , "Size of copy exceeds source maximum: %lld > %lld\n" , ( long long )( size ) , ( long long )( source.maximum()*sizeof( D ) ) ); + ASSERT( 0 ); + exit( 0 ); + } + if( size ) memcpy( &destination[0] , &source[0] , size ); + return destination; +} +template< class D > +void* memcpy( void* destination , Array< D > source , size_t size ) +{ + if( size>source.maximum()*sizeof( D ) ) + { + fprintf( stderr , "Size of copy exceeds source maximum: %lld > %lld\n" , ( long long )( size ) , ( long long )( source.maximum()*sizeof( D ) ) ); + ASSERT( 0 ); + exit( 0 ); + } + if( size ) memcpy( destination , &source[0] , size ); + return destination; +} +template< class D > +void* memcpy( void* destination , ConstArray< D > source , size_t size ) +{ + if( size>source.maximum()*sizeof( D ) ) + { + fprintf( stderr , "Size of copy exceeds source maximum: %lld > %lld\n" , ( long long )( size ) , ( long long )( source.maximum()*sizeof( D ) ) ); + ASSERT( 0 ); + exit( 0 ); + } + if( size ) memcpy( destination , &source[0] , size ); + return destination; +} +template< class C > +Array< C > memset( Array< C > destination , int value , size_t size ) +{ + if( size>destination.maximum()*sizeof( C ) ) + { + fprintf( stderr , "Size of set exceeds destination maximum: %lld > %lld\n" , ( long long )( size ) , ( long long )( destination.maximum()*sizeof( C ) ) ); + ASSERT( 0 ); + exit( 0 ); + } + if( size ) memset( &destination[0] , value , size ); + return destination; +} + +template< class C > +size_t fread( Array< C > destination , size_t eSize , size_t count , FILE* fp ) +{ + if( count*eSize>destination.maximum()*sizeof( C ) ) + { + fprintf( stderr , "Size of read exceeds source maximum: %lld > %lld\n" , ( long long )( count*eSize ) , ( long long )( destination.maximum()*sizeof( C ) ) ); + ASSERT( 0 ); + exit( 0 ); + } + return fread( &destination[0] , eSize , count , fp ); +} +template< class C > +size_t fwrite( Array< C > source , size_t eSize , size_t count , FILE* fp ) +{ + if( count*eSize>source.maximum()*sizeof( C ) ) + { + fprintf( stderr , "Size of write exceeds source maximum: %lld > %lld\n" , ( long long )( count*eSize ) , ( long long )( source.maximum()*sizeof( C ) ) ); + ASSERT( 0 ); + exit( 0 ); + } + return fwrite( &source[0] , eSize , count , fp ); +} +template< class C > +size_t fwrite( ConstArray< C > source , size_t eSize , size_t count , FILE* fp ) +{ + if( count*eSize>source.maximum()*sizeof( C ) ) + { + fprintf( stderr , "Size of write exceeds source maximum: %lld > %lld\n" , ( long long )( count*eSize ) , ( long long )( source.maximum()*sizeof( C ) ) ); + ASSERT( 0 ); + exit( 0 ); + } + return fwrite( &source[0] , eSize , count , fp ); +} +template< class C > +void qsort( Array< C > base , size_t numElements , size_t elementSize , int (*compareFunction)( const void* , const void* ) ) +{ + if( sizeof(C)!=elementSize ) + { + fprintf( stderr , "Element sizes differ: %lld != %lld\n" , ( long long )( sizeof(C) ) , ( long long )( elementSize ) ); + ASSERT( 0 ); + exit( 0 ); + } + if( base.minimum()>0 || base.maximum() +struct BSplineElementCoefficients +{ + int coeffs[Degree+1]; + BSplineElementCoefficients( void ){ memset( coeffs , 0 , sizeof( int ) * ( Degree+1 ) ); } + int& operator[]( int idx ){ return coeffs[idx]; } + const int& operator[]( int idx ) const { return coeffs[idx]; } +}; + +// This class represents a function on the the interval, partitioned into "res" blocks. +// On each block, the function is a degree-Degree polynomial, represented by the coefficients +// in the associated BSplineElementCoefficients. +// [NOTE] This representation of a function is agnostic to the type of boundary conditions (though the constructor is not). +template< int Degree > +struct BSplineElements : public std::vector< BSplineElementCoefficients< Degree > > +{ + static const bool _Primal = (Degree&1)==1; + static const int _Off = (Degree+1)/2; + static int _ReflectLeft ( int offset , int res ); + static int _ReflectRight( int offset , int res ); + static int _RotateLeft ( int offset , int res ); + static int _RotateRight ( int offset , int res ); + template< bool Left > void _addPeriodic( int offset , bool negate ); +public: + // Coefficients are ordered as "/" "-" "\" + // [WARNING] This is the opposite of the order in Polynomial::BSplineComponent + int denominator; + + BSplineElements( void ) { denominator = 1; } + BSplineElements( int res , int offset , bool dirichlet ); + + void upSample( BSplineElements& high ) const; + void differentiate( BSplineElements< Degree-1 >& d ) const; + + void print( FILE* fp=stdout ) const + { + for( int i=0 ; i >::size() ; i++ ) + { + printf( "%d]" , i ); + for( int j=0 ; j<=Degree ; j++ ) printf( " %d" , (*this)[i][j] ); + printf( " (%d)\n" , denominator ); + } + } +}; +#define BSPLINE_SET_BOUNDS( name , s , e ) \ + static const int name ## Start = (s); \ + static const int name ## End = (e); \ + static const int name ## Size = (e)-(s)+1 + +// Assumes that x is non-negative +#define _FLOOR_OF_HALF( x ) ( (x) >>1 ) +#define _CEIL_OF_HALF( x ) ( ( (x)+1 )>>1 ) +// Done with the assumption +#define FLOOR_OF_HALF( x ) ( (x)<0 ? - _CEIL_OF_HALF( -(x) ) : _FLOOR_OF_HALF( x ) ) +#define CEIL_OF_HALF( x ) ( (x)<0 ? - _FLOOR_OF_HALF( -(x) ) : _CEIL_OF_HALF( x ) ) +#define SMALLEST_INTEGER_LARGER_THAN_HALF( x ) ( CEIL_OF_HALF( (x)+1 ) ) +#define LARGEST_INTEGER_SMALLER_THAN_HALF( x ) ( FLOOR_OF_HALF( (x)-1 ) ) +#define SMALLEST_INTEGER_LARGER_THAN_OR_EQUAL_TO_HALF( x ) ( CEIL_OF_HALF( x ) ) +#define LARGEST_INTEGER_SMALLER_THAN_OR_EQUAL_TO_HALF( x ) ( FLOOR_OF_HALF( x ) ) + +template< int Degree > +class BSplineEvaluationData +{ +public: + BSplineEvaluationData( void ); + static double Value( int depth , int off , double s , bool dirichlet , bool derivative ); + + static int Dimension( int depth ){ return ( 1< [-(Degree+1-Inset) , (Degree+1+Inset) ] CONTAINS [ J-(Degree+1-Inset)/2 , J+(Degree+1+Inset)/2 ] + // Which is the same as the smallest/largest integers J such that: + // J - (Degree+1-Inset)/2 >= -(Degree+1-Inset) | J + (Degree+1+Inset)/2 <= (Degree+1+Inset) + // <=> J >= -(Degree+1-Inset)/2 | J <= (Degree+1+Inset)/2 + BSPLINE_SET_BOUNDS( UpSample , - ( Degree + 1 - Inset ) / 2 , ( Degree + 1 + Inset ) /2 ); + + // Setting I=0/1, we are looking for the smallest/largest integers J such that: + // Support( J ) CONTAINS Support( 0/1 ) + // <=> [ 2*J - (Degree+1-Inset) , 2*J + (Degree+1+Inset) ] CONTAINS [ 0/1 - (Degree+1-Inset)/2 , 0/1 + (Degree+1+Inset)/2 ] + // Which is the same as the smallest/largest integers J such that: + // 2*J + (Degree+1+Inset) >= 0/1 + (Degree+1+Inset)/2 | 2*J - (Degree+1-Inset) <= 0/1 - (Degree+1-Inset)/2 + // <=> 2*J >= 0/1 - (Degree+1+Inset)/2 | 2*J <= 0/1 + (Degree+1-Inset)/2 + BSPLINE_SET_BOUNDS( DownSample0 , SMALLEST_INTEGER_LARGER_THAN_OR_EQUAL_TO_HALF( 0 - ( Degree + 1 + Inset ) / 2 ) , LARGEST_INTEGER_SMALLER_THAN_OR_EQUAL_TO_HALF( 0 + ( Degree + 1 - Inset ) / 2 ) ); + BSPLINE_SET_BOUNDS( DownSample1 , SMALLEST_INTEGER_LARGER_THAN_OR_EQUAL_TO_HALF( 1 - ( Degree + 1 + Inset ) / 2 ) , LARGEST_INTEGER_SMALLER_THAN_OR_EQUAL_TO_HALF( 1 + ( Degree + 1 - Inset ) / 2 ) ); + static const int DownSampleStart[] , DownSampleEnd[] , DownSampleSize[]; + + // Note that this struct stores the components in left-to-right order + struct BSplineComponents + { + protected: + Polynomial< Degree > _polys[Degree+1]; + public: + BSplineComponents( void ){ ; } + BSplineComponents( int depth , int offset , bool dirichlet ); + const Polynomial< Degree >& operator[] ( int idx ) const { return _polys[idx]; } + void printnl( void ) const { for( int d=0 ; d<=Degree ; d++ ) printf( "[%d] " , d ) , _polys[d].printnl(); } + }; + struct BSplineUpSamplingCoefficients + { + protected: + int _coefficients[ UpSampleSize ]; + public: + BSplineUpSamplingCoefficients( void ){ ; } + BSplineUpSamplingCoefficients( int depth , int offset , bool dirichlet ); + double operator[] ( int idx ){ return (double)_coefficients[idx] / (1<::Dimension( depth ); + if ( offset=dim-Stop ) return Start + 1 + offset - ( dim-Stop ); + else return Start; + } + struct Evaluator + { + protected: + friend BSplineEvaluationData; + int _depth; + double _ccValues[2][Size][SupportSize]; + public: + double value( int fIdx , int cIdx , bool d ) const; + int depth( void ) const { return _depth; } + }; + struct ChildEvaluator + { + protected: + friend BSplineEvaluationData; + int _parentDepth; + double _pcValues[2][Size][ChildSupportSize]; + public: + double value( int fIdx , int cIdx , bool d ) const; + int parentDepth( void ) const { return _parentDepth; } + int childDepth( void ) const { return _parentDepth+1; } + }; + }; + static void SetCenterEvaluator( typename CenterEvaluator::Evaluator& evaluator , int depth , bool dirichlet ); + static void SetChildCenterEvaluator( typename CenterEvaluator::ChildEvaluator& evaluator , int parentDepth , bool dirichlet ); + + struct CornerEvaluator + { + static const int Start = -SupportStart , Stop = SupportEnd , Size = Start + Stop + 1; + + static const int Index( int depth , int offset ) + { + int dim = BSplineEvaluationData< Degree >::Dimension( depth ); + if ( offset=dim-Stop ) return Start + 1 + offset - ( dim-Stop ); + else return Start; + } + struct Evaluator + { + protected: + friend BSplineEvaluationData; + int _depth; + double _ccValues[2][Size][CornerSize]; + public: + double value( int fIdx , int cIdx , bool d ) const; + int depth( void ) const { return _depth; } + }; + struct ChildEvaluator + { + protected: + friend BSplineEvaluationData; + int _parentDepth; + double _pcValues[2][Size][ChildCornerSize]; + public: + double value( int fIdx , int cIdx , bool d ) const; + int parentDepth( void ) const { return _parentDepth; } + int childDepth( void ) const { return _parentDepth+1; } + }; + }; + static void SetCornerEvaluator( typename CornerEvaluator::Evaluator& evaluator , int depth , bool dirichlet ); + static void SetChildCornerEvaluator( typename CornerEvaluator::ChildEvaluator& evaluator , int parentDepth , bool dirichlet ); + + struct Evaluator + { + typename CenterEvaluator::Evaluator centerEvaluator; + typename CornerEvaluator::Evaluator cornerEvaluator; + double centerValue( int fIdx , int cIdx , bool d ) const { return centerEvaluator.value( fIdx , cIdx , d ); } + double cornerValue( int fIdx , int cIdx , bool d ) const { return cornerEvaluator.value( fIdx , cIdx , d ); } + }; + static void SetEvaluator( Evaluator& evaluator , int depth , bool dirichlet ){ SetCenterEvaluator( evaluator.centerEvaluator , depth , dirichlet ) , SetCornerEvaluator( evaluator.cornerEvaluator , depth , dirichlet ); } + struct ChildEvaluator + { + typename CenterEvaluator::ChildEvaluator centerEvaluator; + typename CornerEvaluator::ChildEvaluator cornerEvaluator; + double centerValue( int fIdx , int cIdx , bool d ) const { return centerEvaluator.value( fIdx , cIdx , d ); } + double cornerValue( int fIdx , int cIdx , bool d ) const { return cornerEvaluator.value( fIdx , cIdx , d ); } + }; + static void SetChildEvaluator( ChildEvaluator& evaluator , int depth , bool dirichlet ){ SetChildCenterEvaluator( evaluator.centerEvaluator , depth , dirichlet ) , SetChildCornerEvaluator( evaluator.cornerEvaluator , depth , dirichlet ); } + + struct UpSampleEvaluator + { + static const int Start = - SupportStart , Stop = SupportEnd , Size = Start + Stop + 1; + static const int Index( int depth , int offset ) + { + int dim = BSplineEvaluationData< Degree >::Dimension( depth ); + if ( offset=dim-Stop ) return Start + 1 + offset - ( dim-Stop ); + else return Start; + } + protected: + friend BSplineEvaluationData; + int _lowDepth; + double _pcValues[Size][UpSampleSize]; + public: + double value( int pIdx , int cIdx ) const; + int lowDepth( void ) const { return _lowDepth; } + }; + static void SetUpSampleEvaluator( UpSampleEvaluator& evaluator , int lowDepth , bool dirichlet ); +}; +template< int Degree > const int BSplineEvaluationData< Degree >::DownSampleStart[] = { DownSample0Start , DownSample1Start }; +template< int Degree > const int BSplineEvaluationData< Degree >::DownSampleEnd [] = { DownSample0End , DownSample1End }; +template< int Degree > const int BSplineEvaluationData< Degree >::DownSampleSize [] = { DownSample0Size , DownSample1Size }; + +template< int Degree1 , int Degree2 > +class BSplineIntegrationData +{ +public: + static double Dot( int depth1 , int off1 , bool dirichlet1 , bool d1 , int depth2 , int off2 , bool dirichlet2 , bool d2 ); + // An index is interiorly overlapped if the support of its overlapping neighbors is in the range [0,1<::SupportStart , end = (1<::SupportEnd; } + + typedef BSplineEvaluationData< Degree1 > EData1; + typedef BSplineEvaluationData< Degree2 > EData2; + BSPLINE_SET_BOUNDS( Overlap , EData1:: SupportStart - EData2::SupportEnd , EData1:: SupportEnd - EData2::SupportStart ); + BSPLINE_SET_BOUNDS( ChildOverlap , EData1::ChildSupportStart - EData2::SupportEnd , EData1::ChildSupportEnd - EData2::SupportStart ); + BSPLINE_SET_BOUNDS( OverlapSupport , OverlapStart + EData2::SupportStart , OverlapEnd + EData2::SupportEnd ); + BSPLINE_SET_BOUNDS( ChildOverlapSupport , ChildOverlapStart + EData2::SupportStart , ChildOverlapEnd + EData2::SupportEnd ); + + // Setting I=0/1, we are looking for the smallest/largest integers J such that: + // Support( 2*J ) * 2 INTERSECTION Support( 0/1 ) NON-EMPTY + // <=> [ 2*J - (Degree2+1-Inset2) , 2*J + (Degree2+1+Inset2) ] INTERSECTION [ 0/1 - (Degree1+1-Inset1)/2 , 0/1 + (Degree1+1+Inset1)/2 ] NON-EMPTY + // Which is the same as the smallest/largest integers J such that: + // 0/1 - (Degree1+1-Inset1)/2 < 2*J + (Degree2+1+Inset2) | 0/1 + (Degree1+1+Inset1)/2 > 2*J - (Degree2+1-Inset2) + // <=> 2*J > 0/1 - ( 2*Degree2 + Degree1 + 3 + 2*Inset2 - Inset1 ) / 2 | 2*J < 0/1 + ( 2*Degree2 + Degree1 + 3 - 2*Inset2 + Inset1 ) / 2 + BSPLINE_SET_BOUNDS( ParentOverlap0 , SMALLEST_INTEGER_LARGER_THAN_HALF( 0 - ( 2*Degree2 + Degree1 + 3 + 2*EData2::Inset - EData1::Inset ) / 2 ) , LARGEST_INTEGER_SMALLER_THAN_HALF( 0 + ( 2*Degree2 + Degree1 + 3 - 2*EData2::Inset + EData1::Inset ) / 2 ) ); + BSPLINE_SET_BOUNDS( ParentOverlap1 , SMALLEST_INTEGER_LARGER_THAN_HALF( 1 - ( 2*Degree2 + Degree1 + 3 + 2*EData2::Inset - EData1::Inset ) / 2 ) , LARGEST_INTEGER_SMALLER_THAN_HALF( 1 + ( 2*Degree2 + Degree1 + 3 - 2*EData2::Inset + EData1::Inset ) / 2 ) ); + static const int ParentOverlapStart[] , ParentOverlapEnd[] , ParentOverlapSize[]; + + struct FunctionIntegrator + { + static const int Start = - OverlapSupportStart , Stop = OverlapSupportEnd , Size = Start + Stop + 1; + static const int Index( int depth , int offset ) + { + int dim = BSplineEvaluationData< Degree2 >::Dimension( depth ); + if ( offset=dim-Stop ) return Start + 1 + offset - ( dim-Stop ); + else return Start; + } + struct Integrator + { + protected: + friend BSplineIntegrationData; + int _depth; + double _ccIntegrals[2][2][Size][OverlapSize]; + public: + double dot( int fIdx1 , int fidx2 , bool d1 , bool d2 ) const; + int depth( void ) const { return _depth; } + }; + struct ChildIntegrator + { + protected: + friend BSplineIntegrationData; + int _parentDepth; + double _pcIntegrals[2][2][Size][ChildOverlapSize]; + public: + double dot( int fIdx1 , int fidx2 , bool d1 , bool d2 ) const; + int parentDepth( void ) const { return _parentDepth; } + int childDepth( void ) const { return _parentDepth+1; } + }; + }; + static void SetIntegrator( typename FunctionIntegrator::Integrator& integrator , int depth , bool dirichlet1 , bool dirichlet2 ); + static void SetChildIntegrator( typename FunctionIntegrator::ChildIntegrator& integrator , int parentDepth , bool dirichlet1 , bool dirichlet2 ); +}; +template< int Degree1 , int Degree2 > const int BSplineIntegrationData< Degree1 , Degree2 >::ParentOverlapStart[] = { ParentOverlap0Start , ParentOverlap1Start }; +template< int Degree1 , int Degree2 > const int BSplineIntegrationData< Degree1 , Degree2 >::ParentOverlapEnd [] = { ParentOverlap0End , ParentOverlap1End }; +template< int Degree1 , int Degree2 > const int BSplineIntegrationData< Degree1 , Degree2 >::ParentOverlapSize [] = { ParentOverlap0Size , ParentOverlap1Size }; +#undef BSPLINE_SET_BOUNDS +#undef _FLOOR_OF_HALF +#undef _CEIL_OF_HALF +#undef FLOOR_OF_HALF +#undef CEIL_OF_HALF +#undef SMALLEST_INTEGER_LARGER_THAN_HALF +#undef LARGEST_INTEGER_SMALLER_THAN_HALF +#undef SMALLEST_INTEGER_LARGER_THAN_OR_EQUAL_TO_HALF +#undef LARGEST_INTEGER_SMALLER_THAN_OR_EQUAL_TO_HALF + +template< int Degree > +class BSplineData +{ + bool _dirichlet; +public: + + inline static int Centers ( int depth ){ return (1<0) ? TotalFunctionCount(depth-1) : 0 , fEnd = TotalFunctionCount(depth); } + inline static void SampleSpan( int depth , int& sStart , int& sEnd ){ sStart = (depth>0) ? TotalSampleCount(depth-1) : 0 , sEnd = TotalSampleCount(depth); } + + inline static int RemapOffset( int depth , int idx , bool& reflect ); + + int depth; + size_t functionCount , sampleCount; + Pointer( typename BSplineEvaluationData< Degree >::BSplineComponents ) baseBSplines; + + BSplineData( void ); + + void set( int maxDepth , bool dirichlet=false ); +}; + +template< int Degree1 , int Degree2 > void SetBSplineElementIntegrals( double integrals[Degree1+1][Degree2+1] ); + + +#include "BSplineData.inl" +#endif // BSPLINE_DATA_INCLUDED \ No newline at end of file diff --git a/colmap/include/colmap/lib/PoissonRecon/BSplineData.inl b/colmap/include/colmap/lib/PoissonRecon/BSplineData.inl new file mode 100644 index 0000000000000000000000000000000000000000..01f6fe85a56ecdf8a44cb9b4cd68db963f6ec476 --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/BSplineData.inl @@ -0,0 +1,474 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +/////////////////////////// +// BSplineEvaluationData // +/////////////////////////// +template< int Degree > +double BSplineEvaluationData< Degree >::Value( int depth , int off , double s , bool dirichlet , bool derivative ) +{ + if( s<0 || s>1 ) return 0.; + + int dim = Dimension(depth) , res = 1<=dim ) return 0; + + BSplineComponents components = BSplineComponents( depth , off , dirichlet ); + + // [NOTE] This is an ugly way to ensure that when s=1 we evaluate using a B-Spline component within the valid range. + int ii = std::max< int >( 0 , std::min< int >( res-1 , (int)floor( s * res ) ) ) - off; + + if( iiSupportEnd ) return 0; + if( derivative ) return components[ii-SupportStart].derivative()(s); + else return components[ii-SupportStart](s); +} +template< int Degree > +void BSplineEvaluationData< Degree >::SetCenterEvaluator( typename CenterEvaluator::Evaluator& evaluator , int depth , bool dirichlet ) +{ + evaluator._depth = depth; + int dim = BSplineEvaluationData< Degree >::Dimension( depth ) , res = 1< +void BSplineEvaluationData< Degree >::SetChildCenterEvaluator( typename CenterEvaluator::ChildEvaluator& evaluator , int parentDepth , bool dirichlet ) +{ + evaluator._parentDepth = parentDepth; + int dim = BSplineEvaluationData< Degree >::Dimension( parentDepth ) , res = 1<<(parentDepth+1); + for( int i=0 ; i +double BSplineEvaluationData< Degree >::CenterEvaluator::Evaluator::value( int fIdx , int cIdx , bool d ) const +{ + int dd = cIdx-fIdx , res = 1<<(_depth) , dim = Dimension(_depth); + if( cIdx<0 || fIdx<0 || cIdx>=res || fIdx>=dim || ddSupportEnd ) return 0; + return _ccValues[d?1:0][ CenterEvaluator::Index( _depth , fIdx ) ][dd-SupportStart]; +} +template< int Degree > +double BSplineEvaluationData< Degree >::CenterEvaluator::ChildEvaluator::value( int fIdx , int cIdx , bool d ) const +{ + int dd = cIdx-2*fIdx , res = 1<<(_parentDepth+1) , dim = Dimension(_parentDepth); + if( cIdx<0 || fIdx<0 || cIdx>=res || fIdx>=dim || ddChildSupportEnd ) return 0; + return _pcValues[d?1:0][ CenterEvaluator::Index( _parentDepth , fIdx ) ][dd-ChildSupportStart]; +} +template< int Degree > +void BSplineEvaluationData< Degree >::SetCornerEvaluator( typename CornerEvaluator::Evaluator& evaluator , int depth , bool dirichlet ) +{ + evaluator._depth = depth; + int dim = BSplineEvaluationData< Degree >::Dimension( depth ) , res = 1< +void BSplineEvaluationData< Degree >::SetChildCornerEvaluator( typename CornerEvaluator::ChildEvaluator& evaluator , int parentDepth , bool dirichlet ) +{ + evaluator._parentDepth = parentDepth; + int dim = BSplineEvaluationData< Degree >::Dimension( parentDepth ) , res = 1<<(parentDepth+1); + for( int i=0 ; i +void BSplineEvaluationData< Degree >::SetUpSampleEvaluator( UpSampleEvaluator& evaluator , int lowDepth , bool dirichlet ) +{ + evaluator._lowDepth = lowDepth; + int lowDim = Dimension(lowDepth); + for( int i=0 ; i +double BSplineEvaluationData< Degree >::CornerEvaluator::Evaluator::value( int fIdx , int cIdx , bool d ) const +{ + int dd = cIdx-fIdx , res = ( 1<<_depth ) + 1 , dim = Dimension(_depth); + if( cIdx<0 || fIdx<0 || cIdx>=res || fIdx>=dim || ddCornerEnd ) return 0; + return _ccValues[d?1:0][ CornerEvaluator::Index( _depth , fIdx ) ][dd-CornerStart]; +} +template< int Degree > +double BSplineEvaluationData< Degree >::CornerEvaluator::ChildEvaluator::value( int fIdx , int cIdx , bool d ) const +{ + int dd = cIdx-2*fIdx , res = ( 1<<(_parentDepth+1) ) + 1 , dim = Dimension(_parentDepth); + if( cIdx<0 || fIdx<0 || cIdx>=res || fIdx>=dim || ddChildCornerEnd ) return 0; + return _pcValues[d?1:0][ CornerEvaluator::Index( _parentDepth , fIdx ) ][dd-ChildCornerStart]; +} +template< int Degree > +double BSplineEvaluationData< Degree >::UpSampleEvaluator::value( int pIdx , int cIdx ) const +{ + int dd = cIdx-2*pIdx , pDim = Dimension( _lowDepth ) , cDim = Dimension( _lowDepth+1 ); + if( cIdx<0 || pIdx<0 || cIdx>=cDim || pIdx>=pDim || ddUpSampleEnd ) return 0; + return _pcValues[ UpSampleEvaluator::Index( _lowDepth , pIdx ) ][dd-UpSampleStart]; +} + +////////////////////////////////////////////// +// BSplineEvaluationData::BSplineComponents // +////////////////////////////////////////////// +template< int Degree > +BSplineEvaluationData< Degree >::BSplineComponents::BSplineComponents( int depth , int offset , bool dirichlet ) +{ + int res = 1< elements( res , offset , dirichlet ); + + // The first index is the position, the second is the element type + Polynomial< Degree > components[Degree+1][Degree+1]; + // Generate the elements that can appear in the base function corresponding to the base function at (depth,offset) = (0,0) + for( int d=0 ; d<=Degree ; d++ ) for( int dd=0 ; dd<=Degree ; dd++ ) components[d][dd] = Polynomial< Degree >::BSplineComponent( Degree-dd ).shift( -( (Degree+1)/2 ) + d ); + + // Now adjust to the desired depth and offset + double width = 1. / res; + for( int d=0 ; d<=Degree ; d++ ) for( int dd=0 ; dd<=Degree ; dd++ ) components[d][dd] = components[d][dd].scale( width ).shift( width*offset ); + + // Now write in the polynomials + for( int d=0 ; d<=Degree ; d++ ) + { + int idx = offset + SupportStart + d; + _polys[d] = Polynomial< Degree >(); + + if( idx>=0 && idx +BSplineEvaluationData< Degree >::BSplineUpSamplingCoefficients::BSplineUpSamplingCoefficients( int depth , int offset , bool dirichlet ) +{ + // [ 1/8 1/2 3/4 1/2 1/8] + // [ 1 , 1 ] -> [ 3/4 , 1/2 , 1/8 ] + [ 1/8 , 1/2 , 3/4 ] = [ 7/8 , 1 , 7/8 ] + int dim = Dimension(depth) , _dim = Dimension(depth+1); + bool reflect; + offset = BSplineData< Degree >::RemapOffset( depth , offset , reflect ); + int multiplier = ( dirichlet && reflect ) ? -1 : 1; + bool useReflected = Inset || ( offset % ( dim-1 ) ); + int b[ UpSampleSize ]; + Polynomial< Degree+1 >::BinomialCoefficients( b ); + + // Clear the values + memset( _coefficients , 0 , sizeof(int) * UpSampleSize ); + + // Get the array of coefficients, relative to the origin + int* coefficients = _coefficients - ( 2*offset + UpSampleStart ); + for( int i=UpSampleStart ; i<=UpSampleEnd ; i++ ) + { + int _offset = 2*offset+i; + _offset = BSplineData< Degree >::RemapOffset( depth+1 , _offset , reflect ); + if( useReflected || !reflect ) + { + int _multiplier = multiplier * ( ( dirichlet && reflect ) ? -1 : 1 ); + coefficients[ _offset ] += b[ i-UpSampleStart ] * _multiplier; + } + // If we are not inset and we are at the boundary, use the reflection as well + if( !Inset && ( offset % (dim-1) ) && !( _offset % (_dim-1) ) ) + { + _offset = BSplineData< Degree >::RemapOffset( depth+1 , _offset , reflect ); + int _multiplier = multiplier * ( ( dirichlet && reflect ) ? -1 : 1 ); + if( dirichlet ) _multiplier *= -1; + coefficients[ _offset ] += b[ i-UpSampleStart ] * _multiplier; + } + } +} + +//////////////////////////// +// BSplineIntegrationData // +//////////////////////////// +template< int Degree1 , int Degree2 > +double BSplineIntegrationData< Degree1 , Degree2 >::Dot( int depth1 , int off1 , bool dirichlet1 , bool d1 , int depth2 , int off2 , bool dirichlet2 , bool d2 ) +{ + const int _Degree1 = (d1 ? (Degree1-1) : Degree1) , _Degree2 = (d2 ? (Degree2-1) : Degree2); + int sums[ Degree1+1 ][ Degree2+1 ]; + + int depth = std::max< int >( depth1 , depth2 ); + + BSplineElements< Degree1 > b1( 1< b2( 1< b; + while( depth1 b; + while( depth2 db1; + BSplineElements< Degree2-1 > db2; + b1.differentiate( db1 ) , b2.differentiate( db2 ); + + int start1=-1 , end1=-1 , start2=-1 , end2=-1; + for( int i=0 ; i=end2 || start2>=end1 ) return 0.; + int start = std::max< int >( start1 , start2 ) , end = std::min< int >( end1 , end2 ); + memset( sums , 0 , sizeof( sums ) ); + + // Iterate over the support + for( int i=start ; i( integrals ); + for( int j=0 ; j<=_Degree1 ; j++ ) for( int k=0 ; k<=_Degree2 ; k++ ) _dot += integrals[j][k] * sums[j][k]; + } + else if( d1 ) + { + double integrals[ Degree1 ][ Degree2+1 ]; + SetBSplineElementIntegrals< Degree1-1 , Degree2 >( integrals ); + for( int j=0 ; j<=_Degree1 ; j++ ) for( int k=0 ; k<=_Degree2 ; k++ ) _dot += integrals[j][k] * sums[j][k]; + } + else if( d2 ) + { + double integrals[ Degree1+1 ][ Degree2 ]; + SetBSplineElementIntegrals< Degree1 , Degree2-1 >( integrals ); + for( int j=0 ; j<=_Degree1 ; j++ ) for( int k=0 ; k<=_Degree2 ; k++ ) _dot += integrals[j][k] * sums[j][k]; + } + else + { + double integrals[ Degree1+1 ][ Degree2+1 ]; + SetBSplineElementIntegrals< Degree1 , Degree2 >( integrals ); + for( int j=0 ; j<=_Degree1 ; j++ ) for( int k=0 ; k<=_Degree2 ; k++ ) _dot += integrals[j][k] * sums[j][k]; + } + + _dot /= b1.denominator; + _dot /= b2.denominator; + if ( d1 && d2 ) return _dot * (1< +void BSplineIntegrationData< Degree1, Degree2 >::SetIntegrator( typename FunctionIntegrator::Integrator& integrator , int depth , bool dirichlet1 , bool dirichlet2 ) +{ + integrator._depth = depth; + int dim = BSplineEvaluationData< Degree2 >::Dimension( depth ); + for( int i=0 ; i +void BSplineIntegrationData< Degree1, Degree2 >::SetChildIntegrator( typename FunctionIntegrator::ChildIntegrator& integrator , int parentDepth , bool dirichlet1 , bool dirichlet2 ) +{ + integrator._parentDepth = parentDepth; + int dim = BSplineEvaluationData< Degree2 >::Dimension( parentDepth ); + for( int i=0 ; i +double BSplineIntegrationData< Degree1 , Degree2 >::FunctionIntegrator::Integrator::dot( int off1 , int off2 , bool d1 , bool d2 ) const +{ + int d = off2-off1 , dim1 = BSplineEvaluationData< Degree1 >::Dimension( _depth ) , dim2 = BSplineEvaluationData< Degree2 >::Dimension( _depth ); + if( off1<0 || off2<0 || off1>=dim1 || off2>=dim2 || dOverlapEnd ) return 0; + return _ccIntegrals[d1?1:0][d2?1:0][ FunctionIntegrator::Index( _depth , off1 ) ][d-OverlapStart]; +} +template< int Degree1 , int Degree2 > +double BSplineIntegrationData< Degree1 , Degree2 >::FunctionIntegrator::ChildIntegrator::dot( int off1 , int off2 , bool d1 , bool d2 ) const +{ + int d = off2-2*off1 , dim1 = BSplineEvaluationData< Degree1 >::Dimension( _parentDepth ) , dim2 = BSplineEvaluationData< Degree2 >::Dimension( _parentDepth+1 ); + if( off1<0 || off2<0 || off1>=dim1 || off2>=dim2 || dChildOverlapEnd ) return 0; + return _pcIntegrals[d1?1:0][d2?1:0][ FunctionIntegrator::Index( _parentDepth , off1 ) ][d-ChildOverlapStart]; +} +///////////////// +// BSplineData // +///////////////// +#define MODULO( A , B ) ( (A)<0 ? ( (B)-((-(A))%(B)) ) % (B) : (A) % (B) ) +template< int Degree > +int BSplineData< Degree >::RemapOffset( int depth , int offset , bool& reflect ) +{ + const int I = ( Degree&1 ) ? 0 : 1; + int dim = Dimension( depth ); + offset = MODULO( offset , 2*(dim-1+I) ); + reflect = offset>=dim; + if( reflect ) return 2*(dim-1+I) - (offset+I); + else return offset; +} +#undef MODULO + +template< int Degree > BSplineData< Degree >::BSplineData( void ){ functionCount = sampleCount = 0; } + + +template< int Degree > +void BSplineData< Degree >::set( int maxDepth , bool dirichlet ) +{ + _dirichlet = dirichlet; + + depth = maxDepth; + functionCount = TotalFunctionCount( depth ); + sampleCount = TotalSampleCount( depth ); + baseBSplines = NewPointer< typename BSplineEvaluationData< Degree >::BSplineComponents >( functionCount ); + + for( size_t i=0 ; i::BSplineComponents( d , off , _dirichlet ); + } +} + +///////////////////// +// BSplineElements // +///////////////////// +template< int Degree > +BSplineElements< Degree >::BSplineElements( int res , int offset , bool dirichlet ) +{ + denominator = 1; + std::vector< BSplineElementCoefficients< Degree > >::resize( res , BSplineElementCoefficients< Degree >() ); + + // If we have primal dirichlet constraints, the boundary functions are necessarily zero + if( _Primal && dirichlet && !(offset%res) ) return; + + // Construct the B-Spline + for( int i=0 ; i<=Degree ; i++ ) + { + int idx = -_Off + offset + i; + if( idx>=0 && idx( _RotateLeft ( offset , res ) , false ) , _addPeriodic< false >( _RotateRight( offset , res ) , false ); + + // Recursively fold in the boundaries + if( _Primal && !(offset%res) ) return; + + // Fold in the reflected instance (which may require negation) + _addPeriodic< true >( _ReflectLeft( offset , res ) , dirichlet ) , _addPeriodic< false >( _ReflectRight( offset , res ) , dirichlet ); +} +template< int Degree > int BSplineElements< Degree >::_ReflectLeft ( int offset , int res ){ return (Degree&1) ? -offset : -1-offset; } +template< int Degree > int BSplineElements< Degree >::_ReflectRight( int offset , int res ){ return (Degree&1) ? 2*res-offset : 2*res-1-offset; } +template< int Degree > int BSplineElements< Degree >::_RotateLeft ( int offset , int res ){ return offset-2*res; } +template< int Degree > int BSplineElements< Degree >::_RotateRight ( int offset , int res ){ return offset+2*res; } + +template< int Degree > +template< bool Left > +void BSplineElements< Degree >::_addPeriodic( int offset , bool negate ) +{ + int res = int( std::vector< BSplineElementCoefficients< Degree > >::size() ); + bool set = false; + // Add in the corresponding B-spline elements (possibly negated) + for( int i=0 ; i<=Degree ; i++ ) + { + int idx = -_Off + offset + i; + if( idx>=0 && idx( Left ? _RotateLeft( offset , res ) : _RotateRight( offset , res ) , negate ); +} +template< int Degree > +void BSplineElements< Degree >::upSample( BSplineElements< Degree >& high ) const +{ + int bCoefficients[ BSplineEvaluationData< Degree >::UpSampleSize ]; + Polynomial< Degree+1 >::BinomialCoefficients( bCoefficients ); + + high.resize( std::vector< BSplineElementCoefficients< Degree > >::size()*2 ); + high.assign( high.size() , BSplineElementCoefficients< Degree >() ); + // [NOTE] We have flipped the order of the B-spline elements + for( int i=0 ; i >::size()) ; i++ ) for( int j=0 ; j<=Degree ; j++ ) + { + // At index I , B-spline element J corresponds to a B-spline centered at: + // I - SupportStart - J + int idx = i - BSplineEvaluationData< Degree >::SupportStart - j; + for( int k=BSplineEvaluationData< Degree >::UpSampleStart ; k<=BSplineEvaluationData< Degree >::UpSampleEnd ; k++ ) + { + // Index idx at the coarser resolution gets up-sampled into indices: + // 2*idx + [UpSampleStart,UpSampleEnd] + // at the finer resolution + int _idx = 2*idx + k; + // Compute the index of the B-spline element relative to 2*i and 2*i+1 + int _j1 = -_idx + 2*i - BSplineEvaluationData< Degree >::SupportStart , _j2 = -_idx + 2*i + 1 - BSplineEvaluationData< Degree >::SupportStart; + if( _j1>=0 && _j1<=Degree ) high[2*i+0][_j1] += (*this)[i][j] * bCoefficients[k-BSplineEvaluationData< Degree >::UpSampleStart]; + if( _j2>=0 && _j2<=Degree ) high[2*i+1][_j2] += (*this)[i][j] * bCoefficients[k-BSplineEvaluationData< Degree >::UpSampleStart]; + } + } + high.denominator = denominator< +void BSplineElements< Degree >::differentiate( BSplineElements< Degree-1 >& d ) const +{ + d.resize( std::vector< BSplineElementCoefficients< Degree > >::size() ); + d.assign( d.size() , BSplineElementCoefficients< Degree-1 >() ); + for( int i=0 ; i >::size()) ; i++ ) for( int j=0 ; j<=Degree ; j++ ) + { + if( j-1>=0 ) d[i][j-1] -= (*this)[i][j]; + if( j +void SetBSplineElementIntegrals( double integrals[Degree1+1][Degree2+1] ) +{ + for( int i=0 ; i<=Degree1 ; i++ ) + { + Polynomial< Degree1 > p1 = Polynomial< Degree1 >::BSplineComponent( Degree1-i ); + for( int j=0 ; j<=Degree2 ; j++ ) + { + Polynomial< Degree2 > p2 = Polynomial< Degree2 >::BSplineComponent( Degree2-j ); + integrals[i][j] = ( p1 * p2 ).integral( 0 , 1 ); + } + } +} diff --git a/colmap/include/colmap/lib/PoissonRecon/BinaryNode.h b/colmap/include/colmap/lib/PoissonRecon/BinaryNode.h new file mode 100644 index 0000000000000000000000000000000000000000..7c5a17153de3c3fd2a5aa1826f08a129e26d74ca --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/BinaryNode.h @@ -0,0 +1,70 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +#ifndef BINARY_NODE_INCLUDED +#define BINARY_NODE_INCLUDED + +class BinaryNode +{ +public: + static inline int CenterCount( int depth ) { return 1< static inline Real Width( int depth ){ return Real(1.0/(1< static inline void CenterAndWidth( int depth , int offset , Real& center , Real& width ){ width = Real (1.0/(1< static inline void CornerAndWidth( int depth , int offset , Real& corner , Real& width ){ width = Real(1.0/(1< static inline void CenterAndWidth( int idx , Real& center , Real& width ) + { + int depth , offset; + CenterDepthAndOffset( idx , depth , offset ); + CenterAndWidth( depth , offset , center , width ); + } + template< class Real > static inline void CornerAndWidth( int idx , Real& corner , Real& width ) + { + int depth , offset; + CornerDepthAndOffset( idx , depth , offset ); + CornerAndWidth( depth , offset , corner , width ); + } + static inline void CenterDepthAndOffset( int idx , int& depth , int& offset ) + { + offset = idx , depth = 0; + while( offset>=(1<=( (1< +#include + + +#ifdef _WIN32 +int strcasecmp(char* c1,char* c2); +#endif + +class cmdLineReadable{ +public: + bool set; + char* name; + cmdLineReadable(const char* name); + virtual ~cmdLineReadable(void); + virtual int read(char** argv,int argc); + virtual void writeValue(char* str); +}; + +class cmdLineInt : public cmdLineReadable { +public: + int value; + cmdLineInt(const char* name); + cmdLineInt(const char* name,const int& v); + int read(char** argv,int argc); + void writeValue(char* str); +}; +template +class cmdLineIntArray : public cmdLineReadable { +public: + int values[Dim]; + cmdLineIntArray(const char* name); + cmdLineIntArray(const char* name,const int v[Dim]); + int read(char** argv,int argc); + void writeValue(char* str); +}; + +class cmdLineFloat : public cmdLineReadable { +public: + float value; + cmdLineFloat(const char* name); + cmdLineFloat(const char* name,const float& f); + int read(char** argv,int argc); + void writeValue(char* str); +}; +template +class cmdLineFloatArray : public cmdLineReadable { +public: + float values[Dim]; + cmdLineFloatArray(const char* name); + cmdLineFloatArray(const char* name,const float f[Dim]); + int read(char** argv,int argc); + void writeValue(char* str); +}; +class cmdLineString : public cmdLineReadable { +public: + char* value; + cmdLineString(const char* name); + ~cmdLineString(); + int read(char** argv,int argc); + void writeValue(char* str); +}; +class cmdLineStrings : public cmdLineReadable { + int Dim; +public: + char** values; + cmdLineStrings(const char* name,int Dim); + ~cmdLineStrings(void); + int read(char** argv,int argc); + void writeValue(char* str); +}; +template +class cmdLineStringArray : public cmdLineReadable { +public: + char* values[Dim]; + cmdLineStringArray(const char* name); + ~cmdLineStringArray(); + int read(char** argv,int argc); + void writeValue(char* str); +}; + +// This reads the arguments in argc, matches them against "names" and sets +// the values of "r" appropriately. Parameters start with "--" +void cmdLineParse(int argc, char **argv,int num,cmdLineReadable** r,int dumpError=1); + +char* GetFileExtension(char* fileName); +char* GetLocalFileName(char* fileName); +char** ReadWords(const char* fileName,int& cnt); + +#include "CmdLineParser.inl" +#endif // CMD_LINE_PARSER_INCLUDED diff --git a/colmap/include/colmap/lib/PoissonRecon/CmdLineParser.inl b/colmap/include/colmap/lib/PoissonRecon/CmdLineParser.inl new file mode 100644 index 0000000000000000000000000000000000000000..eeded680547c1d15052d067f20db1de934b4f62e --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/CmdLineParser.inl @@ -0,0 +1,141 @@ +/* -*- C++ -*- +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +///////////////////// +// cmdLineIntArray // +///////////////////// +template +cmdLineIntArray::cmdLineIntArray(const char* name) : cmdLineReadable(name) +{ + for(int i=0;i +cmdLineIntArray::cmdLineIntArray(const char* name,const int v[Dim]) : cmdLineReadable(name) +{ + for(int i=0;i +int cmdLineIntArray::read(char** argv,int argc) +{ + if(argc>=Dim) + { + for(int i=0;i +void cmdLineIntArray::writeValue(char* str) +{ + char* temp=str; + for(int i=0;i +cmdLineFloatArray::cmdLineFloatArray(const char* name) : cmdLineReadable(name) +{ + for(int i=0;i +cmdLineFloatArray::cmdLineFloatArray(const char* name,const float f[Dim]) : cmdLineReadable(name) +{ + for(int i=0;i +int cmdLineFloatArray::read(char** argv,int argc) +{ + if(argc>=Dim) + { + for(int i=0;i +void cmdLineFloatArray::writeValue(char* str) +{ + char* temp=str; + for(int i=0;i +cmdLineStringArray::cmdLineStringArray(const char* name) : cmdLineReadable(name) +{ + for(int i=0;i +cmdLineStringArray::~cmdLineStringArray(void) +{ + for(int i=0;i +int cmdLineStringArray::read(char** argv,int argc) +{ + if(argc>=Dim) + { + for(int i=0;i +void cmdLineStringArray::writeValue(char* str) +{ + char* temp=str; + for(int i=0;i +class FunctionData{ + bool useDotRatios; + int normalize; +#if BOUNDARY_CONDITIONS + bool reflectBoundary; +#endif // BOUNDARY_CONDITIONS +public: + const static int DOT_FLAG = 1; + const static int D_DOT_FLAG = 2; + const static int D2_DOT_FLAG = 4; + const static int VALUE_FLAG = 1; + const static int D_VALUE_FLAG = 2; + + int depth , res , res2; + Real *dotTable , *dDotTable , *d2DotTable; + Real *valueTables , *dValueTables; +#if BOUNDARY_CONDITIONS + PPolynomial baseFunction , leftBaseFunction , rightBaseFunction; + PPolynomial dBaseFunction , dLeftBaseFunction , dRightBaseFunction; +#else // !BOUNDARY_CONDITIONS + PPolynomial baseFunction; + PPolynomial dBaseFunction; +#endif // BOUNDARY_CONDITIONS + PPolynomial* baseFunctions; + + FunctionData(void); + ~FunctionData(void); + + virtual void setDotTables(const int& flags); + virtual void clearDotTables(const int& flags); + + virtual void setValueTables(const int& flags,const double& smooth=0); + virtual void setValueTables(const int& flags,const double& valueSmooth,const double& normalSmooth); + virtual void clearValueTables(void); + + /******************************************************** + * Sets the translates and scales of the basis function + * up to the prescribed depth + * the maximum depth + * the basis function + * how the functions should be scaled + * 0] Value at zero equals 1 + * 1] Integral equals 1 + * 2] L2-norm equals 1 + * specifies if dot-products of derivatives + * should be pre-divided by function integrals + * spcifies if function space should be + * forced to be reflectively symmetric across the boundary + ********************************************************/ +#if BOUNDARY_CONDITIONS + void set( const int& maxDepth , const PPolynomial& F , const int& normalize , bool useDotRatios=true , bool reflectBoundary=false ); +#else // !BOUNDARY_CONDITIONS + void set(const int& maxDepth,const PPolynomial& F,const int& normalize , bool useDotRatios=true ); +#endif // BOUNDARY_CONDITIONS + +#if BOUNDARY_CONDITIONS + Real dotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const; + Real dDotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const; + Real d2DotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const; +#else // !BOUNDARY_CONDITIONS + Real dotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 ) const; + Real dDotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 ) const; + Real d2DotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 ) const; +#endif // BOUNDARY_CONDITIONS + + static inline int SymmetricIndex( const int& i1 , const int& i2 ); + static inline int SymmetricIndex( const int& i1 , const int& i2 , int& index ); +}; + + +#include "FunctionData.inl" +#endif // FUNCTION_DATA_INCLUDED \ No newline at end of file diff --git a/colmap/include/colmap/lib/PoissonRecon/FunctionData.inl b/colmap/include/colmap/lib/PoissonRecon/FunctionData.inl new file mode 100644 index 0000000000000000000000000000000000000000..4e61b961eb0200d67180c0d2c748c97cf53d5510 --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/FunctionData.inl @@ -0,0 +1,415 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +////////////////// +// FunctionData // +////////////////// +template +FunctionData::FunctionData(void) +{ + dotTable=dDotTable=d2DotTable=NULL; + valueTables=dValueTables=NULL; + res=0; +} + +template +FunctionData::~FunctionData(void) +{ + if(res) + { + if( dotTable) delete[] dotTable; + if( dDotTable) delete[] dDotTable; + if(d2DotTable) delete[] d2DotTable; + if( valueTables) delete[] valueTables; + if(dValueTables) delete[] dValueTables; + } + dotTable=dDotTable=d2DotTable=NULL; + valueTables=dValueTables=NULL; + res=0; +} + +template +#if BOUNDARY_CONDITIONS +void FunctionData::set( const int& maxDepth , const PPolynomial& F , const int& normalize , bool useDotRatios , bool reflectBoundary ) +#else // !BOUNDARY_CONDITIONS +void FunctionData::set(const int& maxDepth,const PPolynomial& F,const int& normalize , bool useDotRatios ) +#endif // BOUNDARY_CONDITIONS +{ + this->normalize = normalize; + this->useDotRatios = useDotRatios; +#if BOUNDARY_CONDITIONS + this->reflectBoundary = reflectBoundary; +#endif // BOUNDARY_CONDITIONS + + depth = maxDepth; + res = BinaryNode::CumulativeCenterCount( depth ); + res2 = (1<<(depth+1))+1; + baseFunctions = new PPolynomial[res]; + // Scale the function so that it has: + // 0] Value 1 at 0 + // 1] Integral equal to 1 + // 2] Square integral equal to 1 + switch( normalize ) + { + case 2: + baseFunction=F/sqrt((F*F).integral(F.polys[0].start,F.polys[F.polyCount-1].start)); + break; + case 1: + baseFunction=F/F.integral(F.polys[0].start,F.polys[F.polyCount-1].start); + break; + default: + baseFunction=F/F(0); + } + dBaseFunction = baseFunction.derivative(); +#if BOUNDARY_CONDITIONS + leftBaseFunction = baseFunction + baseFunction.shift( -1 ); + rightBaseFunction = baseFunction + baseFunction.shift( 1 ); + dLeftBaseFunction = leftBaseFunction.derivative(); + dRightBaseFunction = rightBaseFunction.derivative(); +#endif // BOUNDARY_CONDITIONS + double c1,w1; + for( int i=0 ; i::CenterAndWidth( i , c1 , w1 ); +#if BOUNDARY_CONDITIONS + if( reflectBoundary ) + { + int d , off; + BinaryNode< double >::DepthAndOffset( i , d , off ); + if ( off==0 ) baseFunctions[i] = leftBaseFunction.scale( w1 ).shift( c1 ); + else if( off==((1< +void FunctionData::setDotTables( const int& flags ) +{ + clearDotTables( flags ); + int size; + size = ( res*res + res )>>1; + if( flags & DOT_FLAG ) + { + dotTable = new Real[size]; + memset( dotTable , 0 , sizeof(Real)*size ); + } + if( flags & D_DOT_FLAG ) + { + dDotTable = new Real[size]; + memset( dDotTable , 0 , sizeof(Real)*size ); + } + if( flags & D2_DOT_FLAG ) + { + d2DotTable = new Real[size]; + memset( d2DotTable , 0 , sizeof(Real)*size ); + } + double t1 , t2; + t1 = baseFunction.polys[0].start; + t2 = baseFunction.polys[baseFunction.polyCount-1].start; + for( int i=0 ; i::CenterAndWidth( i , c1 , w1 ); +#if BOUNDARY_CONDITIONS + int d1 , d2 , off1 , off2; + BinaryNode< double >::DepthAndOffset( i , d1 , off1 ); + int boundary1 = 0; + if ( reflectBoundary && off1==0 ) boundary1 = -1; + else if( reflectBoundary && off1==( (1<::CenterAndWidth( j , c2 , w2 ); +#if BOUNDARY_CONDITIONS + BinaryNode< double >::DepthAndOffset( j , d2 , off2 ); + int boundary2 = 0; + if ( reflectBoundary && off2==0 ) boundary2 = -1; + else if( reflectBoundary && off2==( (1<1 ) start = 1; + if( end <0 ) end = 0; + if( end >1 ) end = 1; + } +#endif // BOUNDARY_CONDITIONS + + if( start< start1 ) start = start1; + if( end > end1 ) end = end1; + if( start>= end ) continue; + +#if BOUNDARY_CONDITIONS + Real dot = dotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ); +#else // !BOUNDARY_CONDITIONS + Real dot = dotProduct( c1 , w1 , c2 , w2 ); +#endif // BOUNDARY_CONDITIONS + if( fabs(dot)<1e-15 ) continue; + if( flags & DOT_FLAG ) dotTable[idx]=dot; + if( useDotRatios ) + { +#if BOUNDARY_CONDITIONS + if( flags & D_DOT_FLAG ) dDotTable[idx] = -dDotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ) / dot; + if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ) / dot; +#else // !BOUNDARY_CONDITIONS + if( flags & D_DOT_FLAG ) dDotTable[idx] = -dDotProduct(c1,w1,c2,w2)/dot; + if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct(c1,w1,c2,w2)/dot; +#endif // BOUNDARY_CONDITIONS + } + else + { +#if BOUNDARY_CONDITIONS + if( flags & D_DOT_FLAG ) dDotTable[idx] = dDotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ); + if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ); +#else // !BOUNDARY_CONDTIONS + if( flags & D_DOT_FLAG ) dDotTable[idx] = dDotProduct(c1,w1,c2,w2); + if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct(c1,w1,c2,w2); +#endif // BOUNDARY_CONDITIONS + } + } + } +} +template +void FunctionData::clearDotTables( const int& flags ) +{ + if((flags & DOT_FLAG) && dotTable) + { + delete[] dotTable; + dotTable=NULL; + } + if((flags & D_DOT_FLAG) && dDotTable) + { + delete[] dDotTable; + dDotTable=NULL; + } + if((flags & D2_DOT_FLAG) && d2DotTable) + { + delete[] d2DotTable; + d2DotTable=NULL; + } +} +template +void FunctionData::setValueTables( const int& flags , const double& smooth ) +{ + clearValueTables(); + if( flags & VALUE_FLAG ) valueTables = new Real[res*res2]; + if( flags & D_VALUE_FLAG ) dValueTables = new Real[res*res2]; + PPolynomial function; + PPolynomial dFunction; + for( int i=0 ; i0) + { + function=baseFunctions[i].MovingAverage(smooth); + dFunction=baseFunctions[i].derivative().MovingAverage(smooth); + } + else + { + function=baseFunctions[i]; + dFunction=baseFunctions[i].derivative(); + } + for( int j=0 ; j +void FunctionData::setValueTables(const int& flags,const double& valueSmooth,const double& normalSmooth){ + clearValueTables(); + if(flags & VALUE_FLAG){ valueTables=new Real[res*res2];} + if(flags & D_VALUE_FLAG){dValueTables=new Real[res*res2];} + PPolynomial function; + PPolynomial dFunction; + for(int i=0;i0) { function=baseFunctions[i].MovingAverage(valueSmooth);} + else { function=baseFunctions[i];} + if(normalSmooth>0) {dFunction=baseFunctions[i].derivative().MovingAverage(normalSmooth);} + else {dFunction=baseFunctions[i].derivative();} + + for(int j=0;j +void FunctionData::clearValueTables(void){ + if( valueTables){delete[] valueTables;} + if(dValueTables){delete[] dValueTables;} + valueTables=dValueTables=NULL; +} + +#if BOUNDARY_CONDITIONS +template +Real FunctionData::dotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const +{ + const PPolynomial< Degree > *b1 , *b2; + if ( boundary1==-1 ) b1 = & leftBaseFunction; + else if( boundary1== 0 ) b1 = & baseFunction; + else if( boundary1== 1 ) b1 = &rightBaseFunction; + if ( boundary2==-1 ) b2 = & leftBaseFunction; + else if( boundary2== 0 ) b2 = & baseFunction; + else if( boundary2== 1 ) b2 = &rightBaseFunction; + double r=fabs( baseFunction.polys[0].start ); + switch( normalize ) + { + case 2: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/sqrt(width1*width2)); + case 1: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/(width1*width2)); + default: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1); + } +} +template +Real FunctionData::dDotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const +{ + const PPolynomial< Degree-1 > *b1; + const PPolynomial< Degree > *b2; + if ( boundary1==-1 ) b1 = & dLeftBaseFunction; + else if( boundary1== 0 ) b1 = & dBaseFunction; + else if( boundary1== 1 ) b1 = &dRightBaseFunction; + if ( boundary2==-1 ) b2 = & leftBaseFunction; + else if( boundary2== 0 ) b2 = & baseFunction; + else if( boundary2== 1 ) b2 = & rightBaseFunction; + double r=fabs(baseFunction.polys[0].start); + switch(normalize){ + case 2: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/sqrt(width1*width2)); + case 1: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/(width1*width2)); + default: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)); + } +} +template +Real FunctionData::d2DotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const +{ + const PPolynomial< Degree-1 > *b1 , *b2; + if ( boundary1==-1 ) b1 = & dLeftBaseFunction; + else if( boundary1== 0 ) b1 = & dBaseFunction; + else if( boundary1== 1 ) b1 = &dRightBaseFunction; + if ( boundary2==-1 ) b2 = & dLeftBaseFunction; + else if( boundary2== 0 ) b2 = & dBaseFunction; + else if( boundary2== 1 ) b2 = &dRightBaseFunction; + double r=fabs(baseFunction.polys[0].start); + switch( normalize ) + { + case 2: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/sqrt(width1*width2)); + case 1: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/(width1*width2)); + default: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2); + } +} +#else // !BOUNDARY_CONDITIONS +template +Real FunctionData::dotProduct(const double& center1,const double& width1,const double& center2,const double& width2) const{ + double r=fabs(baseFunction.polys[0].start); + switch( normalize ) + { + case 2: + return Real((baseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/sqrt(width1*width2)); + case 1: + return Real((baseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/(width1*width2)); + default: + return Real((baseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1); + } +} +template +Real FunctionData::dDotProduct(const double& center1,const double& width1,const double& center2,const double& width2) const{ + double r=fabs(baseFunction.polys[0].start); + switch(normalize){ + case 2: + return Real((dBaseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/sqrt(width1*width2)); + case 1: + return Real((dBaseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/(width1*width2)); + default: + return Real((dBaseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)); + } +} +template +Real FunctionData::d2DotProduct(const double& center1,const double& width1,const double& center2,const double& width2) const{ + double r=fabs(baseFunction.polys[0].start); + switch(normalize){ + case 2: + return Real((dBaseFunction*dBaseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/sqrt(width1*width2)); + case 1: + return Real((dBaseFunction*dBaseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/(width1*width2)); + default: + return Real((dBaseFunction*dBaseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2); + } +} +#endif // BOUNDARY_CONDITIONS +template +inline int FunctionData::SymmetricIndex( const int& i1 , const int& i2 ) +{ + if( i1>i2 ) return ((i1*i1+i1)>>1)+i2; + else return ((i2*i2+i2)>>1)+i1; +} +template +inline int FunctionData::SymmetricIndex( const int& i1 , const int& i2 , int& index ) +{ + if( i1>1)+i1; + return 1; + } + else{ + index = ((i1*i1+i1)>>1)+i2; + return 0; + } +} diff --git a/colmap/include/colmap/lib/PoissonRecon/Geometry.h b/colmap/include/colmap/lib/PoissonRecon/Geometry.h new file mode 100644 index 0000000000000000000000000000000000000000..7105ec873217d4b990fc74f59f9b4c32af5bbd63 --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/Geometry.h @@ -0,0 +1,380 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +#ifndef GEOMETRY_INCLUDED +#define GEOMETRY_INCLUDED + +#include +#include +#include +#include +#include "Hash.h" + +template +Real Random(void); + +template< class Real > +struct Point3D +{ + Real coords[3]; + Point3D( void ) { coords[0] = coords[1] = coords[2] = Real(0); } + Point3D( Real v ) { coords[0] = coords[1] = coords[2] = v; } + template< class _Real > Point3D( _Real v0 , _Real v1 , _Real v2 ){ coords[0] = Real(v0) , coords[1] = Real(v1) , coords[2] = Real(v2); } + template< class _Real > Point3D( const Point3D< _Real >& p ){ coords[0] = Real( p[0] ) , coords[1] = Real( p[1] ) , coords[2] = Real( p[2] ); } + inline Real& operator[] ( int i ) { return coords[i]; } + inline const Real& operator[] ( int i ) const { return coords[i]; } + inline Point3D operator - ( void ) const { Point3D q ; q.coords[0] = -coords[0] , q.coords[1] = -coords[1] , q.coords[2] = -coords[2] ; return q; } + + template< class _Real > inline Point3D& operator += ( Point3D< _Real > p ){ coords[0] += Real(p.coords[0]) , coords[1] += Real(p.coords[1]) , coords[2] += Real(p.coords[2]) ; return *this; } + template< class _Real > inline Point3D operator + ( Point3D< _Real > p ) const { Point3D q ; q.coords[0] = coords[0] + Real(p.coords[0]) , q.coords[1] = coords[1] + Real(p.coords[1]) , q.coords[2] = coords[2] + Real(p.coords[2]) ; return q; } + template< class _Real > inline Point3D& operator *= ( _Real r ) { coords[0] *= Real(r) , coords[1] *= Real(r) , coords[2] *= Real(r) ; return *this; } + template< class _Real > inline Point3D operator * ( _Real r ) const { Point3D q ; q.coords[0] = coords[0] * Real(r) , q.coords[1] = coords[1] * Real(r) , q.coords[2] = coords[2] * Real(r) ; return q; } + + template< class _Real > inline Point3D& operator -= ( Point3D< _Real > p ){ return ( (*this)+=(-p) ); } + template< class _Real > inline Point3D operator - ( Point3D< _Real > p ) const { return (*this)+(-p); } + template< class _Real > inline Point3D& operator /= ( _Real r ){ return ( (*this)*=Real(1./r) ); } + template< class _Real > inline Point3D operator / ( _Real r ) const { return (*this) * ( Real(1.)/r ); } + + static Real Dot( const Point3D< Real >& p1 , const Point3D< Real >& p2 ){ return p1.coords[0]*p2.coords[0] + p1.coords[1]*p2.coords[1] + p1.coords[2]*p2.coords[2]; } + template< class Real1 , class Real2 > + static Real Dot( const Point3D< Real1 >& p1 , const Point3D< Real2 >& p2 ){ return Real( p1.coords[0]*p2.coords[0] + p1.coords[1]*p2.coords[1] + p1.coords[2]*p2.coords[2] ); } +}; + +template< class Real > +struct XForm3x3 +{ + Real coords[3][3]; + XForm3x3( void ) { for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) coords[i][j] = Real(0.); } + static XForm3x3 Identity( void ) + { + XForm3x3 xForm; + xForm(0,0) = xForm(1,1) = xForm(2,2) = Real(1.); + return xForm; + } + Real& operator() ( int i , int j ){ return coords[i][j]; } + const Real& operator() ( int i , int j ) const { return coords[i][j]; } + template< class _Real > Point3D< _Real > operator * ( const Point3D< _Real >& p ) const + { + Point3D< _Real > q; + for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) q[i] += _Real( coords[j][i] * p[j] ); + return q; + } + XForm3x3 operator * ( const XForm3x3& m ) const + { + XForm3x3 n; + for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ ) n.coords[i][j] += m.coords[i][k]*coords[k][j]; + return n; + } + XForm3x3 transpose( void ) const + { + XForm3x3 xForm; + for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) xForm( i , j ) = coords[j][i]; + return xForm; + } + Real subDeterminant( int i , int j ) const + { + int i1 = (i+1)%3 , i2 = (i+2)%3; + int j1 = (j+1)%3 , j2 = (j+2)%3; + return coords[i1][j1] * coords[i2][j2] - coords[i1][j2] * coords[i2][j1]; + } + Real determinant( void ) const { return coords[0][0] * subDeterminant( 0 , 0 ) + coords[1][0] * subDeterminant( 1 , 0 ) + coords[2][0] * subDeterminant( 2 , 0 ); } + XForm3x3 inverse( void ) const + { + XForm3x3 xForm; + Real d = determinant(); + for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ;j++ ) xForm.coords[j][i] = subDeterminant( i , j ) / d; + return xForm; + } +}; + +template< class Real > +struct XForm4x4 +{ + Real coords[4][4]; + XForm4x4( void ) { for( int i=0 ; i<4 ; i++ ) for( int j=0 ; j<4 ; j++ ) coords[i][j] = Real(0.); } + static XForm4x4 Identity( void ) + { + XForm4x4 xForm; + xForm(0,0) = xForm(1,1) = xForm(2,2) = xForm(3,3) = Real(1.); + return xForm; + } + Real& operator() ( int i , int j ){ return coords[i][j]; } + const Real& operator() ( int i , int j ) const { return coords[i][j]; } + template< class _Real > Point3D< _Real > operator * ( const Point3D< _Real >& p ) const + { + Point3D< _Real > q; + for( int i=0 ; i<3 ; i++ ) + { + for( int j=0 ; j<3 ; j++ ) q[i] += (_Real)( coords[j][i] * p[j] ); + q[i] += (_Real)coords[3][i]; + } + return q; + } + XForm4x4 operator * ( const XForm4x4& m ) const + { + XForm4x4 n; + for( int i=0 ; i<4 ; i++ ) for( int j=0 ; j<4 ; j++ ) for( int k=0 ; k<4 ; k++ ) n.coords[i][j] += m.coords[i][k]*coords[k][j]; + return n; + } + XForm4x4 transpose( void ) const + { + XForm4x4 xForm; + for( int i=0 ; i<4 ; i++ ) for( int j=0 ; j<4 ; j++ ) xForm( i , j ) = coords[j][i]; + return xForm; + } + Real subDeterminant( int i , int j ) const + { + XForm3x3< Real > xForm; + int ii[] = { (i+1)%4 , (i+2)%4 , (i+3)%4 } , jj[] = { (j+1)%4 , (j+2)%4 , (j+3)%4 }; + for( int _i=0 ; _i<3 ; _i++ ) for( int _j=0 ; _j<3 ; _j++ ) xForm( _i , _j ) = coords[ ii[_i] ][ jj[_j] ]; + return xForm.determinant(); + } + Real determinant( void ) const { return coords[0][0] * subDeterminant( 0 , 0 ) - coords[1][0] * subDeterminant( 1 , 0 ) + coords[2][0] * subDeterminant( 2 , 0 ) - coords[3][0] * subDeterminant( 3 , 0 ); } + XForm4x4 inverse( void ) const + { + XForm4x4 xForm; + Real d = determinant(); + for( int i=0 ; i<4 ; i++ ) for( int j=0 ; j<4 ;j++ ) + if( (i+j)%2==0 ) xForm.coords[j][i] = subDeterminant( i , j ) / d; + else xForm.coords[j][i] = -subDeterminant( i , j ) / d; + return xForm; + } +}; + + +template +Point3D RandomBallPoint(void); + +template +Point3D RandomSpherePoint(void); + +template +double Length(const Point3D& p); + +template +double SquareLength(const Point3D& p); + +template +double Distance(const Point3D& p1,const Point3D& p2); + +template +double SquareDistance(const Point3D& p1,const Point3D& p2); + +template +void CrossProduct(const Point3D& p1,const Point3D& p2,Point3D& p); + + +class Edge{ +public: + double p[2][2]; + double Length(void) const{ + double d[2]; + d[0]=p[0][0]-p[1][0]; + d[1]=p[0][1]-p[1][1]; + + return sqrt(d[0]*d[0]+d[1]*d[1]); + } +}; +class Triangle{ +public: + double p[3][3]; + double Area(void) const{ + double v1[3] , v2[3] , v[3]; + for( int d=0 ; d<3 ; d++ ) + { + v1[d] = p[1][d] - p[0][d]; + v2[d] = p[2][d] - p[0][d]; + } + v[0] = v1[1]*v2[2] - v1[2]*v2[1]; + v[1] = -v1[0]*v2[2] + v1[2]*v2[0]; + v[2] = v1[0]*v2[1] - v1[1]*v2[0]; + return sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] ) / 2; + } + double AspectRatio(void) const{ + double d=0; + int i,j; + for(i=0;i<3;i++){ + for(i=0;i<3;i++) + for(j=0;j<3;j++){d+=(p[(i+1)%3][j]-p[i][j])*(p[(i+1)%3][j]-p[i][j]);} + } + return Area()/d; + } + +}; +class CoredPointIndex +{ +public: + int index; + char inCore; + + int operator == (const CoredPointIndex& cpi) const {return (index==cpi.index) && (inCore==cpi.inCore);}; + int operator != (const CoredPointIndex& cpi) const {return (index!=cpi.index) || (inCore!=cpi.inCore);}; +}; +class EdgeIndex{ +public: + int idx[2]; +}; +class CoredEdgeIndex +{ +public: + CoredPointIndex idx[2]; +}; +class TriangleIndex{ +public: + int idx[3]; +}; + +class TriangulationEdge +{ +public: + TriangulationEdge(void); + int pIndex[2]; + int tIndex[2]; +}; + +class TriangulationTriangle +{ +public: + TriangulationTriangle(void); + int eIndex[3]; +}; + +template +class Triangulation +{ +public: + + std::vector > points; + std::vector edges; + std::vector triangles; + + int factor( int tIndex,int& p1,int& p2,int& p3); + double area(void); + double area( int tIndex ); + double area( int p1 , int p2 , int p3 ); + int flipMinimize( int eIndex); + int addTriangle( int p1 , int p2 , int p3 ); + +protected: + hash_map edgeMap; + static long long EdgeIndex( int p1 , int p2 ); + double area(const Triangle& t); +}; + + +template +void EdgeCollapse(const Real& edgeRatio,std::vector& triangles,std::vector< Point3D >& positions,std::vector >* normals); +template +void TriangleCollapse(const Real& edgeRatio,std::vector& triangles,std::vector >& positions,std::vector >* normals); + +struct CoredVertexIndex +{ + int idx; + bool inCore; +}; +template< class Vertex > +class CoredMeshData +{ +public: + std::vector< Vertex > inCorePoints; + virtual void resetIterator( void ) = 0; + + virtual int addOutOfCorePoint( const Vertex& p ) = 0; + virtual int addOutOfCorePoint_s( const Vertex& p ) = 0; + virtual int addPolygon_s( const std::vector< CoredVertexIndex >& vertices ) = 0; + virtual int addPolygon_s( const std::vector< int >& vertices ) = 0; + + virtual int nextOutOfCorePoint( Vertex& p )=0; + virtual int nextPolygon( std::vector< CoredVertexIndex >& vertices ) = 0; + + virtual int outOfCorePointCount(void)=0; + virtual int polygonCount( void ) = 0; +}; + +template< class Vertex > +class CoredVectorMeshData : public CoredMeshData< Vertex > +{ + std::vector< Vertex > oocPoints; + std::vector< std::vector< int > > polygons; + int polygonIndex; + int oocPointIndex; +public: + CoredVectorMeshData(void); + + void resetIterator(void); + + int addOutOfCorePoint( const Vertex& p ); + int addOutOfCorePoint_s( const Vertex& p ); + int addPolygon_s( const std::vector< CoredVertexIndex >& vertices ); + int addPolygon_s( const std::vector< int >& vertices ); + + int nextOutOfCorePoint( Vertex& p ); + int nextPolygon( std::vector< CoredVertexIndex >& vertices ); + + int outOfCorePointCount(void); + int polygonCount( void ); +}; +class BufferedReadWriteFile +{ + bool tempFile; + FILE* _fp; + char *_buffer , _fileName[1024]; + size_t _bufferIndex , _bufferSize; +public: + BufferedReadWriteFile( char* fileName=NULL , int bufferSize=(1<<20) ); + ~BufferedReadWriteFile( void ); + bool write( const void* data , size_t size ); + bool read ( void* data , size_t size ); + void reset( void ); +}; +template< class Vertex > +class CoredFileMeshData : public CoredMeshData< Vertex > +{ + char pointFileName[1024] , polygonFileName[1024]; + BufferedReadWriteFile *oocPointFile , *polygonFile; + int oocPoints , polygons; +public: + CoredFileMeshData( void ); + ~CoredFileMeshData( void ); + + void resetIterator( void ); + + int addOutOfCorePoint( const Vertex& p ); + int addOutOfCorePoint_s( const Vertex& p ); + int addPolygon_s( const std::vector< CoredVertexIndex >& vertices ); + int addPolygon_s( const std::vector< int >& vertices ); + + int nextOutOfCorePoint( Vertex& p ); + int nextPolygon( std::vector< CoredVertexIndex >& vertices ); + + int outOfCorePointCount( void ); + int polygonCount( void ); +}; +#include "Geometry.inl" + +#endif // GEOMETRY_INCLUDED diff --git a/colmap/include/colmap/lib/PoissonRecon/Geometry.inl b/colmap/include/colmap/lib/PoissonRecon/Geometry.inl new file mode 100644 index 0000000000000000000000000000000000000000..97be93692f5b07914b246e43c48396e7ed9c1031 --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/Geometry.inl @@ -0,0 +1,590 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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 + +template +Real Random(void){return Real(rand())/RAND_MAX;} + +template +Point3D RandomBallPoint(void){ + Point3D p; + while(1){ + p.coords[0]=Real(1.0-2.0*Random()); + p.coords[1]=Real(1.0-2.0*Random()); + p.coords[2]=Real(1.0-2.0*Random()); + double l=SquareLength(p); + if(l<=1){return p;} + } +} +template +Point3D RandomSpherePoint(void){ + Point3D p=RandomBallPoint(); + Real l=Real(Length(p)); + p.coords[0]/=l; + p.coords[1]/=l; + p.coords[2]/=l; + return p; +} + +template +double SquareLength(const Point3D& p){return p.coords[0]*p.coords[0]+p.coords[1]*p.coords[1]+p.coords[2]*p.coords[2];} + +template +double Length(const Point3D& p){return sqrt(SquareLength(p));} + +template +double SquareDistance(const Point3D& p1,const Point3D& p2){ + return (p1.coords[0]-p2.coords[0])*(p1.coords[0]-p2.coords[0])+(p1.coords[1]-p2.coords[1])*(p1.coords[1]-p2.coords[1])+(p1.coords[2]-p2.coords[2])*(p1.coords[2]-p2.coords[2]); +} + +template +double Distance(const Point3D& p1,const Point3D& p2){return sqrt(SquareDistance(p1,p2));} + +template +void CrossProduct(const Point3D& p1,const Point3D& p2,Point3D& p){ + p.coords[0]= p1.coords[1]*p2.coords[2]-p1.coords[2]*p2.coords[1]; + p.coords[1]=-p1.coords[0]*p2.coords[2]+p1.coords[2]*p2.coords[0]; + p.coords[2]= p1.coords[0]*p2.coords[1]-p1.coords[1]*p2.coords[0]; +} +template +void EdgeCollapse(const Real& edgeRatio,std::vector& triangles,std::vector< Point3D >& positions,std::vector< Point3D >* normals){ + int i,j,*remapTable,*pointCount,idx[3]; + Point3D p[3],q[2],c; + double d[3],a; + double Ratio=12.0/sqrt(3.0); // (Sum of Squares Length / Area) for and equilateral triangle + + remapTable=new int[positions.size()]; + pointCount=new int[positions.size()]; + for(i=0;i=0;i--){ + for(j=0;j<3;j++){ + idx[j]=triangles[i].idx[j]; + while(remapTable[idx[j]] a*Ratio){ + // Find the smallest edge + j=0; + if(d[1]=0;i--){ + for(j=0;j<3;j++){ + idx[j]=triangles[i].idx[j]; + while(remapTable[idx[j]] +void TriangleCollapse(const Real& edgeRatio,std::vector& triangles,std::vector< Point3D >& positions,std::vector< Point3D >* normals){ + int i,j,*remapTable,*pointCount,idx[3]; + Point3D p[3],q[2],c; + double d[3],a; + double Ratio=12.0/sqrt(3.0); // (Sum of Squares Length / Area) for and equilateral triangle + + remapTable=new int[positions.size()]; + pointCount=new int[positions.size()]; + for(i=0;i=0;i--){ + for(j=0;j<3;j++){ + idx[j]=triangles[i].idx[j]; + while(remapTable[idx[j]] a*Ratio){ + // Find the smallest edge + j=0; + if(d[1]=0;i--){ + for(j=0;j<3;j++){ + idx[j]=triangles[i].idx[j]; + while(remapTable[idx[j]] +long long Triangulation::EdgeIndex( int p1 , int p2 ) +{ + if(p1>p2) {return ((long long)(p1)<<32) | ((long long)(p2));} + else {return ((long long)(p2)<<32) | ((long long)(p1));} +} + +template +int Triangulation::factor(int tIndex,int& p1,int& p2,int & p3){ + if(triangles[tIndex].eIndex[0]<0 || triangles[tIndex].eIndex[1]<0 || triangles[tIndex].eIndex[2]<0){return 0;} + if(edges[triangles[tIndex].eIndex[0]].tIndex[0]==tIndex){p1=edges[triangles[tIndex].eIndex[0]].pIndex[0];} + else {p1=edges[triangles[tIndex].eIndex[0]].pIndex[1];} + if(edges[triangles[tIndex].eIndex[1]].tIndex[0]==tIndex){p2=edges[triangles[tIndex].eIndex[1]].pIndex[0];} + else {p2=edges[triangles[tIndex].eIndex[1]].pIndex[1];} + if(edges[triangles[tIndex].eIndex[2]].tIndex[0]==tIndex){p3=edges[triangles[tIndex].eIndex[2]].pIndex[0];} + else {p3=edges[triangles[tIndex].eIndex[2]].pIndex[1];} + return 1; +} +template +double Triangulation::area(int p1,int p2,int p3){ + Point3D q1,q2,q; + for(int i=0;i<3;i++){ + q1.coords[i]=points[p2].coords[i]-points[p1].coords[i]; + q2.coords[i]=points[p3].coords[i]-points[p1].coords[i]; + } + CrossProduct(q1,q2,q); + return Length(q); +} +template +double Triangulation::area(int tIndex){ + int p1,p2,p3; + factor(tIndex,p1,p2,p3); + return area(p1,p2,p3); +} +template +double Triangulation::area(void){ + double a=0; + for(int i=0;i +int Triangulation::addTriangle(int p1,int p2,int p3){ + hash_map::iterator iter; + int tIdx,eIdx,p[3]; + p[0]=p1; + p[1]=p2; + p[2]=p3; + triangles.push_back(TriangulationTriangle()); + tIdx=int(triangles.size())-1; + + for(int i=0;i<3;i++) + { + long long e = EdgeIndex(p[i],p[(i+1)%3]); + iter=edgeMap.find(e); + if(iter==edgeMap.end()) + { + TriangulationEdge edge; + edge.pIndex[0]=p[i]; + edge.pIndex[1]=p[(i+1)%3]; + edges.push_back(edge); + eIdx=int(edges.size())-1; + edgeMap[e]=eIdx; + edges[eIdx].tIndex[0]=tIdx; + } + else{ + eIdx=edgeMap[e]; + if(edges[eIdx].pIndex[0]==p[i]){ + if(edges[eIdx].tIndex[0]<0){edges[eIdx].tIndex[0]=tIdx;} + else{printf("Edge Triangle in use 1\n");return 0;} + } + else{ + if(edges[eIdx].tIndex[1]<0){edges[eIdx].tIndex[1]=tIdx;} + else{printf("Edge Triangle in use 2\n");return 0;} + } + + } + triangles[tIdx].eIndex[i]=eIdx; + } + return tIdx; +} +template +int Triangulation::flipMinimize(int eIndex){ + double oldArea,newArea; + int oldP[3],oldQ[3],newP[3],newQ[3]; + TriangulationEdge newEdge; + + if(edges[eIndex].tIndex[0]<0 || edges[eIndex].tIndex[1]<0){return 0;} + + if(!factor(edges[eIndex].tIndex[0],oldP[0],oldP[1],oldP[2])){return 0;} + if(!factor(edges[eIndex].tIndex[1],oldQ[0],oldQ[1],oldQ[2])){return 0;} + + oldArea=area(oldP[0],oldP[1],oldP[2])+area(oldQ[0],oldQ[1],oldQ[2]); + int idxP,idxQ; + for(idxP=0;idxP<3;idxP++){ + int i; + for(i=0;i<3;i++){if(oldP[idxP]==oldQ[i]){break;}} + if(i==3){break;} + } + for(idxQ=0;idxQ<3;idxQ++){ + int i; + for(i=0;i<3;i++){if(oldP[i]==oldQ[idxQ]){break;}} + if(i==3){break;} + } + if(idxP==3 || idxQ==3){return 0;} + newP[0]=oldP[idxP]; + newP[1]=oldP[(idxP+1)%3]; + newP[2]=oldQ[idxQ]; + newQ[0]=oldQ[idxQ]; + newQ[1]=oldP[(idxP+2)%3]; + newQ[2]=oldP[idxP]; + + newArea=area(newP[0],newP[1],newP[2])+area(newQ[0],newQ[1],newQ[2]); + if(oldArea<=newArea){return 0;} + + // Remove the entry in the hash_table for the old edge + edgeMap.erase(EdgeIndex(edges[eIndex].pIndex[0],edges[eIndex].pIndex[1])); + // Set the new edge so that the zero-side is newQ + edges[eIndex].pIndex[0]=newP[0]; + edges[eIndex].pIndex[1]=newQ[0]; + // Insert the entry into the hash_table for the new edge + edgeMap[EdgeIndex(newP[0],newQ[0])]=eIndex; + // Update the triangle information + for(int i=0;i<3;i++){ + int idx; + idx=edgeMap[EdgeIndex(newQ[i],newQ[(i+1)%3])]; + triangles[edges[eIndex].tIndex[0]].eIndex[i]=idx; + if(idx!=eIndex){ + if(edges[idx].tIndex[0]==edges[eIndex].tIndex[1]){edges[idx].tIndex[0]=edges[eIndex].tIndex[0];} + if(edges[idx].tIndex[1]==edges[eIndex].tIndex[1]){edges[idx].tIndex[1]=edges[eIndex].tIndex[0];} + } + + idx=edgeMap[EdgeIndex(newP[i],newP[(i+1)%3])]; + triangles[edges[eIndex].tIndex[1]].eIndex[i]=idx; + if(idx!=eIndex){ + if(edges[idx].tIndex[0]==edges[eIndex].tIndex[0]){edges[idx].tIndex[0]=edges[eIndex].tIndex[1];} + if(edges[idx].tIndex[1]==edges[eIndex].tIndex[0]){edges[idx].tIndex[1]=edges[eIndex].tIndex[1];} + } + } + return 1; +} +///////////////////////// +// CoredVectorMeshData // +///////////////////////// +template< class Vertex > +CoredVectorMeshData< Vertex >::CoredVectorMeshData( void ) { oocPointIndex = polygonIndex = 0; } +template< class Vertex > +void CoredVectorMeshData< Vertex >::resetIterator ( void ) { oocPointIndex = polygonIndex = 0; } +template< class Vertex > +int CoredVectorMeshData< Vertex >::addOutOfCorePoint( const Vertex& p ) +{ + oocPoints.push_back(p); + return int(oocPoints.size())-1; +} +template< class Vertex > +int CoredVectorMeshData< Vertex >::addOutOfCorePoint_s( const Vertex& p ) +{ + size_t sz; +#pragma omp critical (CoredVectorMeshData_addOutOfCorePoint_s ) + { + sz = oocPoints.size(); + oocPoints.push_back(p); + } + return (int)sz; +} +template< class Vertex > +int CoredVectorMeshData< Vertex >::addPolygon_s( const std::vector< int >& polygon ) +{ + size_t sz; +#pragma omp critical (CoredVectorMeshData_addPolygon_s) + { + sz = polygon.size(); + polygons.push_back( polygon ); + } + return (int)sz; +} +template< class Vertex > +int CoredVectorMeshData< Vertex >::addPolygon_s( const std::vector< CoredVertexIndex >& vertices ) +{ + std::vector< int > polygon( vertices.size() ); + for( int i=0 ; i<(int)vertices.size() ; i++ ) + if( vertices[i].inCore ) polygon[i] = vertices[i].idx; + else polygon[i] = -vertices[i].idx-1; + return addPolygon_s( polygon ); +} +template< class Vertex > +int CoredVectorMeshData< Vertex >::nextOutOfCorePoint( Vertex& p ) +{ + if( oocPointIndex +int CoredVectorMeshData< Vertex >::nextPolygon( std::vector< CoredVertexIndex >& vertices ) +{ + if( polygonIndex& polygon = polygons[ polygonIndex++ ]; + vertices.resize( polygon.size() ); + for( int i=0 ; i +int CoredVectorMeshData< Vertex >::outOfCorePointCount(void){return int(oocPoints.size());} +template< class Vertex > +int CoredVectorMeshData< Vertex >::polygonCount( void ) { return int( polygons.size() ); } + +/////////////////////// +// CoredFileMeshData // +/////////////////////// +template< class Vertex > +CoredFileMeshData< Vertex >::CoredFileMeshData( void ) +{ + oocPoints = polygons = 0; + + oocPointFile = new BufferedReadWriteFile(); + polygonFile = new BufferedReadWriteFile(); +} +template< class Vertex > +CoredFileMeshData< Vertex >::~CoredFileMeshData( void ) +{ + delete oocPointFile; + delete polygonFile; +} +template< class Vertex > +void CoredFileMeshData< Vertex >::resetIterator ( void ) +{ + oocPointFile->reset(); + polygonFile->reset(); +} +template< class Vertex > +int CoredFileMeshData< Vertex >::addOutOfCorePoint( const Vertex& p ) +{ + oocPointFile->write( &p , sizeof( Vertex ) ); + oocPoints++; + return oocPoints-1; +} +template< class Vertex > +int CoredFileMeshData< Vertex >::addOutOfCorePoint_s( const Vertex& p ) +{ + int sz; +#pragma omp critical (CoredFileMeshData_addOutOfCorePoint_s) + { + sz = oocPoints; + oocPointFile->write( &p , sizeof( Vertex ) ); + oocPoints++; + } + return sz; +} +template< class Vertex > +int CoredFileMeshData< Vertex >::addPolygon_s( const std::vector< int >& vertices ) +{ + int sz , vSize = (int)vertices.size(); +#pragma omp critical (CoredFileMeshData_addPolygon_s ) + { + sz = polygons; + polygonFile->write( &vSize , sizeof(int) ); + polygonFile->write( &vertices[0] , sizeof(int) * vSize ); + polygons++; + } + return sz; +} +template< class Vertex > +int CoredFileMeshData< Vertex >::addPolygon_s( const std::vector< CoredVertexIndex >& vertices ) +{ + std::vector< int > polygon( vertices.size() ); + for( int i=0 ; i<(int)vertices.size() ; i++ ) + if( vertices[i].inCore ) polygon[i] = vertices[i].idx; + else polygon[i] = -vertices[i].idx-1; + return addPolygon_s( polygon ); +} +template< class Vertex > +int CoredFileMeshData< Vertex >::nextOutOfCorePoint( Vertex& p ) +{ + if( oocPointFile->read( &p , sizeof( Vertex ) ) ) return 1; + else return 0; +} +template< class Vertex > +int CoredFileMeshData< Vertex >::nextPolygon( std::vector< CoredVertexIndex >& vertices ) +{ + int pSize; + if( polygonFile->read( &pSize , sizeof(int) ) ) + { + std::vector< int > polygon( pSize ); + if( polygonFile->read( &polygon[0] , sizeof(int)*pSize ) ) + { + vertices.resize( pSize ); + for( int i=0 ; i +int CoredFileMeshData< Vertex >::outOfCorePointCount( void ){ return oocPoints; } +template< class Vertex > +int CoredFileMeshData< Vertex >::polygonCount( void ) { return polygons; } diff --git a/colmap/include/colmap/lib/PoissonRecon/Hash.h b/colmap/include/colmap/lib/PoissonRecon/Hash.h new file mode 100644 index 0000000000000000000000000000000000000000..343d11652adf908708f2238de10044a45154bfe3 --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/Hash.h @@ -0,0 +1,5 @@ +#ifndef HASH_INCLUDED +#define HASH_INCLUDED +#include +#define hash_map std::unordered_map +#endif // HASH_INCLUDED diff --git a/colmap/include/colmap/lib/PoissonRecon/MAT.h b/colmap/include/colmap/lib/PoissonRecon/MAT.h new file mode 100644 index 0000000000000000000000000000000000000000..c090450d63bcd7ab50f15797960de564eaffab5d --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/MAT.h @@ -0,0 +1,48 @@ +/* +Copyright (c) 2007, Michael Kazhdan +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ +#ifndef MAT_INCLUDED +#define MAT_INCLUDED +#include "Geometry.h" + +template +class MinimalAreaTriangulation +{ + Real* bestTriangulation; + int* midPoint; + Real GetArea(const size_t& i,const size_t& j,const std::vector >& vertices); + void GetTriangulation(const size_t& i,const size_t& j,const std::vector >& vertices,std::vector& triangles); +public: + MinimalAreaTriangulation(void); + ~MinimalAreaTriangulation(void); + Real GetArea(const std::vector >& vertices); + void GetTriangulation(const std::vector >& vertices,std::vector& triangles); +}; + +#include "MAT.inl" + +#endif // MAT_INCLUDED diff --git a/colmap/include/colmap/lib/PoissonRecon/MAT.inl b/colmap/include/colmap/lib/PoissonRecon/MAT.inl new file mode 100644 index 0000000000000000000000000000000000000000..eaa5ad0fc9998ef7aea748af3341d3c041daa53f --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/MAT.inl @@ -0,0 +1,213 @@ +/* +Copyright (c) 2007, Michael Kazhdan +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ +////////////////////////////// +// MinimalAreaTriangulation // +////////////////////////////// +template +MinimalAreaTriangulation::MinimalAreaTriangulation(void) +{ + bestTriangulation=NULL; + midPoint=NULL; +} +template +MinimalAreaTriangulation::~MinimalAreaTriangulation(void) +{ + if(bestTriangulation) + delete[] bestTriangulation; + bestTriangulation=NULL; + if(midPoint) + delete[] midPoint; + midPoint=NULL; +} +template +void MinimalAreaTriangulation::GetTriangulation(const std::vector >& vertices,std::vector& triangles) +{ + if(vertices.size()==3) + { + triangles.resize(1); + triangles[0].idx[0]=0; + triangles[0].idx[1]=1; + triangles[0].idx[2]=2; + return; + } + else if(vertices.size()==4) + { + TriangleIndex tIndex[2][2]; + Real area[2]; + + area[0]=area[1]=0; + triangles.resize(2); + + tIndex[0][0].idx[0]=0; + tIndex[0][0].idx[1]=1; + tIndex[0][0].idx[2]=2; + tIndex[0][1].idx[0]=2; + tIndex[0][1].idx[1]=3; + tIndex[0][1].idx[2]=0; + + tIndex[1][0].idx[0]=0; + tIndex[1][0].idx[1]=1; + tIndex[1][0].idx[2]=3; + tIndex[1][1].idx[0]=3; + tIndex[1][1].idx[1]=1; + tIndex[1][1].idx[2]=2; + + Point3D n,p1,p2; + for(int i=0;i<2;i++) + for(int j=0;j<2;j++) + { + p1=vertices[tIndex[i][j].idx[1]]-vertices[tIndex[i][j].idx[0]]; + p2=vertices[tIndex[i][j].idx[2]]-vertices[tIndex[i][j].idx[0]]; + CrossProduct(p1,p2,n); + area[i] += Real( Length(n) ); + } + if(area[0]>area[1]) + { + triangles[0]=tIndex[1][0]; + triangles[1]=tIndex[1][1]; + } + else + { + triangles[0]=tIndex[0][0]; + triangles[1]=tIndex[0][1]; + } + return; + } + if(bestTriangulation) + delete[] bestTriangulation; + if(midPoint) + delete[] midPoint; + bestTriangulation=NULL; + midPoint=NULL; + size_t eCount=vertices.size(); + bestTriangulation=new Real[eCount*eCount]; + midPoint=new int[eCount*eCount]; + for(size_t i=0;i +Real MinimalAreaTriangulation::GetArea(const std::vector >& vertices) +{ + if(bestTriangulation) + delete[] bestTriangulation; + if(midPoint) + delete[] midPoint; + bestTriangulation=NULL; + midPoint=NULL; + int eCount=vertices.size(); + bestTriangulation=new double[eCount*eCount]; + midPoint=new int[eCount*eCount]; + for(int i=0;i +void MinimalAreaTriangulation::GetTriangulation(const size_t& i,const size_t& j,const std::vector >& vertices,std::vector& triangles) +{ + TriangleIndex tIndex; + size_t eCount=vertices.size(); + size_t ii=i; + if(i=ii) + return; + ii=midPoint[i*eCount+j]; + if(ii>=0) + { + tIndex.idx[0] = int( i ); + tIndex.idx[1] = int( j ); + tIndex.idx[2] = int( ii ); + triangles.push_back(tIndex); + GetTriangulation(i,ii,vertices,triangles); + GetTriangulation(ii,j,vertices,triangles); + } +} + +template +Real MinimalAreaTriangulation::GetArea(const size_t& i,const size_t& j,const std::vector >& vertices) +{ + Real a=FLT_MAX,temp; + size_t eCount=vertices.size(); + size_t idx=i*eCount+j; + size_t ii=i; + if(i=ii) + { + bestTriangulation[idx]=0; + return 0; + } + if(midPoint[idx]!=-1) + return bestTriangulation[idx]; + int mid=-1; + for(size_t r=j+1;r p,p1,p2; + p1=vertices[i]-vertices[rr]; + p2=vertices[j]-vertices[rr]; + CrossProduct(p1,p2,p); + temp = Real( Length(p) ); + if(bestTriangulation[idx1]>=0) + { + temp+=bestTriangulation[idx1]; + if(temp>a) + continue; + if(bestTriangulation[idx2]>0) + temp+=bestTriangulation[idx2]; + else + temp+=GetArea(rr,j,vertices); + } + else + { + if(bestTriangulation[idx2]>=0) + temp+=bestTriangulation[idx2]; + else + temp+=GetArea(rr,j,vertices); + if(temp>a) + continue; + temp+=GetArea(i,rr,vertices); + } + + if(temp +#include "Geometry.h" + +#define NEW_ORDERING 1 + +class Square +{ +public: + const static unsigned int CORNERS=4 , EDGES=4 , FACES=1; + static int CornerIndex (int x,int y); + static int AntipodalCornerIndex(int idx); + static void FactorCornerIndex (int idx,int& x,int& y); + static int EdgeIndex (int orientation,int i); + static void FactorEdgeIndex (int idx,int& orientation,int& i); + + static int ReflectCornerIndex (int idx,int edgeIndex); + static int ReflectEdgeIndex (int idx,int edgeIndex); + + static void EdgeCorners(int idx,int& c1,int &c2); +}; + +class Cube{ +public: + const static unsigned int CORNERS=8 , EDGES=12 , FACES=6; + + static int CornerIndex ( int x , int y , int z ); + static void FactorCornerIndex ( int idx , int& x , int& y , int& z ); + static int EdgeIndex ( int orientation , int i , int j ); + static void FactorEdgeIndex ( int idx , int& orientation , int& i , int &j); + static int FaceIndex ( int dir , int offSet ); + static int FaceIndex ( int x , int y , int z ); + static void FactorFaceIndex ( int idx , int& x , int &y , int& z ); + static void FactorFaceIndex ( int idx , int& dir , int& offSet ); + + static int AntipodalCornerIndex ( int idx ); + static int FaceReflectCornerIndex ( int idx , int faceIndex ); + static int FaceReflectEdgeIndex ( int idx , int faceIndex ); + static int FaceReflectFaceIndex ( int idx , int faceIndex ); + static int EdgeReflectCornerIndex ( int idx , int edgeIndex ); + static int EdgeReflectEdgeIndex ( int edgeIndex ); + + static int FaceAdjacentToEdges ( int eIndex1 , int eIndex2 ); + static void FacesAdjacentToEdge ( int eIndex , int& f1Index , int& f2Index ); + + static void EdgeCorners( int idx , int& c1 , int &c2 ); + static void FaceCorners( int idx , int& c1 , int &c2 , int& c3 , int& c4 ); + + static bool IsEdgeCorner( int cIndex , int e ); + static bool IsFaceCorner( int cIndex , int f ); +}; + +class MarchingSquares +{ + static double Interpolate(double v1,double v2); + static void SetVertex(int e,const double values[Square::CORNERS],double iso); +public: + const static unsigned int MAX_EDGES=2; + static const int edgeMask[1< +class MemoryInfo +{ +public: + size_t TotalPhysicalMemory; + size_t FreePhysicalMemory; + size_t TotalSwapSpace; + size_t FreeSwapSpace; + size_t TotalVirtualAddressSpace; + size_t FreeVirtualAddressSpace; + size_t PageSize; + + void set(void){ + MEMORYSTATUSEX Mem; + SYSTEM_INFO Info; + ZeroMemory( &Mem, sizeof(Mem)); + ZeroMemory( &Info, sizeof(Info)); + Mem.dwLength = sizeof(Mem); + ::GlobalMemoryStatusEx( &Mem ); + ::GetSystemInfo( &Info ); + + TotalPhysicalMemory = (size_t)Mem.ullTotalPhys; + FreePhysicalMemory = (size_t)Mem.ullAvailPhys; + TotalSwapSpace = (size_t)Mem.ullTotalPageFile; + FreeSwapSpace = (size_t)Mem.ullAvailPageFile; + TotalVirtualAddressSpace = (size_t)Mem.ullTotalVirtual; + FreeVirtualAddressSpace = (size_t)Mem.ullAvailVirtual; + PageSize = (size_t)Info.dwPageSize; + } + size_t usage(void) const {return TotalVirtualAddressSpace-FreeVirtualAddressSpace;} + + static size_t Usage(void){ + MEMORY_BASIC_INFORMATION mbi; + size_t dwMemUsed = 0; + PVOID pvAddress = 0; + + + memset(&mbi, 0, sizeof(MEMORY_BASIC_INFORMATION)); + while(VirtualQuery(pvAddress, &mbi, sizeof(MEMORY_BASIC_INFORMATION)) == sizeof(MEMORY_BASIC_INFORMATION)){ + if(mbi.State == MEM_COMMIT && mbi.Type == MEM_PRIVATE){dwMemUsed += mbi.RegionSize;} + pvAddress = ((BYTE*)mbi.BaseAddress) + mbi.RegionSize; + } + return dwMemUsed; + } +}; + +#else // !WIN32 + +#ifndef __APPLE__ // Linux variants + +#include +#include + +class MemoryInfo +{ + public: + static size_t Usage(void) + { + FILE* f = fopen("/proc/self/stat","rb"); + + int d; + long ld; + unsigned long lu; + unsigned long long llu; + char s[1024]; + char c; + + int pid; + unsigned long vm; + + int n = fscanf(f, "%d %s %c %d %d %d %d %d %lu %lu %lu %lu %lu %lu %lu %ld %ld %ld %ld %d %ld %llu %lu %ld %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %d %d %lu %lu" + ,&pid ,s ,&c ,&d ,&d ,&d ,&d ,&d ,&lu ,&lu ,&lu ,&lu ,&lu ,&lu ,&lu ,&ld ,&ld ,&ld ,&ld ,&d ,&ld ,&llu ,&vm ,&ld ,&lu ,&lu ,&lu ,&lu ,&lu ,&lu ,&lu ,&lu ,&lu ,&lu ,&lu ,&lu ,&lu ,&d ,&d ,&lu ,&lu ); + + fclose(f); +/* +pid %d +comm %s +state %c +ppid %d +pgrp %d +session %d +tty_nr %d +tpgid %d +flags %lu +minflt %lu +cminflt %lu +majflt %lu +cmajflt %lu +utime %lu +stime %lu +cutime %ld +cstime %ld +priority %ld +nice %ld +0 %ld +itrealvalue %ld +starttime %lu +vsize %lu +rss %ld +rlim %lu +startcode %lu +endcode %lu +startstack %lu +kstkesp %lu +kstkeip %lu +signal %lu +blocked %lu +sigignore %lu +sigcatch %lu +wchan %lu +nswap %lu +cnswap %lu +exit_signal %d +processor %d +rt_priority %lu (since kernel 2.5.19) +policy %lu (since kernel 2.5.19) +*/ + return vm; + } + +}; +#else // __APPLE__: has no "/proc" pseudo-file system + +// Thanks to David O'Gwynn for providing this fix. +// This comes from a post by Michael Knight: +// +// http://miknight.blogspot.com/2005/11/resident-set-size-in-mac-os-x.html + +#include +#include +#include +#include +#include +#include +#include + +void getres(task_t task, unsigned long *rss, unsigned long *vs) +{ + struct task_basic_info t_info; + mach_msg_type_number_t t_info_count = TASK_BASIC_INFO_COUNT; + + task_info(task, TASK_BASIC_INFO, (task_info_t)&t_info, &t_info_count); + *rss = t_info.resident_size; + *vs = t_info.virtual_size; +} + +class MemoryInfo +{ + public: + static size_t Usage(void) + { + unsigned long rss, vs, psize; + task_t task = MACH_PORT_NULL; + + if (task_for_pid(current_task(), getpid(), &task) != KERN_SUCCESS) + abort(); + getres(task, &rss, &vs); + return rss; + } + +}; + +#endif // !__APPLE__ + +#endif // WIN32 + +#endif // MEMORY_USAGE_INCLUDE diff --git a/colmap/include/colmap/lib/PoissonRecon/MultiGridOctreeData.Evaluation.inl b/colmap/include/colmap/lib/PoissonRecon/MultiGridOctreeData.Evaluation.inl new file mode 100644 index 0000000000000000000000000000000000000000..7f4ec81c03e654c389bf3d61ddb8352983b59c9b --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/MultiGridOctreeData.Evaluation.inl @@ -0,0 +1,806 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +template< class Real > +template< int FEMDegree > +void Octree< Real >::_Evaluator< FEMDegree >::set( int depth , bool dirichlet ) +{ + static const int LeftPointSupportRadius = BSplineEvaluationData< FEMDegree >::SupportEnd; + static const int RightPointSupportRadius = -BSplineEvaluationData< FEMDegree >::SupportStart; + + BSplineEvaluationData< FEMDegree >::SetEvaluator( evaluator , depth , dirichlet ); + if( depth>0 ) BSplineEvaluationData< FEMDegree >::SetChildEvaluator( childEvaluator , depth-1 , dirichlet ); + int center = BSplineData< FEMDegree >::Dimension( depth )>>1; + + // First set the stencils for the current depth + for( int x=-LeftPointSupportRadius ; x<=RightPointSupportRadius ; x++ ) for( int y=-LeftPointSupportRadius ; y<=RightPointSupportRadius ; y++ ) for( int z=-LeftPointSupportRadius ; z<=RightPointSupportRadius ; z++ ) + { + int fIdx[] = { center+x , center+y , center+z }; + + //// The cell stencil + { + double vv[3] , dv[3]; + for( int dd=0 ; dd( dv[0] * vv[1] * vv[2] , vv[0] * dv[1] * vv[2] , vv[0] * vv[1] * dv[2] ); + } + + //// The face stencil + for( int f=0 ; f( dv[0] * vv[1] * vv[2] , vv[0] * dv[1] * vv[2] , vv[0] * vv[1] * dv[2] ); + } + + //// The edge stencil + for( int e=0 ; e( dv[0] * vv[1] * vv[2] , vv[0] * dv[1] * vv[2] , vv[0] * vv[1] * dv[2] ); + } + + //// The corner stencil + for( int c=0 ; c( dv[0] * vv[1] * vv[2] , vv[0] * dv[1] * vv[2] , vv[0] * vv[1] * dv[2] ); + } + } + + // Now set the stencils for the parents + for( int child=0 ; child( dv[0] * vv[1] * vv[2] , vv[0] * dv[1] * vv[2] , vv[0] * vv[1] * dv[2] ); + } + + //// The face stencil + for( int f=0 ; f( dv[0] * vv[1] * vv[2] , vv[0] * dv[1] * vv[2] , vv[0] * vv[1] * dv[2] ); + } + + //// The edge stencil + for( int e=0 ; e( dv[0] * vv[1] * vv[2] , vv[0] * dv[1] * vv[2] , vv[0] * vv[1] * dv[2] ); + } + + //// The corner stencil + for( int c=0 ; c( dv[0] * vv[1] * vv[2] , vv[0] * dv[1] * vv[2] , vv[0] * vv[1] * dv[2] ); + } + } + } +} +template< class Real > +template< class V , int FEMDegree > +V Octree< Real >::_getCenterValue( const ConstPointSupportKey< FEMDegree >& neighborKey , const TreeOctNode* node , const DenseNodeData< V , FEMDegree >& solution , const DenseNodeData< V , FEMDegree >& metSolution , const _Evaluator< FEMDegree >& evaluator , bool isInterior ) const +{ + static const int SupportSize = BSplineEvaluationData< FEMDegree >::SupportSize; + static const int LeftPointSupportRadius = BSplineEvaluationData< FEMDegree >::SupportEnd; + static const int RightPointSupportRadius = - BSplineEvaluationData< FEMDegree >::SupportStart; + + if( node->children ) fprintf( stderr , "[WARNING] getCenterValue assumes leaf node\n" ); + V value(0); + int d = _Depth( node ); + + if( isInterior ) + { + const typename TreeOctNode::ConstNeighbors< SupportSize >& neighbors = _Neighbors< LeftPointSupportRadius , RightPointSupportRadius >( neighborKey , d ); + for( int i=0 ; inodeData.nodeIndex ] * Real( evaluator.cellStencil.values[i][j][k] ); + } + if( d>_minDepth-1 ) + { + int _corner = int( node - node->parent->children ); + const typename TreeOctNode::ConstNeighbors< SupportSize >& neighbors = _Neighbors< LeftPointSupportRadius , RightPointSupportRadius >( neighborKey , d-1 ); + for( int i=0 ; inodeData.nodeIndex] * Real( evaluator.cellStencils[_corner].values[i][j][k] ); + } + } + } + else + { + int cIdx[3]; + _DepthAndOffset( node , d , cIdx ); + const typename TreeOctNode::ConstNeighbors< SupportSize >& neighbors = _Neighbors< LeftPointSupportRadius , RightPointSupportRadius >( neighborKey , d ); + + for( int i=0 ; i( n ) ) + { + int _d , fIdx[3]; + _DepthAndOffset( n , _d , fIdx ); + value += + solution[ n->nodeData.nodeIndex ] * + Real( + evaluator.evaluator.centerValue( fIdx[0] , cIdx[0] , false ) * + evaluator.evaluator.centerValue( fIdx[1] , cIdx[1] , false ) * + evaluator.evaluator.centerValue( fIdx[2] , cIdx[2] , false ) + ); + } + } + if( d>_minDepth-1 ) + { + const typename TreeOctNode::ConstNeighbors< SupportSize >& neighbors = _Neighbors< LeftPointSupportRadius , RightPointSupportRadius >( neighborKey , d-1 ); + for( int i=0 ; i( n ) ) + { + int _d , fIdx[3]; + _DepthAndOffset( n , _d , fIdx ); + value += + metSolution[ n->nodeData.nodeIndex ] * + Real( + evaluator.childEvaluator.centerValue( fIdx[0] , cIdx[0] , false ) * + evaluator.childEvaluator.centerValue( fIdx[1] , cIdx[1] , false ) * + evaluator.childEvaluator.centerValue( fIdx[2] , cIdx[2] , false ) + ); + } + } + } + } + return value; +} +template< class Real > +template< class V , int FEMDegree > +V Octree< Real >::_getEdgeValue( const ConstPointSupportKey< FEMDegree >& neighborKey , const TreeOctNode* node , int edge , const DenseNodeData< V , FEMDegree >& solution , const DenseNodeData< V , FEMDegree >& metSolution , const _Evaluator< FEMDegree >& evaluator , bool isInterior ) const +{ + static const int SupportSize = BSplineEvaluationData< FEMDegree >::SupportSize; + static const int LeftPointSupportRadius = BSplineEvaluationData< FEMDegree >::SupportEnd; + static const int RightPointSupportRadius = -BSplineEvaluationData< FEMDegree >::SupportStart; + V value(0); + int d , cIdx[3]; + _DepthAndOffset( node , d , cIdx ); + int startX = 0 , endX = SupportSize , startY = 0 , endY = SupportSize , startZ = 0 , endZ = SupportSize; + int orientation , i1 , i2; + Cube::FactorEdgeIndex( edge , orientation , i1 , i2 ); + switch( orientation ) + { + case 0: + cIdx[1] += i1 , cIdx[2] += i2; + if( i1 ) startY++ ; else endY--; + if( i2 ) startZ++ ; else endZ--; + break; + case 1: + cIdx[0] += i1 , cIdx[2] += i2; + if( i1 ) startX++ ; else endX--; + if( i2 ) startZ++ ; else endZ--; + break; + case 2: + cIdx[0] += i1 , cIdx[1] += i2; + if( i1 ) startX++ ; else endX--; + if( i2 ) startY++ ; else endY--; + break; + } + + { + const typename TreeOctNode::ConstNeighbors< SupportSize >& neighbors = _Neighbors< LeftPointSupportRadius , RightPointSupportRadius >( neighborKey , d ); + for( int x=startX ; x( _node ) ) + { + if( isInterior ) value += solution[ _node->nodeData.nodeIndex ] * evaluator.edgeStencil[edge].values[x][y][z]; + else + { + int _d , fIdx[3]; + _DepthAndOffset( _node , _d , fIdx ); + switch( orientation ) + { + case 0: + value += + solution[ _node->nodeData.nodeIndex ] * + Real( + evaluator.evaluator.centerValue( fIdx[0] , cIdx[0] , false ) * + evaluator.evaluator.cornerValue( fIdx[1] , cIdx[1] , false ) * + evaluator.evaluator.cornerValue( fIdx[2] , cIdx[2] , false ) + ); + break; + case 1: + value += + solution[ _node->nodeData.nodeIndex ] * + Real( + evaluator.evaluator.cornerValue( fIdx[0] , cIdx[0] , false ) * + evaluator.evaluator.centerValue( fIdx[1] , cIdx[1] , false ) * + evaluator.evaluator.cornerValue( fIdx[2] , cIdx[2] , false ) + ); + break; + case 2: + value += + solution[ _node->nodeData.nodeIndex ] * + Real( + evaluator.evaluator.cornerValue( fIdx[0] , cIdx[0] , false ) * + evaluator.evaluator.cornerValue( fIdx[1] , cIdx[1] , false ) * + evaluator.evaluator.centerValue( fIdx[2] , cIdx[2] , false ) + ); + break; + } + } + } + } + } + if( d>_minDepth-1 ) + { + int _corner = int( node - node->parent->children ); + int _cx , _cy , _cz; + Cube::FactorCornerIndex( _corner , _cx , _cy , _cz ); + // If the corner/child indices don't match, then the sample position is in the interior of the + // coarser cell and so the full support resolution should be used. + switch( orientation ) + { + case 0: + if( _cy!=i1 ) startY = 0 , endY = SupportSize; + if( _cz!=i2 ) startZ = 0 , endZ = SupportSize; + break; + case 1: + if( _cx!=i1 ) startX = 0 , endX = SupportSize; + if( _cz!=i2 ) startZ = 0 , endZ = SupportSize; + break; + case 2: + if( _cx!=i1 ) startX = 0 , endX = SupportSize; + if( _cy!=i2 ) startY = 0 , endY = SupportSize; + break; + } + const typename TreeOctNode::ConstNeighbors< SupportSize >& neighbors = _Neighbors< LeftPointSupportRadius , RightPointSupportRadius >( neighborKey , d-1 ); + for( int x=startX ; x( _node ) ) + { + if( isInterior ) value += metSolution[ _node->nodeData.nodeIndex ] * evaluator.edgeStencils[_corner][edge].values[x][y][z]; + else + { + int _d , fIdx[3]; + _DepthAndOffset( _node , _d , fIdx ); + switch( orientation ) + { + case 0: + value += + metSolution[ _node->nodeData.nodeIndex ] * + Real( + evaluator.childEvaluator.centerValue( fIdx[0] , cIdx[0] , false ) * + evaluator.childEvaluator.cornerValue( fIdx[1] , cIdx[1] , false ) * + evaluator.childEvaluator.cornerValue( fIdx[2] , cIdx[2] , false ) + ); + break; + case 1: + value += + metSolution[ _node->nodeData.nodeIndex ] * + Real( + evaluator.childEvaluator.cornerValue( fIdx[0] , cIdx[0] , false ) * + evaluator.childEvaluator.centerValue( fIdx[1] , cIdx[1] , false ) * + evaluator.childEvaluator.cornerValue( fIdx[2] , cIdx[2] , false ) + ); + break; + case 2: + value += + metSolution[ _node->nodeData.nodeIndex ] * + Real( + evaluator.childEvaluator.cornerValue( fIdx[0] , cIdx[0] , false ) * + evaluator.childEvaluator.cornerValue( fIdx[1] , cIdx[1] , false ) * + evaluator.childEvaluator.centerValue( fIdx[2] , cIdx[2] , false ) + ); + break; + } + } + } + } + } + return Real( value ); +} +template< class Real > +template< int FEMDegree > +std::pair< Real , Point3D< Real > > Octree< Real >::_getEdgeValueAndGradient( const ConstPointSupportKey< FEMDegree >& neighborKey , const TreeOctNode* node , int edge , const DenseNodeData< Real , FEMDegree >& solution , const DenseNodeData< Real , FEMDegree >& metSolution , const _Evaluator< FEMDegree >& evaluator , bool isInterior ) const +{ + static const int SupportSize = BSplineEvaluationData< FEMDegree >::SupportSize; + static const int LeftPointSupportRadius = BSplineEvaluationData< FEMDegree >::SupportEnd; + static const int RightPointSupportRadius = -BSplineEvaluationData< FEMDegree >::SupportStart; + double value = 0; + Point3D< double > gradient; + int d , cIdx[3]; + _DepthAndOffset( node , d , cIdx ); + + int startX = 0 , endX = SupportSize , startY = 0 , endY = SupportSize , startZ = 0 , endZ = SupportSize; + int orientation , i1 , i2; + Cube::FactorEdgeIndex( edge , orientation , i1 , i2 ); + switch( orientation ) + { + case 0: + cIdx[1] += i1 , cIdx[2] += i2; + if( i1 ) startY++ ; else endY--; + if( i2 ) startZ++ ; else endZ--; + break; + case 1: + cIdx[0] += i1 , cIdx[2] += i2; + if( i1 ) startX++ ; else endX--; + if( i2 ) startZ++ ; else endZ--; + break; + case 2: + cIdx[0] += i1 , cIdx[1] += i2; + if( i1 ) startX++ ; else endX--; + if( i2 ) startY++ ; else endY--; + break; + } + { + const typename TreeOctNode::ConstNeighbors< SupportSize >& neighbors = _Neighbors< LeftPointSupportRadius , RightPointSupportRadius >( neighborKey , d ); + for( int x=startX ; x( _node ) ) + { + if( isInterior ) + { + value += evaluator. edgeStencil[edge].values[x][y][z] * solution[ _node->nodeData.nodeIndex ]; + gradient += evaluator.dEdgeStencil[edge].values[x][y][z] * solution[ _node->nodeData.nodeIndex ]; + } + else + { + int _d , fIdx[3]; + _DepthAndOffset( _node , _d , fIdx ); + + double vv[3] , dv[3]; + switch( orientation ) + { + case 0: + vv[0] = evaluator.evaluator.centerValue( fIdx[0] , cIdx[0] , false ); + vv[1] = evaluator.evaluator.cornerValue( fIdx[1] , cIdx[1] , false ); + vv[2] = evaluator.evaluator.cornerValue( fIdx[2] , cIdx[2] , false ); + dv[0] = evaluator.evaluator.centerValue( fIdx[0] , cIdx[0] , true ); + dv[1] = evaluator.evaluator.cornerValue( fIdx[1] , cIdx[1] , true ); + dv[2] = evaluator.evaluator.cornerValue( fIdx[2] , cIdx[2] , true ); + break; + case 1: + vv[0] = evaluator.evaluator.cornerValue( fIdx[0] , cIdx[0] , false ); + vv[1] = evaluator.evaluator.centerValue( fIdx[1] , cIdx[1] , false ); + vv[2] = evaluator.evaluator.cornerValue( fIdx[2] , cIdx[2] , false ); + dv[0] = evaluator.evaluator.cornerValue( fIdx[0] , cIdx[0] , true ); + dv[1] = evaluator.evaluator.centerValue( fIdx[1] , cIdx[1] , true ); + dv[2] = evaluator.evaluator.cornerValue( fIdx[2] , cIdx[2] , true ); + break; + case 2: + vv[0] = evaluator.evaluator.cornerValue( fIdx[0] , cIdx[0] , false ); + vv[1] = evaluator.evaluator.cornerValue( fIdx[1] , cIdx[1] , false ); + vv[2] = evaluator.evaluator.centerValue( fIdx[2] , cIdx[2] , false ); + dv[0] = evaluator.evaluator.cornerValue( fIdx[0] , cIdx[0] , true ); + dv[1] = evaluator.evaluator.cornerValue( fIdx[1] , cIdx[1] , true ); + dv[2] = evaluator.evaluator.centerValue( fIdx[2] , cIdx[2] , true ); + break; + } + value += solution[ _node->nodeData.nodeIndex ] * vv[0] * vv[1] * vv[2]; + gradient += Point3D< double >( dv[0]*vv[1]*vv[2] , vv[0]*dv[1]*vv[2] , vv[0]*vv[1]*dv[2] ) * solution[ _node->nodeData.nodeIndex ]; + } + } + } + } + if( d>_minDepth-1 ) + { + int _corner = int( node - node->parent->children ); + int _cx , _cy , _cz; + Cube::FactorCornerIndex( _corner , _cx , _cy , _cz ); + // If the corner/child indices don't match, then the sample position is in the interior of the + // coarser cell and so the full support resolution should be used. + switch( orientation ) + { + case 0: + if( _cy!=i1 ) startY = 0 , endY = SupportSize; + if( _cz!=i2 ) startZ = 0 , endZ = SupportSize; + break; + case 1: + if( _cx!=i1 ) startX = 0 , endX = SupportSize; + if( _cz!=i2 ) startZ = 0 , endZ = SupportSize; + break; + case 2: + if( _cx!=i1 ) startX = 0 , endX = SupportSize; + if( _cy!=i2 ) startY = 0 , endY = SupportSize; + break; + } + const typename TreeOctNode::ConstNeighbors< SupportSize >& neighbors = _Neighbors< LeftPointSupportRadius , RightPointSupportRadius >( neighborKey , d-1 ); + for( int x=startX ; x( _node ) ) + { + if( isInterior ) + { + value += evaluator. edgeStencils[_corner][edge].values[x][y][z] * metSolution[ _node->nodeData.nodeIndex ]; + gradient += evaluator.dEdgeStencils[_corner][edge].values[x][y][z] * metSolution[ _node->nodeData.nodeIndex ]; + } + else + { + int _d , fIdx[3]; + _DepthAndOffset( _node , _d , fIdx ); + double vv[3] , dv[3]; + switch( orientation ) + { + case 0: + vv[0] = evaluator.childEvaluator.centerValue( fIdx[0] , cIdx[0] , false ); + vv[1] = evaluator.childEvaluator.cornerValue( fIdx[1] , cIdx[1] , false ); + vv[2] = evaluator.childEvaluator.cornerValue( fIdx[2] , cIdx[2] , false ); + dv[0] = evaluator.childEvaluator.centerValue( fIdx[0] , cIdx[0] , true ); + dv[1] = evaluator.childEvaluator.cornerValue( fIdx[1] , cIdx[1] , true ); + dv[2] = evaluator.childEvaluator.cornerValue( fIdx[2] , cIdx[2] , true ); + break; + case 1: + vv[0] = evaluator.childEvaluator.cornerValue( fIdx[0] , cIdx[0] , false ); + vv[1] = evaluator.childEvaluator.centerValue( fIdx[1] , cIdx[1] , false ); + vv[2] = evaluator.childEvaluator.cornerValue( fIdx[2] , cIdx[2] , false ); + dv[0] = evaluator.childEvaluator.cornerValue( fIdx[0] , cIdx[0] , true ); + dv[1] = evaluator.childEvaluator.centerValue( fIdx[1] , cIdx[1] , true ); + dv[2] = evaluator.childEvaluator.cornerValue( fIdx[2] , cIdx[2] , true ); + break; + case 2: + vv[0] = evaluator.childEvaluator.cornerValue( fIdx[0] , cIdx[0] , false ); + vv[1] = evaluator.childEvaluator.cornerValue( fIdx[1] , cIdx[1] , false ); + vv[2] = evaluator.childEvaluator.centerValue( fIdx[2] , cIdx[2] , false ); + dv[0] = evaluator.childEvaluator.cornerValue( fIdx[0] , cIdx[0] , true ); + dv[1] = evaluator.childEvaluator.cornerValue( fIdx[1] , cIdx[1] , true ); + dv[2] = evaluator.childEvaluator.centerValue( fIdx[2] , cIdx[2] , true ); + break; + } + value += metSolution[ _node->nodeData.nodeIndex ] * vv[0] * vv[1] * vv[2]; + gradient += Point3D< double >( dv[0]*vv[1]*vv[2] , vv[0]*dv[1]*vv[2] , vv[0]*vv[1]*dv[2] ) * metSolution[ _node->nodeData.nodeIndex ]; + } + } + } + } + return std::pair< Real , Point3D< Real > >( Real( value ) , Point3D< Real >( gradient ) ); +} + +template< class Real > +template< class V , int FEMDegree > +V Octree< Real >::_getCornerValue( const ConstPointSupportKey< FEMDegree >& neighborKey , const TreeOctNode* node , int corner , const DenseNodeData< V , FEMDegree >& solution , const DenseNodeData< V , FEMDegree >& metSolution , const _Evaluator< FEMDegree >& evaluator , bool isInterior ) const +{ + static const int SupportSize = BSplineEvaluationData< FEMDegree >::SupportSize; + static const int LeftPointSupportRadius = BSplineEvaluationData< FEMDegree >::SupportEnd; + static const int RightPointSupportRadius = - BSplineEvaluationData< FEMDegree >::SupportStart; + + V value(0); + int d , cIdx[3]; + _DepthAndOffset( node , d , cIdx ); + + int cx , cy , cz; + int startX = 0 , endX = SupportSize , startY = 0 , endY = SupportSize , startZ = 0 , endZ = SupportSize; + Cube::FactorCornerIndex( corner , cx , cy , cz ); + cIdx[0] += cx , cIdx[1] += cy , cIdx[2] += cz; + { + const typename TreeOctNode::ConstNeighbors< SupportSize >& neighbors = _Neighbors< LeftPointSupportRadius , RightPointSupportRadius >( neighborKey , d ); + if( cx==0 ) endX--; + else startX++; + if( cy==0 ) endY--; + else startY++; + if( cz==0 ) endZ--; + else startZ++; + if( isInterior ) + for( int x=startX ; xnodeData.nodeIndex ] * Real( evaluator.cornerStencil[corner].values[x][y][z] ); + } + else + for( int x=startX ; x( _node ) ) + { + int _d , fIdx[3]; + _DepthAndOffset( _node , _d , fIdx ); + value += + solution[ _node->nodeData.nodeIndex ] * + Real( + evaluator.evaluator.cornerValue( fIdx[0] , cIdx[0] , false ) * + evaluator.evaluator.cornerValue( fIdx[1] , cIdx[1] , false ) * + evaluator.evaluator.cornerValue( fIdx[2] , cIdx[2] , false ) + ); + } + } + } + if( d>_minDepth-1 ) + { + int _corner = int( node - node->parent->children ); + int _cx , _cy , _cz; + Cube::FactorCornerIndex( _corner , _cx , _cy , _cz ); + // If the corner/child indices don't match, then the sample position is in the interior of the + // coarser cell and so the full support resolution should be used. + if( cx!=_cx ) startX = 0 , endX = SupportSize; + if( cy!=_cy ) startY = 0 , endY = SupportSize; + if( cz!=_cz ) startZ = 0 , endZ = SupportSize; + const typename TreeOctNode::ConstNeighbors< SupportSize >& neighbors = _Neighbors< LeftPointSupportRadius , RightPointSupportRadius >( neighborKey , d-1 ); + if( isInterior ) + for( int x=startX ; xnodeData.nodeIndex ] * Real( evaluator.cornerStencils[_corner][corner].values[x][y][z] ); + } + else + for( int x=startX ; x( _node ) ) + { + int _d , fIdx[3]; + _DepthAndOffset( _node , _d , fIdx ); + value += + metSolution[ _node->nodeData.nodeIndex ] * + Real( + evaluator.childEvaluator.cornerValue( fIdx[0] , cIdx[0] , false ) * + evaluator.childEvaluator.cornerValue( fIdx[1] , cIdx[1] , false ) * + evaluator.childEvaluator.cornerValue( fIdx[2] , cIdx[2] , false ) + ); + } + } + } + return Real( value ); +} +template< class Real > +template< int FEMDegree > +std::pair< Real , Point3D< Real > > Octree< Real >::_getCornerValueAndGradient( const ConstPointSupportKey< FEMDegree >& neighborKey , const TreeOctNode* node , int corner , const DenseNodeData< Real , FEMDegree >& solution , const DenseNodeData< Real , FEMDegree >& metSolution , const _Evaluator< FEMDegree >& evaluator , bool isInterior ) const +{ + static const int SupportSize = BSplineEvaluationData< FEMDegree >::SupportSize; + static const int LeftPointSupportRadius = BSplineEvaluationData< FEMDegree >::SupportEnd; + static const int RightPointSupportRadius = - BSplineEvaluationData< FEMDegree >::SupportStart; + + double value = 0; + Point3D< double > gradient; + int d , cIdx[3]; + _DepthAndOffset( node , d , cIdx ); + + int cx , cy , cz; + int startX = 0 , endX = SupportSize , startY = 0 , endY = SupportSize , startZ = 0 , endZ = SupportSize; + Cube::FactorCornerIndex( corner , cx , cy , cz ); + cIdx[0] += cx , cIdx[1] += cy , cIdx[2] += cz; + { + if( cx==0 ) endX--; + else startX++; + if( cy==0 ) endY--; + else startY++; + if( cz==0 ) endZ--; + else startZ++; + const typename TreeOctNode::ConstNeighbors< SupportSize >& neighbors = _Neighbors< LeftPointSupportRadius , RightPointSupportRadius >( neighborKey , d ); + if( isInterior ) + for( int x=startX ; xnodeData.nodeIndex ] * evaluator.cornerStencil[corner].values[x][y][z] , gradient += evaluator.dCornerStencil[corner].values[x][y][z] * solution[ _node->nodeData.nodeIndex ]; + } + else + for( int x=startX ; x( _node ) ) + { + int _d , fIdx[3]; + _DepthAndOffset( _node , _d , fIdx ); + double v [] = { evaluator.evaluator.cornerValue( fIdx[0] , cIdx[0] , false ) , evaluator.evaluator.cornerValue( fIdx[1] , cIdx[1] , false ) , evaluator.evaluator.cornerValue( fIdx[2] , cIdx[2] , false ) }; + double dv[] = { evaluator.evaluator.cornerValue( fIdx[0] , cIdx[0] , true ) , evaluator.evaluator.cornerValue( fIdx[1] , cIdx[1] , true ) , evaluator.evaluator.cornerValue( fIdx[2] , cIdx[2] , true ) }; + value += solution[ _node->nodeData.nodeIndex ] * v[0] * v[1] * v[2]; + gradient += Point3D< double >( dv[0]*v[1]*v[2] , v[0]*dv[1]*v[2] , v[0]*v[1]*dv[2] ) * solution[ _node->nodeData.nodeIndex ]; + } + } + } + if( d>_minDepth-1 ) + { + int _corner = int( node - node->parent->children ); + int _cx , _cy , _cz; + Cube::FactorCornerIndex( _corner , _cx , _cy , _cz ); + if( cx!=_cx ) startX = 0 , endX = SupportSize; + if( cy!=_cy ) startY = 0 , endY = SupportSize; + if( cz!=_cz ) startZ = 0 , endZ = SupportSize; + const typename TreeOctNode::ConstNeighbors< SupportSize >& neighbors = _Neighbors< LeftPointSupportRadius , RightPointSupportRadius >( neighborKey , d-1 ); + if( isInterior ) + for( int x=startX ; xnodeData.nodeIndex ] * evaluator.cornerStencils[_corner][corner].values[x][y][z] , gradient += evaluator.dCornerStencils[_corner][corner].values[x][y][z] * metSolution[ _node->nodeData.nodeIndex ]; + } + else + for( int x=startX ; x( _node ) ) + { + int _d , fIdx[3]; + _DepthAndOffset( _node , _d , fIdx ); + double v [] = { evaluator.childEvaluator.cornerValue( fIdx[0] , cIdx[0] , false ) , evaluator.childEvaluator.cornerValue( fIdx[1] , cIdx[1] , false ) , evaluator.childEvaluator.cornerValue( fIdx[2] , cIdx[2] , false ) }; + double dv[] = { evaluator.childEvaluator.cornerValue( fIdx[0] , cIdx[0] , true ) , evaluator.childEvaluator.cornerValue( fIdx[1] , cIdx[1] , true ) , evaluator.childEvaluator.cornerValue( fIdx[2] , cIdx[2] , true ) }; + value += metSolution[ _node->nodeData.nodeIndex ] * v[0] * v[1] * v[2]; + gradient += Point3D< double >( dv[0]*v[1]*v[2] , v[0]*dv[1]*v[2] , v[0]*v[1]*dv[2] ) * metSolution[ _node->nodeData.nodeIndex ]; + } + } + } + return std::pair< Real , Point3D< Real > >( Real( value ) , Point3D< Real >( gradient ) ); +} diff --git a/colmap/include/colmap/lib/PoissonRecon/MultiGridOctreeData.IsoSurface.inl b/colmap/include/colmap/lib/PoissonRecon/MultiGridOctreeData.IsoSurface.inl new file mode 100644 index 0000000000000000000000000000000000000000..53e6df21811ac961643247b5f06fa5af52f52833 --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/MultiGridOctreeData.IsoSurface.inl @@ -0,0 +1,1161 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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 "Octree.h" +#include "MyTime.h" +#include "MemoryUsage.h" +#include "MAT.h" + +template< class Real > +template< class Vertex > +Octree< Real >::SliceValues< Vertex >::SliceValues( void ) +{ + _oldCCount = _oldECount = _oldFCount = _oldNCount = 0; + cornerValues = NullPointer( Real ) ; cornerGradients = NullPointer( Point3D< Real > ) ; cornerSet = NullPointer( char ); + edgeKeys = NullPointer( long long ) ; edgeSet = NullPointer( char ); + faceEdges = NullPointer( FaceEdges ) ; faceSet = NullPointer( char ); + mcIndices = NullPointer( char ); +} +template< class Real > +template< class Vertex > +Octree< Real >::SliceValues< Vertex >::~SliceValues( void ) +{ + _oldCCount = _oldECount = _oldFCount = _oldNCount = 0; + FreePointer( cornerValues ) ; FreePointer( cornerGradients ) ; FreePointer( cornerSet ); + FreePointer( edgeKeys ) ; FreePointer( edgeSet ); + FreePointer( faceEdges ) ; FreePointer( faceSet ); + FreePointer( mcIndices ); +} +template< class Real > +template< class Vertex > +void Octree< Real >::SliceValues< Vertex >::reset( bool nonLinearFit ) +{ + faceEdgeMap.clear() , edgeVertexMap.clear() , vertexPairMap.clear(); + + if( _oldNCount0 ) mcIndices = AllocPointer< char >( _oldNCount ); + } + if( _oldCCount0 ) + { + cornerValues = AllocPointer< Real >( _oldCCount ); + if( nonLinearFit ) cornerGradients = AllocPointer< Point3D< Real > >( _oldCCount ); + cornerSet = AllocPointer< char >( _oldCCount ); + } + } + if( _oldECount( _oldECount ); + edgeSet = AllocPointer< char >( _oldECount ); + } + if( _oldFCount( _oldFCount ); + faceSet = AllocPointer< char >( _oldFCount ); + } + + if( sliceData.cCount>0 ) memset( cornerSet , 0 , sizeof( char ) * sliceData.cCount ); + if( sliceData.eCount>0 ) memset( edgeSet , 0 , sizeof( char ) * sliceData.eCount ); + if( sliceData.fCount>0 ) memset( faceSet , 0 , sizeof( char ) * sliceData.fCount ); +} +template< class Real > +template< class Vertex > +Octree< Real >::XSliceValues< Vertex >::XSliceValues( void ) +{ + _oldECount = _oldFCount = 0; + edgeKeys = NullPointer( long long ) ; edgeSet = NullPointer( char ); + faceEdges = NullPointer( FaceEdges ) ; faceSet = NullPointer( char ); +} +template< class Real > +template< class Vertex > +Octree< Real >::XSliceValues< Vertex >::~XSliceValues( void ) +{ + _oldECount = _oldFCount = 0; + FreePointer( edgeKeys ) ; FreePointer( edgeSet ); + FreePointer( faceEdges ) ; FreePointer( faceSet ); +} +template< class Real > +template< class Vertex > +void Octree< Real >::XSliceValues< Vertex >::reset( void ) +{ + faceEdgeMap.clear() , edgeVertexMap.clear() , vertexPairMap.clear(); + + if( _oldECount( _oldECount ); + edgeSet = AllocPointer< char >( _oldECount ); + } + if( _oldFCount( _oldFCount ); + faceSet = AllocPointer< char >( _oldFCount ); + } + if( xSliceData.eCount>0 ) memset( edgeSet , 0 , sizeof( char ) * xSliceData.eCount ); + if( xSliceData.fCount>0 ) memset( faceSet , 0 , sizeof( char ) * xSliceData.fCount ); +} + +template< class Real > +template< int FEMDegree , int WeightDegree , int ColorDegree , class Vertex > +void Octree< Real >::GetMCIsoSurface( const SparseNodeData< Real , WeightDegree >* densityWeights , const SparseNodeData< ProjectiveData< Point3D< Real > > , ColorDegree >* colorData , const DenseNodeData< Real , FEMDegree >& solution , Real isoValue , CoredMeshData< Vertex >& mesh , bool nonLinearFit , bool addBarycenter , bool polygonMesh ) +{ + int maxDepth = _tree.maxDepth(); + if( FEMDegree==1 && nonLinearFit ) fprintf( stderr , "[WARNING] First order B-Splines do not support non-linear interpolation\n" ) , nonLinearFit = false; + + BSplineData< ColorDegree >* colorBSData = NULL; + if( colorData ) + { + colorBSData = new BSplineData< ColorDegree >(); + colorBSData->set( maxDepth , _dirichlet ); + } + + DenseNodeData< Real , FEMDegree > coarseSolution( _sNodes.end( maxDepth-1 ) ); + memset( coarseSolution.data , 0 , sizeof(Real)*_sNodes.end( maxDepth-1 ) ); +#pragma omp parallel for num_threads( threads ) + for( int i=_sNodes.begin(_minDepth) ; i<_sNodes.end(maxDepth-1) ; i++ ) coarseSolution[i] = solution[i]; + for( int d=_minDepth+1 ; d > evaluators( maxDepth+1 ); + for( int d=_minDepth ; d<=maxDepth ; d++ ) evaluators[d].set( d-1 , _dirichlet ); + int vertexOffset = 0; + std::vector< SlabValues< Vertex > > slabValues( maxDepth+1 ); + + // Initialize the back slice + for( int d=maxDepth ; d>=_minDepth ; d-- ) + { + _sNodes.setSliceTableData ( slabValues[d]. sliceValues(0). sliceData , d , 0 , threads ); + _sNodes.setSliceTableData ( slabValues[d]. sliceValues(1). sliceData , d , 1 , threads ); + _sNodes.setXSliceTableData( slabValues[d].xSliceValues(0).xSliceData , d , 0 , threads ); + slabValues[d].sliceValues (0).reset( nonLinearFit ); + slabValues[d].sliceValues (1).reset( nonLinearFit ); + slabValues[d].xSliceValues(0).reset( ); + } + for( int d=maxDepth ; d>=_minDepth ; d-- ) + { + // Copy edges from finer + if( d( colorBSData , densityWeights , colorData , isoValue , d , 0 , vertexOffset , mesh , slabValues , threads ); + SetSliceIsoEdges( d , 0 , slabValues , threads ); + } + // Iterate over the slices at the finest level + for( int slice=0 ; slice<( 1<<(maxDepth-1) ) ; slice++ ) + { + // Process at all depths that that contain this slice + for( int d=maxDepth , o=slice+1 ; d>=_minDepth ; d-- , o>>=1 ) + { + // Copy edges from finer (required to ensure we correctly track edge cancellations) + if( d( colorBSData , densityWeights , colorData , isoValue , d , o , vertexOffset , mesh , slabValues , threads ); + SetSliceIsoEdges( d , o , slabValues , threads ); + + // Set the cross-slice edges + SetXSliceIsoVertices< WeightDegree , ColorDegree >( colorBSData , densityWeights , colorData , isoValue , d , o-1 , vertexOffset , mesh , slabValues , threads ); + SetXSliceIsoEdges( d , o-1 , slabValues , threads ); + + // Add the triangles + SetIsoSurface( d , o-1 , slabValues[d].sliceValues(o-1) , slabValues[d].sliceValues(o) , slabValues[d].xSliceValues(o-1) , mesh , polygonMesh , addBarycenter , vertexOffset , threads ); + + if( o&1 ) break; + } + for( int d=maxDepth , o=slice+1 ; d>=_minDepth ; d-- , o>>=1 ) + { + // Initialize for the next pass + if( o<(1< +template< int FEMDegree , int NormalDegree > +Real Octree< Real >::GetIsoValue( const DenseNodeData< Real , FEMDegree >& solution , const SparseNodeData< Real , NormalDegree >& nodeWeights ) +{ + Real isoValue=0 , weightSum=0; + int maxDepth = _tree.maxDepth(); + + Pointer( Real ) nodeValues = AllocPointer< Real >( _sNodes.end(maxDepth) ); + memset( nodeValues , 0 , sizeof(Real) * _sNodes.end(maxDepth) ); + DenseNodeData< Real , FEMDegree > metSolution( _sNodes.end( maxDepth-1 ) ); + memset( metSolution.data , 0 , sizeof(Real)*_sNodes.end( maxDepth-1 ) ); +#pragma omp parallel for num_threads( threads ) + for( int i=_sNodes.begin(_minDepth) ; i<_sNodes.end(maxDepth-1) ; i++ ) metSolution[i] = solution[i]; + for( int d=_minDepth+1 ; d=_minDepth ; d-- ) + { + _Evaluator< FEMDegree > evaluator; + evaluator.set( d-1 , _dirichlet ); + std::vector< ConstPointSupportKey< FEMDegree > > neighborKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i( _sNodes.treeNodes[i] ) ) + { + ConstPointSupportKey< FEMDegree >& neighborKey = neighborKeys[ omp_get_thread_num() ]; + TreeOctNode* node = _sNodes.treeNodes[i]; + Real value = Real(0); + if( node->children ) + { + if( NormalDegree&1 ) value = nodeValues[ node->children->nodeData.nodeIndex ]; + else for( int c=0 ; cchildren[c].nodeData.nodeIndex ] / Cube::CORNERS; + } + else if( nodeWeights.index( _sNodes.treeNodes[i] )>=0 ) + { + neighborKey.getNeighbors( node ); + int c=0 , x , y , z; + if( node->parent ) c = int( node - node->parent->children ); + Cube::FactorCornerIndex( c , x , y , z ); + + // Since evaluation requires parent indices, we need to check that the node's parent is interiorly supported + bool isInterior = _IsInteriorlySupported< FEMDegree >( node->parent ); + + if( NormalDegree&1 ) value = _getCornerValue( neighborKey , node , 0 , solution , metSolution , evaluator , isInterior ); + else value = _getCenterValue( neighborKey , node , solution , metSolution , evaluator , isInterior ); + } + nodeValues[i] = value; + int idx = nodeWeights.index( _sNodes.treeNodes[i] ); + if( idx!=-1 ) + { + Real w = nodeWeights.data[ idx ]; + if( w!=0 ) isoValue += value * w , weightSum += w; + } + } + } + metSolution.resize( 0 ); + FreePointer( nodeValues ); + + return isoValue / weightSum; +} + +template< class Real > +template< class Vertex , int FEMDegree > +void Octree< Real >::SetSliceIsoCorners( const DenseNodeData< Real , FEMDegree >& solution , const DenseNodeData< Real , FEMDegree >& coarseSolution , Real isoValue , int depth , int slice , std::vector< SlabValues< Vertex > >& slabValues , const _Evaluator< FEMDegree >& evaluator , int threads ) +{ + if( slice>0 ) SetSliceIsoCorners( solution , coarseSolution , isoValue , depth , slice , 1 , slabValues , evaluator , threads ); + if( slice<(1< +template< class Vertex , int FEMDegree > +void Octree< Real >::SetSliceIsoCorners( const DenseNodeData< Real , FEMDegree >& solution , const DenseNodeData< Real , FEMDegree >& coarseSolution , Real isoValue , int depth , int slice , int z , std::vector< SlabValues< Vertex > >& slabValues , const struct _Evaluator< FEMDegree >& evaluator , int threads ) +{ + typename Octree::template SliceValues< Vertex >& sValues = slabValues[depth].sliceValues( slice ); + std::vector< ConstPointSupportKey< FEMDegree > > neighborKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i( _sNodes.treeNodes[i] ) ) + { + Real squareValues[ Square::CORNERS ]; + ConstPointSupportKey< FEMDegree >& neighborKey = neighborKeys[ omp_get_thread_num() ]; + TreeOctNode* leaf = _sNodes.treeNodes[i]; + if( !leaf->children ) + { + const typename SortedTreeNodes::SquareCornerIndices& cIndices = sValues.sliceData.cornerIndices( leaf ); + + bool isInterior = _IsInteriorlySupported< FEMDegree >( leaf->parent ); + neighborKey.getNeighbors( leaf ); + + for( int x=0 ; x<2 ; x++ ) for( int y=0 ; y<2 ; y++ ) + { + int cc = Cube::CornerIndex( x , y , z ); + int fc = Square::CornerIndex( x , y ); + int vIndex = cIndices[fc]; + if( !sValues.cornerSet[vIndex] ) + { + if( sValues.cornerGradients ) + { + std::pair< Real , Point3D< Real > > p = _getCornerValueAndGradient( neighborKey , leaf , cc , solution , coarseSolution , evaluator , isInterior ); + sValues.cornerValues[vIndex] = p.first , sValues.cornerGradients[vIndex] = p.second; + } + else sValues.cornerValues[vIndex] = _getCornerValue( neighborKey , leaf , cc , solution , coarseSolution , evaluator , isInterior ); + sValues.cornerSet[vIndex] = 1; + } + squareValues[fc] = sValues.cornerValues[ vIndex ]; + TreeOctNode* node = leaf; + int _depth = depth , _slice = slice; + while( _IsValidNode< 0 >( node->parent ) && (node-node->parent->children)==cc ) + { + node = node->parent , _depth-- , _slice >>= 1; + typename Octree::template SliceValues< Vertex >& _sValues = slabValues[_depth].sliceValues( _slice ); + const typename SortedTreeNodes::SquareCornerIndices& _cIndices = _sValues.sliceData.cornerIndices( node ); + int _vIndex = _cIndices[fc]; + _sValues.cornerValues[_vIndex] = sValues.cornerValues[vIndex]; + if( _sValues.cornerGradients ) _sValues.cornerGradients[_vIndex] = sValues.cornerGradients[vIndex]; + _sValues.cornerSet[_vIndex] = 1; + } + } + sValues.mcIndices[ i - sValues.sliceData.nodeOffset ] = MarchingSquares::GetIndex( squareValues , isoValue ); + } + } +} + +template< class Real > +template< int WeightDegree , int ColorDegree , class Vertex > +void Octree< Real >::SetSliceIsoVertices( const BSplineData< ColorDegree >* colorBSData , const SparseNodeData< Real , WeightDegree >* densityWeights , const SparseNodeData< ProjectiveData< Point3D< Real > > , ColorDegree >* colorData , Real isoValue , int depth , int slice , int& vOffset , CoredMeshData< Vertex >& mesh , std::vector< SlabValues< Vertex > >& slabValues , int threads ) +{ + if( slice>0 ) SetSliceIsoVertices< WeightDegree , ColorDegree >( colorBSData , densityWeights , colorData , isoValue , depth , slice , 1 , vOffset , mesh , slabValues , threads ); + if( slice<(1<( colorBSData , densityWeights , colorData , isoValue , depth , slice , 0 , vOffset , mesh , slabValues , threads ); +} +template< class Real > +template< int WeightDegree , int ColorDegree , class Vertex > +void Octree< Real >::SetSliceIsoVertices( const BSplineData< ColorDegree >* colorBSData , const SparseNodeData< Real , WeightDegree >* densityWeights , const SparseNodeData< ProjectiveData< Point3D< Real > > , ColorDegree >* colorData , Real isoValue , int depth , int slice , int z , int& vOffset , CoredMeshData< Vertex >& mesh , std::vector< SlabValues< Vertex > >& slabValues , int threads ) +{ + typename Octree::template SliceValues< Vertex >& sValues = slabValues[depth].sliceValues( slice ); + // [WARNING] In the case Degree=2, these two keys are the same, so we don't have to maintain them separately. + std::vector< ConstAdjacenctNodeKey > neighborKeys( std::max< int >( 1 , threads ) ); + std::vector< ConstPointSupportKey< WeightDegree > > weightKeys( std::max< int >( 1 , threads ) ); + std::vector< ConstPointSupportKey< ColorDegree > > colorKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i( _sNodes.treeNodes[i] ) ) + { + ConstAdjacenctNodeKey& neighborKey = neighborKeys[ omp_get_thread_num() ]; + ConstPointSupportKey< WeightDegree >& weightKey = weightKeys[ omp_get_thread_num() ]; + ConstPointSupportKey< ColorDegree >& colorKey = colorKeys[ omp_get_thread_num() ]; + TreeOctNode* leaf = _sNodes.treeNodes[i]; + if( !leaf->children ) + { + int idx = i - sValues.sliceData.nodeOffset; + const typename SortedTreeNodes::SquareEdgeIndices& eIndices = sValues.sliceData.edgeIndices( leaf ); + if( MarchingSquares::HasRoots( sValues.mcIndices[idx] ) ) + { + neighborKey.getNeighbors( leaf ); + if( densityWeights ) weightKey.getNeighbors( leaf ); + if( colorData ) colorKey.getNeighbors( leaf ); + for( int e=0 ; e hashed_vertex; +#pragma omp critical (add_point_access) + { + if( !sValues.edgeSet[vIndex] ) + { + mesh.addOutOfCorePoint( vertex ); + sValues.edgeSet[ vIndex ] = 1; + sValues.edgeKeys[ vIndex ] = key; + sValues.edgeVertexMap[key] = hashed_vertex = std::pair< int , Vertex >( vOffset , vertex ); + vOffset++; + stillOwner = true; + } + } + if( stillOwner ) + { + // We only need to pass the iso-vertex down if the edge it lies on is adjacent to a coarser leaf + bool isNeeded; + switch( o ) + { + case 0: isNeeded = ( !_IsValidNode< 0 >( neighborKey.neighbors[depth].neighbors[1][2*y][1] ) || !_IsValidNode< 0 >( neighborKey.neighbors[depth].neighbors[1][2*y][2*z] ) || !_IsValidNode< 0 >( neighborKey.neighbors[depth].neighbors[1][1][2*z] ) ) ; break; + case 1: isNeeded = ( !_IsValidNode< 0 >( neighborKey.neighbors[depth].neighbors[2*y][1][1] ) || !_IsValidNode< 0 >( neighborKey.neighbors[depth].neighbors[2*y][1][2*z] ) || !_IsValidNode< 0 >( neighborKey.neighbors[depth].neighbors[1][1][2*z] ) ) ; break; + } + if( isNeeded ) + { + int f[2]; + Cube::FacesAdjacentToEdge( Cube::EdgeIndex( o , y , z ) , f[0] , f[1] ); + for( int k=0 ; k<2 ; k++ ) + { + TreeOctNode* node = leaf; + int _depth = depth , _slice = slice; + bool _isNeeded = isNeeded; + while( _isNeeded && node->parent && Cube::IsFaceCorner( (int)(node-node->parent->children) , f[k] ) ) + { + node = node->parent , _depth-- , _slice >>= 1; + typename Octree::template SliceValues< Vertex >& _sValues = slabValues[_depth].sliceValues( _slice ); +#pragma omp critical (add_coarser_point_access) + _sValues.edgeVertexMap[key] = hashed_vertex; + switch( o ) + { + case 0: _isNeeded = ( !_IsValidNode< 0 >( neighborKey.neighbors[_depth].neighbors[1][2*y][1] ) || !_IsValidNode< 0 >( neighborKey.neighbors[_depth].neighbors[1][2*y][2*z] ) || !_IsValidNode< 0 >( neighborKey.neighbors[_depth].neighbors[1][1][2*z] ) ) ; break; + case 1: _isNeeded = ( !_IsValidNode< 0 >( neighborKey.neighbors[_depth].neighbors[2*y][1][1] ) || !_IsValidNode< 0 >( neighborKey.neighbors[_depth].neighbors[2*y][1][2*z] ) || !_IsValidNode< 0 >( neighborKey.neighbors[_depth].neighbors[1][1][2*z] ) ) ; break; + } + } + } + } + } + } + } + } + } + } +} +template< class Real > +template< int WeightDegree , int ColorDegree , class Vertex > +void Octree< Real >::SetXSliceIsoVertices( const BSplineData< ColorDegree >* colorBSData , const SparseNodeData< Real , WeightDegree >* densityWeights , const SparseNodeData< ProjectiveData< Point3D< Real > > , ColorDegree >* colorData , Real isoValue , int depth , int slab , int& vOffset , CoredMeshData< Vertex >& mesh , std::vector< SlabValues< Vertex > >& slabValues , int threads ) +{ + typename Octree::template SliceValues< Vertex >& bValues = slabValues[depth].sliceValues ( slab ); + typename Octree::template SliceValues< Vertex >& fValues = slabValues[depth].sliceValues ( slab+1 ); + typename Octree::template XSliceValues< Vertex >& xValues = slabValues[depth].xSliceValues( slab ); + + // [WARNING] In the case Degree=2, these two keys are the same, so we don't have to maintain them separately. + std::vector< ConstAdjacenctNodeKey > neighborKeys( std::max< int >( 1 , threads ) ); + std::vector< ConstPointSupportKey< WeightDegree > > weightKeys( std::max< int >( 1 , threads ) ); + std::vector< ConstPointSupportKey< ColorDegree > > colorKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i( _sNodes.treeNodes[i] ) ) + { + ConstAdjacenctNodeKey& neighborKey = neighborKeys[ omp_get_thread_num() ]; + ConstPointSupportKey< WeightDegree >& weightKey = weightKeys[ omp_get_thread_num() ]; + ConstPointSupportKey< ColorDegree >& colorKey = colorKeys[ omp_get_thread_num() ]; + TreeOctNode* leaf = _sNodes.treeNodes[i]; + if( !leaf->children ) + { + unsigned char mcIndex = ( bValues.mcIndices[ i - bValues.sliceData.nodeOffset ] ) | ( fValues.mcIndices[ i - fValues.sliceData.nodeOffset ] )<<4; + const typename SortedTreeNodes::SquareCornerIndices& eIndices = xValues.xSliceData.edgeIndices( leaf ); + if( MarchingCubes::HasRoots( mcIndex ) ) + { + neighborKey.getNeighbors( leaf ); + if( densityWeights ) weightKey.getNeighbors( leaf ); + if( colorData ) colorKey.getNeighbors( leaf ); + for( int x=0 ; x<2 ; x++ ) for( int y=0 ; y<2 ; y++ ) + { + int c = Square::CornerIndex( x , y ); + int e = Cube::EdgeIndex( 2 , x , y ); + if( MarchingCubes::HasEdgeRoots( mcIndex , e ) ) + { + int vIndex = eIndices[c]; + if( !xValues.edgeSet[vIndex] ) + { + Vertex vertex; + long long key = VertexData::EdgeIndex( leaf , e , _sNodes.levels() ); + GetIsoVertex( colorBSData , densityWeights , colorData , isoValue , weightKey , colorKey , leaf , c , bValues , fValues , vertex ); + vertex.point = vertex.point * _scale + _center; + bool stillOwner = false; + std::pair< int , Vertex > hashed_vertex; +#pragma omp critical (add_x_point_access) + { + if( !xValues.edgeSet[vIndex] ) + { + mesh.addOutOfCorePoint( vertex ); + xValues.edgeSet[ vIndex ] = 1; + xValues.edgeKeys[ vIndex ] = key; + xValues.edgeVertexMap[key] = hashed_vertex = std::pair< int , Vertex >( vOffset , vertex ); + stillOwner = true; + vOffset++; + } + } + if( stillOwner ) + { + // We only need to pass the iso-vertex down if the edge it lies on is adjacent to a coarser leaf + bool isNeeded = ( !_IsValidNode< 0 >( neighborKey.neighbors[depth].neighbors[2*x][1][1] ) || !_IsValidNode< 0 >( neighborKey.neighbors[depth].neighbors[2*x][2*y][1] ) || !_IsValidNode< 0 >( neighborKey.neighbors[depth].neighbors[1][2*y][1] ) ); + if( isNeeded ) + { + int f[2]; + Cube::FacesAdjacentToEdge( e , f[0] , f[1] ); + for( int k=0 ; k<2 ; k++ ) + { + TreeOctNode* node = leaf; + int _depth = depth , _slab = slab; + bool _isNeeded = isNeeded; + while( _isNeeded && node->parent && Cube::IsFaceCorner( (int)(node-node->parent->children) , f[k] ) ) + { + node = node->parent , _depth-- , _slab >>= 1; + typename Octree::template XSliceValues< Vertex >& _xValues = slabValues[_depth].xSliceValues( _slab ); +#pragma omp critical (add_x_coarser_point_access) + _xValues.edgeVertexMap[key] = hashed_vertex; + _isNeeded = ( !_IsValidNode< 0 >( neighborKey.neighbors[_depth].neighbors[2*x][1][1] ) || !_IsValidNode< 0 >( neighborKey.neighbors[_depth].neighbors[2*x][2*y][1] ) || !_IsValidNode< 0 >( neighborKey.neighbors[_depth].neighbors[1][2*y][1] ) ); + } + } + } + } + } + } + } + } + } + } +} +template< class Real > +template< class Vertex > +void Octree< Real >::CopyFinerSliceIsoEdgeKeys( int depth , int slice , std::vector< SlabValues< Vertex > >& slabValues , int threads ) +{ + if( slice>0 ) CopyFinerSliceIsoEdgeKeys( depth , slice , 1 , slabValues , threads ); + if( slice<(1< +template< class Vertex > +void Octree< Real >::CopyFinerSliceIsoEdgeKeys( int depth , int slice , int z , std::vector< SlabValues< Vertex > >& slabValues , int threads ) +{ + SliceValues< Vertex >& pSliceValues = slabValues[depth ].sliceValues(slice ); + SliceValues< Vertex >& cSliceValues = slabValues[depth+1].sliceValues(slice<<1); + typename SortedTreeNodes::SliceTableData& pSliceData = pSliceValues.sliceData; + typename SortedTreeNodes::SliceTableData& cSliceData = cSliceValues.sliceData; +#pragma omp parallel for num_threads( threads ) + for( int i=_sNodes.begin(depth,slice-z) ; i<_sNodes.end(depth,slice-z) ; i++ ) if( _IsValidNode< 0 >( _sNodes.treeNodes[i] ) ) + if( _sNodes.treeNodes[i]->children ) + { + typename SortedTreeNodes::SquareEdgeIndices& pIndices = pSliceData.edgeIndices( i ); + // Copy the edges that overlap the coarser edges + for( int orientation=0 ; orientation<2 ; orientation++ ) for( int y=0 ; y<2 ; y++ ) + { + int fe = Square::EdgeIndex( orientation , y ); + int pIndex = pIndices[fe]; + if( !pSliceValues.edgeSet[ pIndex ] ) + { + int ce = Cube::EdgeIndex( orientation , y , z ); + int c1 , c2; + switch( orientation ) + { + case 0: c1 = Cube::CornerIndex( 0 , y , z ) , c2 = Cube::CornerIndex( 1 , y , z ) ; break; + case 1: c1 = Cube::CornerIndex( y , 0 , z ) , c2 = Cube::CornerIndex( y , 1 , z ) ; break; + } + // [SANITY CHECK] +// if( _IsValidNode< 0 >( _sNodes.treeNodes[i]->children + c1 )!=_IsValidNode< 0 >( _sNodes.treeNodes[i]->children + c2 ) ) fprintf( stderr , "[WARNING] Finer edges should both be valid or invalid\n" ) , exit( 0 ); + if( !_IsValidNode< 0 >( _sNodes.treeNodes[i]->children + c1 ) || !_IsValidNode< 0 >( _sNodes.treeNodes[i]->children + c2 ) ) continue; + + int cIndex1 = cSliceData.edgeIndices( _sNodes.treeNodes[i]->children + c1 )[fe]; + int cIndex2 = cSliceData.edgeIndices( _sNodes.treeNodes[i]->children + c2 )[fe]; + if( cSliceValues.edgeSet[cIndex1] != cSliceValues.edgeSet[cIndex2] ) + { + long long key; + if( cSliceValues.edgeSet[cIndex1] ) key = cSliceValues.edgeKeys[cIndex1]; + else key = cSliceValues.edgeKeys[cIndex2]; + std::pair< int , Vertex > vPair = cSliceValues.edgeVertexMap.find( key )->second; +#pragma omp critical ( copy_finer_edge_keys ) + pSliceValues.edgeVertexMap[key] = vPair; + pSliceValues.edgeKeys[pIndex] = key; + pSliceValues.edgeSet[pIndex] = 1; + } + else if( cSliceValues.edgeSet[cIndex1] && cSliceValues.edgeSet[cIndex2] ) + { + long long key1 = cSliceValues.edgeKeys[cIndex1] , key2 = cSliceValues.edgeKeys[cIndex2]; +#pragma omp critical ( set_edge_pairs ) + pSliceValues.vertexPairMap[ key1 ] = key2 , pSliceValues.vertexPairMap[ key2 ] = key1; + + const TreeOctNode* node = _sNodes.treeNodes[i]; + int _depth = depth , _slice = slice; + while( node->parent && Cube::IsEdgeCorner( (int)( node - node->parent->children ) , ce ) ) + { + node = node->parent , _depth-- , _slice >>= 1; + SliceValues< Vertex >& _pSliceValues = slabValues[_depth].sliceValues(_slice); +#pragma omp critical ( set_edge_pairs ) + _pSliceValues.vertexPairMap[ key1 ] = key2 , _pSliceValues.vertexPairMap[ key2 ] = key1; + } + } + } + } + } +} +template< class Real > +template< class Vertex > +void Octree< Real >::CopyFinerXSliceIsoEdgeKeys( int depth , int slab , std::vector< SlabValues< Vertex > >& slabValues , int threads ) +{ + XSliceValues< Vertex >& pSliceValues = slabValues[depth ].xSliceValues(slab); + XSliceValues< Vertex >& cSliceValues0 = slabValues[depth+1].xSliceValues( (slab<<1)|0 ); + XSliceValues< Vertex >& cSliceValues1 = slabValues[depth+1].xSliceValues( (slab<<1)|1 ); + typename SortedTreeNodes::XSliceTableData& pSliceData = pSliceValues.xSliceData; + typename SortedTreeNodes::XSliceTableData& cSliceData0 = cSliceValues0.xSliceData; + typename SortedTreeNodes::XSliceTableData& cSliceData1 = cSliceValues1.xSliceData; +#pragma omp parallel for num_threads( threads ) + for( int i=_sNodes.begin(depth,slab) ; i<_sNodes.end(depth,slab) ; i++ ) if( _IsValidNode< 0 >( _sNodes.treeNodes[i] ) ) + if( _sNodes.treeNodes[i]->children ) + { + typename SortedTreeNodes::SquareCornerIndices& pIndices = pSliceData.edgeIndices( i ); + for( int x=0 ; x<2 ; x++ ) for( int y=0 ; y<2 ; y++ ) + { + int fc = Square::CornerIndex( x , y ); + int pIndex = pIndices[fc]; + if( !pSliceValues.edgeSet[pIndex] ) + { + int c0 = Cube::CornerIndex( x , y , 0 ) , c1 = Cube::CornerIndex( x , y , 1 ); + + // [SANITY CHECK] +// if( _IsValidNode< 0 >( _sNodes.treeNodes[i]->children + c0 )!=_IsValidNode< 0 >( _sNodes.treeNodes[i]->children + c1 ) ) fprintf( stderr , "[ERROR] Finer edges should both be valid or invalid\n" ) , exit( 0 ); + if( !_IsValidNode< 0 >( _sNodes.treeNodes[i]->children + c0 ) || !_IsValidNode< 0 >( _sNodes.treeNodes[i]->children + c1 ) ) continue; + + int cIndex0 = cSliceData0.edgeIndices( _sNodes.treeNodes[i]->children + c0 )[fc]; + int cIndex1 = cSliceData1.edgeIndices( _sNodes.treeNodes[i]->children + c1 )[fc]; + if( cSliceValues0.edgeSet[cIndex0] != cSliceValues1.edgeSet[cIndex1] ) + { + long long key; + std::pair< int , Vertex > vPair; + if( cSliceValues0.edgeSet[cIndex0] ) key = cSliceValues0.edgeKeys[cIndex0] , vPair = cSliceValues0.edgeVertexMap.find( key )->second; + else key = cSliceValues1.edgeKeys[cIndex1] , vPair = cSliceValues1.edgeVertexMap.find( key )->second; +#pragma omp critical ( copy_finer_x_edge_keys ) + pSliceValues.edgeVertexMap[key] = vPair; + pSliceValues.edgeKeys[ pIndex ] = key; + pSliceValues.edgeSet[ pIndex ] = 1; + } + else if( cSliceValues0.edgeSet[cIndex0] && cSliceValues1.edgeSet[cIndex1] ) + { + long long key0 = cSliceValues0.edgeKeys[cIndex0] , key1 = cSliceValues1.edgeKeys[cIndex1]; +#pragma omp critical ( set_x_edge_pairs ) + pSliceValues.vertexPairMap[ key0 ] = key1 , pSliceValues.vertexPairMap[ key1 ] = key0; + const TreeOctNode* node = _sNodes.treeNodes[i]; + int _depth = depth , _slab = slab , ce = Cube::CornerIndex( 2 , x , y ); + while( node->parent && Cube::IsEdgeCorner( (int)( node - node->parent->children ) , ce ) ) + { + node = node->parent , _depth-- , _slab>>= 1; + SliceValues< Vertex >& _pSliceValues = slabValues[_depth].sliceValues(_slab); +#pragma omp critical ( set_x_edge_pairs ) + _pSliceValues.vertexPairMap[ key0 ] = key1 , _pSliceValues.vertexPairMap[ key1 ] = key0; + } + } + } + } + } +} +template< class Real > +template< class Vertex > +void Octree< Real >::SetSliceIsoEdges( int depth , int slice , std::vector< SlabValues< Vertex > >& slabValues , int threads ) +{ + if( slice>0 ) SetSliceIsoEdges( depth , slice , 1 , slabValues , threads ); + if( slice<(1< +template< class Vertex > +void Octree< Real >::SetSliceIsoEdges( int depth , int slice , int z , std::vector< SlabValues< Vertex > >& slabValues , int threads ) +{ + typename Octree::template SliceValues< Vertex >& sValues = slabValues[depth].sliceValues( slice ); + std::vector< ConstAdjacenctNodeKey > neighborKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i( _sNodes.treeNodes[i] ) ) + { + int isoEdges[ 2 * MarchingSquares::MAX_EDGES ]; + ConstAdjacenctNodeKey& neighborKey = neighborKeys[ omp_get_thread_num() ]; + TreeOctNode* leaf = _sNodes.treeNodes[i]; + if( !leaf->children ) + { + int idx = i - sValues.sliceData.nodeOffset; + const typename SortedTreeNodes::SquareEdgeIndices& eIndices = sValues.sliceData.edgeIndices( leaf ); + const typename SortedTreeNodes::SquareFaceIndices& fIndices = sValues.sliceData.faceIndices( leaf ); + unsigned char mcIndex = sValues.mcIndices[idx]; + if( !sValues.faceSet[ fIndices[0] ] ) + { + neighborKey.getNeighbors( leaf ); + if( !neighborKey.neighbors[depth].neighbors[1][1][2*z] || !neighborKey.neighbors[depth].neighbors[1][1][2*z]->children ) + { + FaceEdges fe; + fe.count = MarchingSquares::AddEdgeIndices( mcIndex , isoEdges ); + for( int j=0 ; j edges; + edges.resize( fe.count ); + for( int j=0 ; jparent && Cube::IsFaceCorner( (int)(node-node->parent->children) , f ) ) + { + node = node->parent , _depth-- , _slice >>= 1; + if( neighborKey.neighbors[_depth].neighbors[1][1][2*z] && neighborKey.neighbors[_depth].neighbors[1][1][2*z]->children ) break; + long long key = VertexData::FaceIndex( node , f , _sNodes.levels() ); +#pragma omp critical( add_iso_edge_access ) + { + typename Octree::template SliceValues< Vertex >& _sValues = slabValues[_depth].sliceValues( _slice ); + typename hash_map< long long , std::vector< IsoEdge > >::iterator iter = _sValues.faceEdgeMap.find(key); + if( iter==_sValues.faceEdgeMap.end() ) _sValues.faceEdgeMap[key] = edges; + else for( int j=0 ; jsecond.push_back( fe.edges[j] ); + } + } + } + } + } + } +} +template< class Real > +template< class Vertex > +void Octree< Real >::SetXSliceIsoEdges( int depth , int slab , std::vector< SlabValues< Vertex > >& slabValues , int threads ) +{ + typename Octree::template SliceValues< Vertex >& bValues = slabValues[depth].sliceValues ( slab ); + typename Octree::template SliceValues< Vertex >& fValues = slabValues[depth].sliceValues ( slab+1 ); + typename Octree::template XSliceValues< Vertex >& xValues = slabValues[depth].xSliceValues( slab ); + + std::vector< ConstAdjacenctNodeKey > neighborKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i( _sNodes.treeNodes[i] ) ) + { + int isoEdges[ 2 * MarchingSquares::MAX_EDGES ]; + ConstAdjacenctNodeKey& neighborKey = neighborKeys[ omp_get_thread_num() ]; + TreeOctNode* leaf = _sNodes.treeNodes[i]; + if( !leaf->children ) + { + const typename SortedTreeNodes::SquareCornerIndices& cIndices = xValues.xSliceData.edgeIndices( leaf ); + const typename SortedTreeNodes::SquareEdgeIndices& eIndices = xValues.xSliceData.faceIndices( leaf ); + unsigned char mcIndex = ( bValues.mcIndices[ i - bValues.sliceData.nodeOffset ] ) | ( fValues.mcIndices[ i - fValues.sliceData.nodeOffset ]<<4 ); + { + neighborKey.getNeighbors( leaf ); + for( int o=0 ; o<2 ; o++ ) for( int x=0 ; x<2 ; x++ ) + { + int e = Square::EdgeIndex( o , x ); + int f = Cube::FaceIndex( 1-o , x ); + unsigned char _mcIndex = MarchingCubes::GetFaceIndex( mcIndex , f ); + int xx = o==1 ? 2*x : 1 , yy = o==0 ? 2*x : 1 , zz = 1; + if( !xValues.faceSet[ eIndices[e] ] && ( !neighborKey.neighbors[depth].neighbors[xx][yy][zz] || !neighborKey.neighbors[depth].neighbors[xx][yy][zz]->children ) ) + { + FaceEdges fe; + fe.count = MarchingSquares::AddEdgeIndices( _mcIndex , isoEdges ); + for( int j=0 ; j& sValues = (_x==0) ? bValues : fValues; + int idx = sValues.sliceData.edgeIndices(i)[ Square::EdgeIndex(o,x) ]; + if( !sValues.edgeSet[ idx ] ) fprintf( stderr , "[ERROR] Edge not set 5: %d / %d\n" , slab , 1< edges; + edges.resize( fe.count ); + for( int j=0 ; jparent && Cube::IsFaceCorner( (int)(node-node->parent->children) , f ) ) + { + node = node->parent , _depth-- , _slab >>= 1; + if( neighborKey.neighbors[_depth].neighbors[xx][yy][zz] && neighborKey.neighbors[_depth].neighbors[xx][yy][zz]->children ) break; + long long key = VertexData::FaceIndex( node , f , _sNodes.levels() ); +#pragma omp critical( add_x_iso_edge_access ) + { + typename Octree::template XSliceValues< Vertex >& _xValues = slabValues[_depth].xSliceValues( _slab ); + typename hash_map< long long , std::vector< IsoEdge > >::iterator iter = _xValues.faceEdgeMap.find(key); + if( iter==_xValues.faceEdgeMap.end() ) _xValues.faceEdgeMap[key] = edges; + else for( int j=0 ; jsecond.push_back( fe.edges[j] ); + } + } + } + } + } + } + } +} +template< class Real > +template< class Vertex > +void Octree< Real >::SetIsoSurface( int depth , int offset , const SliceValues< Vertex >& bValues , const SliceValues< Vertex >& fValues , const XSliceValues< Vertex >& xValues , CoredMeshData< Vertex >& mesh , bool polygonMesh , bool addBarycenter , int& vOffset , int threads ) +{ + std::vector< std::pair< int , Vertex > > polygon; + std::vector< std::vector< IsoEdge > > edgess( std::max< int >( 1 , threads ) ); +#pragma omp parallel for num_threads( threads ) + for( int i=_sNodes.begin(depth,offset) ; i<_sNodes.end(depth,offset) ; i++ ) if( _IsValidNode< 0 >( _sNodes.treeNodes[i] ) ) + { + std::vector< IsoEdge >& edges = edgess[ omp_get_thread_num() ]; + TreeOctNode* leaf = _sNodes.treeNodes[i]; + int d , off[3]; + leaf->depthAndOffset( d , off ); + int res = _Resolution( depth ); + bool inBounds = off[0]children ) + { + edges.clear(); + unsigned char mcIndex = ( bValues.mcIndices[ i - bValues.sliceData.nodeOffset ] ) | ( fValues.mcIndices[ i - fValues.sliceData.nodeOffset ]<<4 ); + // [WARNING] Just because the node looks empty doesn't mean it doesn't get eges from finer neighbors + { + // Gather the edges from the faces (with the correct orientation) + for( int f=0 ; f& sValues = (o==0) ? bValues : fValues; + int fIdx = sValues.sliceData.faceIndices(i)[0]; + if( sValues.faceSet[fIdx] ) + { + const FaceEdges& fe = sValues.faceEdges[ fIdx ]; + for( int j=0 ; j >::const_iterator iter = sValues.faceEdgeMap.find( key ); + if( iter!=sValues.faceEdgeMap.end() ) + { + const std::vector< IsoEdge >& _edges = iter->second; + for( size_t j=0 ; j<_edges.size() ; j++ ) edges.push_back( IsoEdge( _edges[j][flip] , _edges[j][1-flip] ) ); + } + else fprintf( stderr , "[ERROR] Invalid faces: %d %d %d\n" , i , d , o ) , exit( 0 ); + } + } + else + { + int fIdx = xValues.xSliceData.faceIndices(i)[ Square::EdgeIndex( 1-d , o ) ]; + if( xValues.faceSet[fIdx] ) + { + const FaceEdges& fe = xValues.faceEdges[ fIdx ]; + for( int j=0 ; j >::const_iterator iter = xValues.faceEdgeMap.find( key ); + if( iter!=xValues.faceEdgeMap.end() ) + { + const std::vector< IsoEdge >& _edges = iter->second; + for( size_t j=0 ; j<_edges.size() ; j++ ) edges.push_back( IsoEdge( _edges[j][flip] , _edges[j][1-flip] ) ); + } + else fprintf( stderr , "[ERROR] Invalid faces: %d %d %d\n" , i , d , o ) , exit( 0 ); + } + } + } + // Get the edge loops + std::vector< std::vector< long long > > loops; + while( edges.size() ) + { + loops.resize( loops.size()+1 ); + IsoEdge edge = edges.back(); + edges.pop_back(); + long long start = edge[0] , current = edge[1]; + while( current!=start ) + { + int idx; + for( idx=0 ; idx<(int)edges.size() ; idx++ ) if( edges[idx][0]==current ) break; + if( idx==edges.size() ) + { + typename hash_map< long long , long long >::const_iterator iter; + if ( (iter=bValues.vertexPairMap.find(current))!=bValues.vertexPairMap.end() ) loops.back().push_back( current ) , current = iter->second; + else if( (iter=fValues.vertexPairMap.find(current))!=fValues.vertexPairMap.end() ) loops.back().push_back( current ) , current = iter->second; + else if( (iter=xValues.vertexPairMap.find(current))!=xValues.vertexPairMap.end() ) loops.back().push_back( current ) , current = iter->second; + else + { + int d , off[3]; + leaf->depthAndOffset( d , off ); + fprintf( stderr , "[ERROR] Failed to close loop [%d: %d %d %d] | (%d): %lld\n" , d-1 , off[0] , off[1] , off[2] , i , current ); + exit( 0 ); + } + } + else + { + loops.back().push_back( current ); + current = edges[idx][1]; + edges[idx] = edges.back() , edges.pop_back(); + } + } + loops.back().push_back( start ); + } + // Add the loops to the mesh + for( size_t j=0 ; j > polygon( loops[j].size() ); + for( size_t k=0 ; k >::const_iterator iter; + if ( ( iter=bValues.edgeVertexMap.find( key ) )!=bValues.edgeVertexMap.end() ) polygon[k] = iter->second; + else if( ( iter=fValues.edgeVertexMap.find( key ) )!=fValues.edgeVertexMap.end() ) polygon[k] = iter->second; + else if( ( iter=xValues.edgeVertexMap.find( key ) )!=xValues.edgeVertexMap.end() ) polygon[k] = iter->second; + else fprintf( stderr , "[ERROR] Couldn't find vertex in edge map\n" ) , exit( 0 ); + } + AddIsoPolygons( mesh , polygon , polygonMesh , addBarycenter , vOffset ); + } + } + } + } +} +template< class Real > void SetColor( Point3D< Real >& color , unsigned char c[3] ){ for( int i=0 ; i<3 ; i++ ) c[i] = (unsigned char)std::max< int >( 0 , std::min< int >( 255 , (int)( color[i]+0.5 ) ) ); } + +template< class Real > void SetIsoVertex( PlyVertex< float >& vertex , Point3D< Real > color , Real value ){ ; } +template< class Real > void SetIsoVertex( PlyColorVertex< float >& vertex , Point3D< Real > color , Real value ){ SetColor( color , vertex.color ); } +template< class Real > void SetIsoVertex( PlyValueVertex< float >& vertex , Point3D< Real > color , Real value ){ vertex.value = float(value); } +template< class Real > void SetIsoVertex( PlyColorAndValueVertex< float >& vertex , Point3D< Real > color , Real value ){ SetColor( color , vertex.color ) , vertex.value = float(value); } +template< class Real > void SetIsoVertex( PlyVertex< double >& vertex , Point3D< Real > color , Real value ){ ; } +template< class Real > void SetIsoVertex( PlyColorVertex< double >& vertex , Point3D< Real > color , Real value ){ SetColor( color , vertex.color ); } +template< class Real > void SetIsoVertex( PlyValueVertex< double >& vertex , Point3D< Real > color , Real value ){ vertex.value = double(value); } +template< class Real > void SetIsoVertex( PlyColorAndValueVertex< double >& vertex , Point3D< Real > color , Real value ){ SetColor( color , vertex.color ) , vertex.value = double(value); } + +template< class Real > +template< int WeightDegree , int ColorDegree , class Vertex > +bool Octree< Real >::GetIsoVertex( const BSplineData< ColorDegree >* colorBSData , const SparseNodeData< Real , WeightDegree >* densityWeights , const SparseNodeData< ProjectiveData< Point3D< Real > > , ColorDegree >* colorData , Real isoValue , ConstPointSupportKey< WeightDegree >& weightKey , ConstPointSupportKey< ColorDegree >& colorKey , const TreeOctNode* node , int edgeIndex , int z , const SliceValues< Vertex >& sValues , Vertex& vertex ) +{ + Point3D< Real > position; + int c0 , c1; + Square::EdgeCorners( edgeIndex , c0 , c1 ); + + bool nonLinearFit = sValues.cornerGradients!=NullPointer( Point3D< Real > ); + const typename SortedTreeNodes::SquareCornerIndices& idx = sValues.sliceData.cornerIndices( node ); + Real x0 = sValues.cornerValues[idx[c0]] , x1 = sValues.cornerValues[idx[c1]]; + Point3D< Real > s; + Real start , width; + _StartAndWidth( node , s , width ); + int o , y; + Square::FactorEdgeIndex( edgeIndex , o , y ); + start = s[o]; + switch( o ) + { + case 0: + position[1] = s[1] + width*y; + position[2] = s[2] + width*z; + break; + case 1: + position[0] = s[0] + width*y; + position[2] = s[2] + width*z; + break; + } + + double averageRoot; + if( nonLinearFit ) + { + double dx0 = sValues.cornerGradients[idx[c0]][o] * width , dx1 = sValues.cornerGradients[idx[c1]][o] * width; + + // The scaling will turn the Hermite Spline into a quadratic + double scl = (x1-x0) / ( (dx1+dx0 ) / 2 ); + dx0 *= scl , dx1 *= scl; + + // Hermite Spline + Polynomial< 2 > P; + P.coefficients[0] = x0; + P.coefficients[1] = dx0; + P.coefficients[2] = 3*(x1-x0)-dx1-2*dx0; + + double roots[2]; + int rCount = 0 , rootCount = P.getSolutions( isoValue , roots , EPSILON ); + averageRoot = 0; + for( int i=0 ; i=0 && roots[i]<=1 ) averageRoot += roots[i] , rCount++; + averageRoot /= rCount; + } + else + { + // We have a linear function L, with L(0) = x0 and L(1) = x1 + // => L(t) = x0 + t * (x1-x0) + // => L(t) = isoValue <=> t = ( isoValue - x0 ) / ( x1 - x0 ) + if( x0==x1 ) fprintf( stderr , "[ERROR] Not a zero-crossing root: %g %g\n" , x0 , x1 ) , exit( 0 ); + averageRoot = ( isoValue - x0 ) / ( x1 - x0 ); + } + if( averageRoot<0 || averageRoot>1 ) + { + fprintf( stderr , "[WARNING] Bad average root: %f\n" , averageRoot ); + fprintf( stderr , "\t(%f %f) (%f)\n" , x0 , x1 , isoValue ); + if( averageRoot<0 ) averageRoot = 0; + if( averageRoot>1 ) averageRoot = 1; + } + position[o] = Real( start + width*averageRoot ); + vertex.point = position; + Point3D< Real > color; + Real depth(0); + if( densityWeights ) + { + Real weight; + const TreeOctNode* temp = node; + while( _Depth( temp )>_splatDepth ) temp=temp->parent; + _GetSampleDepthAndWeight( *densityWeights , temp , position , weightKey , depth , weight ); + } + if( colorData ) color = Point3D< Real >( _Evaluate( *colorData , position , *colorBSData , colorKey ) ); + SetIsoVertex( vertex , color , depth ); + return true; +} +template< class Real > +template< int WeightDegree , int ColorDegree , class Vertex > +bool Octree< Real >::GetIsoVertex( const BSplineData< ColorDegree >* colorBSData , const SparseNodeData< Real , WeightDegree >* densityWeights , const SparseNodeData< ProjectiveData< Point3D< Real > > , ColorDegree >* colorData , Real isoValue , ConstPointSupportKey< WeightDegree >& weightKey , ConstPointSupportKey< ColorDegree >& colorKey , const TreeOctNode* node , int cornerIndex , const SliceValues< Vertex >& bValues , const SliceValues< Vertex >& fValues , Vertex& vertex ) +{ + Point3D< Real > position; + + bool nonLinearFit = bValues.cornerGradients!=NullPointer( Point3D< Real > ) && fValues.cornerGradients!=NullPointer( Point3D< Real > ); + const typename SortedTreeNodes::SquareCornerIndices& idx0 = bValues.sliceData.cornerIndices( node ); + const typename SortedTreeNodes::SquareCornerIndices& idx1 = fValues.sliceData.cornerIndices( node ); + Real x0 = bValues.cornerValues[ idx0[cornerIndex] ] , x1 = fValues.cornerValues[ idx1[cornerIndex] ]; + Point3D< Real > s; + Real start , width; + _StartAndWidth( node , s , width ); + start = s[2]; + int x , y; + Square::FactorCornerIndex( cornerIndex , x , y ); + + + position[0] = s[0] + width*x; + position[1] = s[1] + width*y; + + double averageRoot; + + if( nonLinearFit ) + { + double dx0 = bValues.cornerGradients[ idx0[cornerIndex] ][2] * width , dx1 = fValues.cornerGradients[ idx1[cornerIndex] ][2] * width; + // The scaling will turn the Hermite Spline into a quadratic + double scl = (x1-x0) / ( (dx1+dx0 ) / 2 ); + dx0 *= scl , dx1 *= scl; + + // Hermite Spline + Polynomial< 2 > P; + P.coefficients[0] = x0; + P.coefficients[1] = dx0; + P.coefficients[2] = 3*(x1-x0)-dx1-2*dx0; + + double roots[2]; + int rCount = 0 , rootCount = P.getSolutions( isoValue , roots , EPSILON ); + averageRoot = 0; + for( int i=0 ; i=0 && roots[i]<=1 ) averageRoot += roots[i] , rCount++; + averageRoot /= rCount; + } + else + { + // We have a linear function L, with L(0) = x0 and L(1) = x1 + // => L(t) = x0 + t * (x1-x0) + // => L(t) = isoValue <=> t = ( isoValue - x0 ) / ( x1 - x0 ) + if( x0==x1 ) fprintf( stderr , "[ERROR] Not a zero-crossing root: %g %g\n" , x0 , x1 ) , exit( 0 ); + averageRoot = ( isoValue - x0 ) / ( x1 - x0 ); + } + if( averageRoot<0 || averageRoot>1 ) + { + fprintf( stderr , "[WARNING] Bad average root: %f\n" , averageRoot ); + fprintf( stderr , "\t(%f %f) (%f)\n" , x0 , x1 , isoValue ); + if( averageRoot<0 ) averageRoot = 0; + if( averageRoot>1 ) averageRoot = 1; + } + position[2] = Real( start + width*averageRoot ); + vertex.point = position; + Point3D< Real > color; + Real depth(0); + if( densityWeights ) + { + Real weight; + const TreeOctNode* temp = node; + while( _Depth( temp )>_splatDepth ) temp=temp->parent; + _GetSampleDepthAndWeight( *densityWeights , temp , position , weightKey , depth , weight ); + } + if( colorData ) color = Point3D< Real >( _Evaluate( *colorData , position , *colorBSData , colorKey ) ); + SetIsoVertex( vertex , color , depth ); + return true; +} + +template< class Real > +template< class Vertex > +int Octree< Real >::AddIsoPolygons( CoredMeshData< Vertex >& mesh , std::vector< std::pair< int , Vertex > >& polygon , bool polygonMesh , bool addBarycenter , int& vOffset ) +{ + if( polygonMesh ) + { + std::vector< int > vertices( polygon.size() ); + for( int i=0 ; i<(int)polygon.size() ; i++ ) vertices[i] = polygon[polygon.size()-1-i].first; + mesh.addPolygon_s( vertices ); + return 1; + } + if( polygon.size()>3 ) + { + bool isCoplanar = false; + std::vector< int > triangle( 3 ); + + if( addBarycenter ) + for( int i=0 ; i<(int)polygon.size() ; i++ ) + for( int j=0 ; j MAT; + std::vector< Point3D< Real > > vertices; + std::vector< TriangleIndex > triangles; + vertices.resize( polygon.size() ); + // Add the points + for( int i=0 ; i<(int)polygon.size() ; i++ ) vertices[i] = polygon[i].second.point; + MAT.GetTriangulation( vertices , triangles ); + for( int i=0 ; i<(int)triangles.size() ; i++ ) + { + for( int j=0 ; j<3 ; j++ ) triangle[2-j] = polygon[ triangles[i].idx[j] ].first; + mesh.addPolygon_s( triangle ); + } + } + } + else if( polygon.size()==3 ) + { + std::vector< int > vertices( 3 ); + for( int i=0 ; i<3 ; i++ ) vertices[2-i] = polygon[i].first; + mesh.addPolygon_s( vertices ); + } + return (int)polygon.size()-2; +} diff --git a/colmap/include/colmap/lib/PoissonRecon/MultiGridOctreeData.SortedTreeNodes.inl b/colmap/include/colmap/lib/PoissonRecon/MultiGridOctreeData.SortedTreeNodes.inl new file mode 100644 index 0000000000000000000000000000000000000000..b7da88c6dab25b708d23235f229719bd975a6b4f --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/MultiGridOctreeData.SortedTreeNodes.inl @@ -0,0 +1,357 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +///////////////////// +// SortedTreeNodes // +///////////////////// +SortedTreeNodes::SortedTreeNodes( void ) +{ + _sliceStart = NullPointer( Pointer( int ) ); + treeNodes = NullPointer( TreeOctNode* ); + _levels = 0; +} +SortedTreeNodes::~SortedTreeNodes( void ) +{ + if( _sliceStart ) for( int d=0 ; d<_levels ; d++ ) FreePointer( _sliceStart[d] ); + FreePointer( _sliceStart ); + DeletePointer( treeNodes ); +} +void SortedTreeNodes::set( TreeOctNode& root , std::vector< int >* map ) +{ + set( root ); + + if( map ) + { + map->resize( _sliceStart[_levels-1][(size_t)1<<(_levels-1)] ); + for( int i=0 ; i<_sliceStart[_levels-1][(size_t)1<<(_levels-1)] ; i++ ) (*map)[i] = treeNodes[i]->nodeData.nodeIndex; + } + for( int i=0 ; i<_sliceStart[_levels-1][(size_t)1<<(_levels-1)] ; i++ ) treeNodes[i]->nodeData.nodeIndex = i; +} +void SortedTreeNodes::set( TreeOctNode& root ) +{ + _levels = root.maxDepth()+1; + + if( _sliceStart ) for( int d=0 ; d<_levels ; d++ ) FreePointer( _sliceStart[d] ); + FreePointer( _sliceStart ); + DeletePointer( treeNodes ); + + _sliceStart = AllocPointer< Pointer( int ) >( _levels ); + for( int l=0 ; l<_levels ; l++ ) + { + _sliceStart[l] = AllocPointer< int >( ((size_t)1<depthAndOffset( d , off ); + _sliceStart[d][ off[2]+1 ]++; + } + + // Get the start index for each slice + { + int levelOffset = 0; + for( int l=0 ; l<_levels ; l++ ) + { + _sliceStart[l][0] = levelOffset; + for( int s=0 ; s<((size_t)1<( _sliceStart[_levels-1][(size_t)1<<(_levels-1)] ); + + // Add the tree nodes + for( TreeOctNode* node=root.nextNode() ; node ; node=root.nextNode( node ) ) + { + int d , off[3]; + node->depthAndOffset( d , off ); + treeNodes[ _sliceStart[d][ off[2] ]++ ] = node; + } + + // Shift the slice offsets up since we incremented as we added + for( int l=0 ; l<_levels ; l++ ) + { + for( int s=(1<0 ; s-- ) _sliceStart[l][s] = _sliceStart[l][s-1]; + _sliceStart[l][0] = l>0 ? _sliceStart[l-1][(size_t)1<<(l-1)] : 0; + } +} +SortedTreeNodes::SquareCornerIndices& SortedTreeNodes::SliceTableData::cornerIndices( const TreeOctNode* node ) { return cTable[ node->nodeData.nodeIndex - nodeOffset ]; } +SortedTreeNodes::SquareCornerIndices& SortedTreeNodes::SliceTableData::cornerIndices( int idx ) { return cTable[ idx - nodeOffset ]; } +const SortedTreeNodes::SquareCornerIndices& SortedTreeNodes::SliceTableData::cornerIndices( const TreeOctNode* node ) const { return cTable[ node->nodeData.nodeIndex - nodeOffset ]; } +const SortedTreeNodes::SquareCornerIndices& SortedTreeNodes::SliceTableData::cornerIndices( int idx ) const { return cTable[ idx - nodeOffset ]; } +SortedTreeNodes::SquareEdgeIndices& SortedTreeNodes::SliceTableData::edgeIndices( const TreeOctNode* node ) { return eTable[ node->nodeData.nodeIndex - nodeOffset ]; } +SortedTreeNodes::SquareEdgeIndices& SortedTreeNodes::SliceTableData::edgeIndices( int idx ) { return eTable[ idx - nodeOffset ]; } +const SortedTreeNodes::SquareEdgeIndices& SortedTreeNodes::SliceTableData::edgeIndices( const TreeOctNode* node ) const { return eTable[ node->nodeData.nodeIndex - nodeOffset ]; } +const SortedTreeNodes::SquareEdgeIndices& SortedTreeNodes::SliceTableData::edgeIndices( int idx ) const { return eTable[ idx - nodeOffset ]; } +SortedTreeNodes::SquareFaceIndices& SortedTreeNodes::SliceTableData::faceIndices( const TreeOctNode* node ) { return fTable[ node->nodeData.nodeIndex - nodeOffset ]; } +SortedTreeNodes::SquareFaceIndices& SortedTreeNodes::SliceTableData::faceIndices( int idx ) { return fTable[ idx - nodeOffset ]; } +const SortedTreeNodes::SquareFaceIndices& SortedTreeNodes::SliceTableData::faceIndices( const TreeOctNode* node ) const { return fTable[ node->nodeData.nodeIndex - nodeOffset ]; } +const SortedTreeNodes::SquareFaceIndices& SortedTreeNodes::SliceTableData::faceIndices( int idx ) const { return fTable[ idx - nodeOffset ]; } +SortedTreeNodes::SquareCornerIndices& SortedTreeNodes::XSliceTableData::edgeIndices( const TreeOctNode* node ) { return eTable[ node->nodeData.nodeIndex - nodeOffset ]; } +SortedTreeNodes::SquareCornerIndices& SortedTreeNodes::XSliceTableData::edgeIndices( int idx ) { return eTable[ idx - nodeOffset ]; } +const SortedTreeNodes::SquareCornerIndices& SortedTreeNodes::XSliceTableData::edgeIndices( const TreeOctNode* node ) const { return eTable[ node->nodeData.nodeIndex - nodeOffset ]; } +const SortedTreeNodes::SquareCornerIndices& SortedTreeNodes::XSliceTableData::edgeIndices( int idx ) const { return eTable[ idx - nodeOffset ]; } +SortedTreeNodes::SquareEdgeIndices& SortedTreeNodes::XSliceTableData::faceIndices( const TreeOctNode* node ) { return fTable[ node->nodeData.nodeIndex - nodeOffset ]; } +SortedTreeNodes::SquareEdgeIndices& SortedTreeNodes::XSliceTableData::faceIndices( int idx ) { return fTable[ idx - nodeOffset ]; } +const SortedTreeNodes::SquareEdgeIndices& SortedTreeNodes::XSliceTableData::faceIndices( const TreeOctNode* node ) const { return fTable[ node->nodeData.nodeIndex - nodeOffset ]; } +const SortedTreeNodes::SquareEdgeIndices& SortedTreeNodes::XSliceTableData::faceIndices( int idx ) const { return fTable[ idx - nodeOffset ]; } + +void SortedTreeNodes::setSliceTableData( SliceTableData& sData , int depth , int offset , int threads ) const +{ + // [NOTE] This is structure is purely for determining adjacency and is independent of the FEM degree + typedef OctNode< TreeNodeData >::template ConstNeighborKey< 1 , 1 > ConstAdjacenctNodeKey; + if( offset<0 || offset>((size_t)1< span( _sliceStart[depth][ std::max< int >( 0 , offset-1 ) ] , _sliceStart[depth][ std::min< int >( (size_t)1<( sData.nodeCount * Square::CORNERS ); + sData._eMap = NewPointer< int >( sData.nodeCount * Square::EDGES ); + sData._fMap = NewPointer< int >( sData.nodeCount * Square::FACES ); + sData.cTable = NewPointer< typename SortedTreeNodes::SquareCornerIndices >( sData.nodeCount ); + sData.eTable = NewPointer< typename SortedTreeNodes::SquareCornerIndices >( sData.nodeCount ); + sData.fTable = NewPointer< typename SortedTreeNodes::SquareFaceIndices >( sData.nodeCount ); + memset( sData._cMap , 0 , sizeof(int) * sData.nodeCount * Square::CORNERS ); + memset( sData._eMap , 0 , sizeof(int) * sData.nodeCount * Square::EDGES ); + memset( sData._fMap , 0 , sizeof(int) * sData.nodeCount * Square::FACES ); + } + std::vector< ConstAdjacenctNodeKey > neighborKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i& neighbors = neighborKey.getNeighbors( node ); + int d , off[3]; + node->depthAndOffset( d , off ); + int z; + if ( off[2]==offset-1 ) z = 1; + else if( off[2]==offset ) z = 0; + else fprintf( stderr , "[ERROR] Node out of bounds: %d %d\n" , offset , off[2] ) , exit( 0 ); + // Process the corners + for( int x=0 ; x<2 ; x++ ) for( int y=0 ; y<2 ; y++ ) + { + int c = Cube::CornerIndex( x , y , z ); + int fc = Square::CornerIndex( x , y ); + bool cornerOwner = true; + int ac = Cube::AntipodalCornerIndex(c); // The index of the node relative to the corner + for( int cc=0 ; cc::template ConstNeighborKey< 1 , 1 > ConstAdjacenctNodeKey; + if( offset<0 || offset>=((size_t)1< span( _sliceStart[depth][offset] , _sliceStart[depth][offset+1] ); + sData.nodeOffset = span.first; + sData.nodeCount = span.second - span.first; + + DeletePointer( sData._eMap ) ; DeletePointer( sData._fMap ); + DeletePointer( sData.eTable ) ; DeletePointer( sData.fTable ); + if( sData.nodeCount ) + { + sData._eMap = NewPointer< int >( sData.nodeCount * Square::CORNERS ); + sData._fMap = NewPointer< int >( sData.nodeCount * Square::EDGES ); + sData.eTable = NewPointer< typename SortedTreeNodes::SquareCornerIndices >( sData.nodeCount ); + sData.fTable = NewPointer< typename SortedTreeNodes::SquareEdgeIndices >( sData.nodeCount ); + memset( sData._eMap , 0 , sizeof(int) * sData.nodeCount * Square::CORNERS ); + memset( sData._fMap , 0 , sizeof(int) * sData.nodeCount * Square::EDGES ); + } + + std::vector< ConstAdjacenctNodeKey > neighborKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i& neighbors = neighborKey.getNeighbors( node ); + int d , off[3]; + node->depthAndOffset( d , off ); + // Process the edges + int o=2; + for( int x=0 ; x<2 ; x++ ) for( int y=0 ; y<2 ; y++ ) + { + int fc = Square::CornerIndex( x , y ); + bool edgeOwner = true; + + int ac = Square::AntipodalCornerIndex( Square::CornerIndex( x , y ) ); + for( int cc=0 ; cc +double SystemCoefficients< Degree1 , Degree2 >::GetLaplacian( const typename FunctionIntegrator::Integrator& integrator , const int off1[] , const int off2[] ) +{ + double vv[] = { integrator.dot( off1[0] , off2[0] , false , false ) , integrator.dot( off1[1] , off2[1] , false , false ) , integrator.dot( off1[2] , off2[2] , false , false ) }; + double dd[] = { integrator.dot( off1[0] , off2[0] , true , true ) , integrator.dot( off1[1] , off2[1] , true , true ) , integrator.dot( off1[2] , off2[2] , true , true ) }; + return dd[0]*vv[1]*vv[2] + vv[0]*dd[1]*vv[2] + vv[0]*vv[1]*dd[2]; +} +template< int Degree1 , int Degree2 > +double SystemCoefficients< Degree1 , Degree2 >::GetLaplacian( const typename FunctionIntegrator::ChildIntegrator& integrator , const int off1[] , const int off2[] ) +{ + double vv[] = { integrator.dot( off1[0] , off2[0] , false , false ) , integrator.dot( off1[1] , off2[1] , false , false ) , integrator.dot( off1[2] , off2[2] , false , false ) }; + double dd[] = { integrator.dot( off1[0] , off2[0] , true , true ) , integrator.dot( off1[1] , off2[1] , true , true ) , integrator.dot( off1[2] , off2[2] , true , true ) }; + return dd[0]*vv[1]*vv[2] + vv[0]*dd[1]*vv[2] + vv[0]*vv[1]*dd[2]; +} +template< int Degree1 , int Degree2 > +double SystemCoefficients< Degree1 , Degree2 >::GetDivergence1( const typename FunctionIntegrator::Integrator& integrator , const int off1[] , const int off2[] , Point3D< double > normal1 ) +{ + return Point3D< double >::Dot( GetDivergence1( integrator , off1 , off2 ) , normal1 ); +} +template< int Degree1 , int Degree2 > +double SystemCoefficients< Degree1 , Degree2 >::GetDivergence1( const typename FunctionIntegrator::ChildIntegrator& integrator , const int off1[] , const int off2[] , Point3D< double > normal1 ) +{ + return Point3D< double >::Dot( GetDivergence1( integrator , off1 , off2 ) , normal1 ); +} +template< int Degree1 , int Degree2 > +double SystemCoefficients< Degree1 , Degree2 >::GetDivergence2( const typename FunctionIntegrator::Integrator& integrator , const int off1[] , const int off2[] , Point3D< double > normal2 ) +{ + return Point3D< double >::Dot( GetDivergence2( integrator , off1 , off2 ) , normal2 ); +} +template< int Degree1 , int Degree2 > +double SystemCoefficients< Degree1 , Degree2 >::GetDivergence2( const typename FunctionIntegrator::ChildIntegrator& integrator , const int off1[] , const int off2[] , Point3D< double > normal2 ) +{ + return Point3D< double >::Dot( GetDivergence2( integrator , off1 , off2 ) , normal2 ); +} +template< int Degree1 , int Degree2 > +Point3D< double > SystemCoefficients< Degree1 , Degree2 >::GetDivergence1( const typename FunctionIntegrator::Integrator& integrator , const int off1[] , const int off2[] ) +{ + double vv[] = { integrator.dot( off1[0] , off2[0] , false , false ) , integrator.dot( off1[1] , off2[1] , false , false ) , integrator.dot( off1[2] , off2[2] , false , false ) }; +#if GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the vector-field with the gradient of the basis function + double vd[] = { integrator.dot( off1[0] , off2[0] , true , false ) , integrator.dot( off1[1] , off2[1] , true , false ) , integrator.dot( off1[2] , off2[2] , true , false ) }; + return Point3D< double >( vd[0]*vv[1]*vv[2] , vv[0]*vd[1]*vv[2] , vv[0]*vv[1]*vd[2] ); +#else // !GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the divergence of the vector-field with the basis function + double dv[] = { integrator.dot( off1[0] , off2[0] , false , true ) , integrator.dot( off1[1] , off2[1] , false , true ) , integrator.dot( off1[2] , off2[2] , false , true ) }; + return -Point3D< double >( dv[0]*vv[1]*vv[2] , vv[0]*dv[1]*vv[2] , vv[0]*vv[1]*dv[2] ); +#endif // GRADIENT_DOMAIN_SOLUTION +} +template< int Degree1 , int Degree2 > +Point3D< double > SystemCoefficients< Degree1 , Degree2 >::GetDivergence1( const typename FunctionIntegrator::ChildIntegrator& integrator , const int off1[] , const int off2[] ) +{ + double vv[] = { integrator.dot( off1[0] , off2[0] , false , false ) , integrator.dot( off1[1] , off2[1] , false , false ) , integrator.dot( off1[2] , off2[2] , false , false ) }; +#if GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the vector-field with the gradient of the basis function + double vd[] = { integrator.dot( off1[0] , off2[0] , true , false ) , integrator.dot( off1[1] , off2[1] , true , false ) , integrator.dot( off1[2] , off2[2] , true , false ) }; + return Point3D< double >( vd[0]*vv[1]*vv[2] , vv[0]*vd[1]*vv[2] , vv[0]*vv[1]*vd[2] ); +#else // !GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the divergence of the vector-field with the basis function + double dv[] = { integrator.dot( off1[0] , off2[0] , false , true ) , integrator.dot( off1[1] , off2[1] , false , true ) , integrator.dot( off1[2] , off2[2] , false , true ) }; + return -Point3D< double >( dv[0]*vv[1]*vv[2] , vv[0]*dv[1]*vv[2] , vv[0]*vv[1]*dv[2] ); +#endif // GRADIENT_DOMAIN_SOLUTION +} +template< int Degree1 , int Degree2 > +Point3D< double > SystemCoefficients< Degree1 , Degree2 >::GetDivergence2( const typename FunctionIntegrator::Integrator& integrator , const int off1[] , const int off2[] ) +{ + double vv[] = { integrator.dot( off1[0] , off2[0] , false , false ) , integrator.dot( off1[1] , off2[1] , false , false ) , integrator.dot( off1[2] , off2[2] , false , false ) }; +#if GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the vector-field with the gradient of the basis function + double dv[] = { integrator.dot( off1[0] , off2[0] , false , true ) , integrator.dot( off1[1] , off2[1] , false , true ) , integrator.dot( off1[2] , off2[2] , false , true ) }; + return Point3D< double >( dv[0]*vv[1]*vv[2] , vv[0]*dv[1]*vv[2] , vv[0]*vv[1]*dv[2] ); +#else // !GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the divergence of the vector-field with the basis function + double vd[] = { integrator.dot( off1[0] , off2[0] , true , false ) , integrator.dot( off1[1] , off2[1] , true , false ) , integrator.dot( off1[2] , off2[2] , true , false ) }; + return -Point3D< double >( vd[0]*vv[1]*vv[2] , vv[0]*vd[1]*vv[2] , vv[0]*vv[1]*vd[2] ); +#endif // GRADIENT_DOMAIN_SOLUTION +} +template< int Degree1 , int Degree2 > +Point3D< double > SystemCoefficients< Degree1 , Degree2 >::GetDivergence2( const typename FunctionIntegrator::ChildIntegrator& integrator , const int off1[] , const int off2[] ) +{ + double vv[] = { integrator.dot( off1[0] , off2[0] , false , false ) , integrator.dot( off1[1] , off2[1] , false , false ) , integrator.dot( off1[2] , off2[2] , false , false ) }; +#if GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the vector-field with the gradient of the basis function + double dv[] = { integrator.dot( off1[0] , off2[0] , false , true ) , integrator.dot( off1[1] , off2[1] , false , true ) , integrator.dot( off1[2] , off2[2] , false , true ) }; + return Point3D< double >( dv[0]*vv[1]*vv[2] , vv[0]*dv[1]*vv[2] , vv[0]*vv[1]*dv[2] ); +#else // !GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the divergence of the vector-field with the basis function + double vd[] = { integrator.dot( off1[0] , off2[0] , true , false ) , integrator.dot( off1[1] , off2[1] , true , false ) , integrator.dot( off1[2] , off2[2] , true , false ) }; + return -Point3D< double >( vd[0]*vv[1]*vv[2] , vv[0]*vd[1]*vv[2] , vv[0]*vv[1]*vd[2] ); +#endif // GRADIENT_DOMAIN_SOLUTION +} +// if( scatter ) normals come from the center node +// else normals come from the neighbors +template< int Degree1 , int Degree2 > +void SystemCoefficients< Degree1 , Degree2 >::SetCentralDivergenceStencil( const typename FunctionIntegrator::Integrator& integrator , Stencil< Point3D< double > , OverlapSize >& stencil , bool scatter ) +{ + int center = ( 1<>1; + int offset[] = { center , center , center }; + for( int x=0 ; x +void SystemCoefficients< Degree1 , Degree2 >::SetCentralDivergenceStencils( const typename FunctionIntegrator::ChildIntegrator& integrator , Stencil< Point3D< double > , OverlapSize > stencils[2][2][2] , bool scatter ) +{ + int center = ( 1<>1; + for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ ) + { + int offset[] = { center+i , center+j , center+k }; + for( int x=0 ; x +void SystemCoefficients< Degree1 , Degree2 >::SetCentralLaplacianStencil( const typename FunctionIntegrator::Integrator& integrator , Stencil< double , OverlapSize >& stencil ) +{ + int center = ( 1<>1; + int offset[] = { center , center , center }; + for( int x=0 ; x +void SystemCoefficients< Degree1 , Degree2 >::SetCentralLaplacianStencils( const typename FunctionIntegrator::ChildIntegrator& integrator , Stencil< double , OverlapSize > stencils[2][2][2] ) +{ + int center = ( 1<>1; + for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ ) + { + int offset[] = { center+i , center+j , center+k }; + for( int x=0 ; x +template< int FEMDegree > +void Octree< Real >::_setMultiColorIndices( int start , int end , std::vector< std::vector< int > >& indices ) const +{ + static const int OverlapRadius = - BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapStart; + + const int modulus = OverlapRadius+1; + indices.resize( modulus*modulus*modulus ); + int count[modulus*modulus*modulus]; + memset( count , 0 , sizeof(int)*modulus*modulus*modulus ); +#pragma omp parallel for num_threads( threads ) + for( int i=start ; i( _sNodes.treeNodes[i] ) ) + { + int d , off[3]; + _sNodes.treeNodes[i]->depthAndOffset( d , off ); + int idx = (modulus*modulus) * ( off[2]%modulus ) + modulus * ( off[1]%modulus ) + ( off[0]%modulus ); +#pragma omp atomic + count[idx]++; + } + + for( int i=0 ; i( _sNodes.treeNodes[i] ) ) + { + int d , off[3]; + _sNodes.treeNodes[i]->depthAndOffset( d , off ); + int idx = (modulus*modulus) * ( off[2]%modulus ) + modulus * ( off[1]%modulus ) + ( off[0]%modulus ); + indices[idx].push_back( i - start ); + } +} + +template< class Real > +template< class C , int FEMDegree > +void Octree< Real >::_DownSample( int highDepth , DenseNodeData< C , FEMDegree >& constraints ) const +{ + typedef typename TreeOctNode::NeighborKey< -BSplineEvaluationData< FEMDegree >::UpSampleStart , BSplineEvaluationData< FEMDegree >::UpSampleEnd > UpSampleKey; + + int lowDepth = highDepth-1; + if( lowDepth<_minDepth ) return; + + typename BSplineEvaluationData< FEMDegree >::UpSampleEvaluator upSampleEvaluator; + BSplineEvaluationData< FEMDegree >::SetUpSampleEvaluator( upSampleEvaluator , lowDepth-1 , _dirichlet ); + std::vector< UpSampleKey > neighborKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i::UpSampleSize > upSampleStencil; + int lowCenter = _Dimension< FEMDegree >(lowDepth)>>1; + for( int i=0 ; i::UpSampleSize ; i++ ) for( int j=0 ; j::UpSampleSize ; j++ ) for( int k=0 ; k::UpSampleSize ; k++ ) + upSampleStencil.values[i][j][k] = + upSampleEvaluator.value( lowCenter , 2*lowCenter + i + BSplineEvaluationData< FEMDegree >::UpSampleStart ) * + upSampleEvaluator.value( lowCenter , 2*lowCenter + j + BSplineEvaluationData< FEMDegree >::UpSampleStart ) * + upSampleEvaluator.value( lowCenter , 2*lowCenter + k + BSplineEvaluationData< FEMDegree >::UpSampleStart ); + int dim = _Dimension< FEMDegree >(lowDepth); + + // Iterate over all (valid) parent nodes +#pragma omp parallel for num_threads( threads ) + for( int i=_sNodes.begin(lowDepth) ; i<_sNodes.end(lowDepth) ; i++ ) if( _IsValidNode< FEMDegree >( _sNodes.treeNodes[i] ) ) + { + TreeOctNode* pNode = _sNodes.treeNodes[i]; + + UpSampleKey& neighborKey = neighborKeys[ omp_get_thread_num() ]; + int d , off[3]; + pNode->depthAndOffset( d , off ); + + neighborKey.template getNeighbors< false >( pNode ); + + // Get the child neighbors + typename TreeOctNode::Neighbors< BSplineEvaluationData< FEMDegree >::UpSampleSize > neighbors; + neighborKey.template getChildNeighbors< false >( 0 , d , neighbors ); + + C& coarseConstraint = constraints[i]; + + // Want to make sure test if contained children are interior. + // This is more conservative because we are test that overlapping children are interior + bool isInterior = _IsInteriorlyOverlapped< FEMDegree , FEMDegree >( pNode ); + if( isInterior ) + { + for( int ii=0 ; ii::UpSampleSize ; ii++ ) for( int jj=0 ; jj::UpSampleSize ; jj++ ) for( int kk=0 ; kk::UpSampleSize ; kk++ ) + { + const TreeOctNode* cNode = neighbors.neighbors[ii][jj][kk]; + if( cNode ) coarseConstraint += (C)( constraints[ cNode->nodeData.nodeIndex ] * upSampleStencil.values[ii][jj][kk] ); + } + } + else + { + double upSampleValues[3][ BSplineEvaluationData< FEMDegree >::UpSampleSize ]; + for( int ii=0 ; ii::UpSampleSize ; ii++ ) + { + upSampleValues[0][ii] = upSampleEvaluator.value( off[0] , 2*off[0] + ii + BSplineEvaluationData< FEMDegree >::UpSampleStart ); + upSampleValues[1][ii] = upSampleEvaluator.value( off[1] , 2*off[1] + ii + BSplineEvaluationData< FEMDegree >::UpSampleStart ); + upSampleValues[2][ii] = upSampleEvaluator.value( off[2] , 2*off[2] + ii + BSplineEvaluationData< FEMDegree >::UpSampleStart ); + } + + for( int ii=0 ; ii::UpSampleSize ; ii++ ) for( int jj=0 ; jj::UpSampleSize ; jj++ ) + { + double dxy = upSampleValues[0][ii] * upSampleValues[1][jj]; + for( int kk=0 ; kk::UpSampleSize ; kk++ ) + { + const TreeOctNode* cNode = neighbors.neighbors[ii][jj][kk]; + if( _IsValidNode< FEMDegree >( cNode ) ) coarseConstraint += (C)( constraints[ cNode->nodeData.nodeIndex ] * dxy * upSampleValues[2][kk] ); + } + } + } + } +} +template< class Real > +template< class C , int FEMDegree> +void Octree< Real >::_UpSample( int highDepth , DenseNodeData< C , FEMDegree >& coefficients ) const +{ + static const int LeftDownSampleRadius = -( ( BSplineEvaluationData< FEMDegree >::DownSample0Start < BSplineEvaluationData< FEMDegree >::DownSample1Start ) ? BSplineEvaluationData< FEMDegree >::DownSample0Start : BSplineEvaluationData< FEMDegree >::DownSample1Start ); + static const int RightDownSampleRadius = ( ( BSplineEvaluationData< FEMDegree >::DownSample0End > BSplineEvaluationData< FEMDegree >::DownSample1End ) ? BSplineEvaluationData< FEMDegree >::DownSample0End : BSplineEvaluationData< FEMDegree >::DownSample1End ); + typedef TreeOctNode::NeighborKey< LeftDownSampleRadius , RightDownSampleRadius > DownSampleKey; + + int lowDepth = highDepth-1; + if( lowDepth<_minDepth ) return; + + typename BSplineEvaluationData< FEMDegree >::UpSampleEvaluator upSampleEvaluator; + BSplineEvaluationData< FEMDegree >::SetUpSampleEvaluator( upSampleEvaluator , lowDepth-1 , _dirichlet ); + std::vector< DownSampleKey > neighborKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i::DownSample0Size > BSplineEvaluationData< FEMDegree >::DownSample1Size ? BSplineEvaluationData< FEMDegree >::DownSample0Size : BSplineEvaluationData< FEMDegree >::DownSample1Size; + Stencil< double , DownSampleSize > downSampleStencils[ Cube::CORNERS ]; + int lowCenter = _Dimension< FEMDegree >( lowDepth )>>1; + for( int c=0 ; c::DownSampleSize[cx] ; ii++ ) + for( int jj=0 ; jj::DownSampleSize[cy] ; jj++ ) + for( int kk=0 ; kk::DownSampleSize[cz] ; kk++ ) + downSampleStencils[c].values[ii][jj][kk] = + upSampleEvaluator.value( lowCenter + ii + BSplineEvaluationData< FEMDegree >::DownSampleStart[cx] , 2*lowCenter + cx ) * + upSampleEvaluator.value( lowCenter + jj + BSplineEvaluationData< FEMDegree >::DownSampleStart[cy] , 2*lowCenter + cy ) * + upSampleEvaluator.value( lowCenter + kk + BSplineEvaluationData< FEMDegree >::DownSampleStart[cz] , 2*lowCenter + cz ) ; + } + int dim = _Dimension< FEMDegree >( lowDepth ); + + // For Dirichlet constraints, can't get to all children from parents because boundary nodes are invalid +#pragma omp parallel for num_threads( threads ) + for( int i=_sNodes.begin(highDepth) ; i<_sNodes.end(highDepth) ; i++ ) if( _IsValidNode< FEMDegree >( _sNodes.treeNodes[i] ) ) + { + TreeOctNode *cNode = _sNodes.treeNodes[i] , *pNode = cNode->parent; + int c = (int)( cNode-pNode->children ); + + DownSampleKey& neighborKey = neighborKeys[ omp_get_thread_num() ]; + int d , off[3]; + pNode->depthAndOffset( d , off ); + typename TreeOctNode::Neighbors< LeftDownSampleRadius + RightDownSampleRadius + 1 >& neighbors = neighborKey.template getNeighbors< false >( pNode ); + + // Want to make sure test if contained children are interior. + // This is more conservative because we are test that overlapping children are interior + bool isInterior = _IsInteriorlyOverlapped< FEMDegree , FEMDegree >( pNode ); + + C& fineCoefficient = coefficients[ cNode->nodeData.nodeIndex ]; + + int cx , cy , cz; + Cube::FactorCornerIndex( c , cx , cy , cz ); + + if( isInterior ) + { + for( int ii=0 ; ii::DownSampleSize[cx] ; ii++ ) for( int jj=0 ; jj::DownSampleSize[cy] ; jj++ ) + { + int _ii = ii + BSplineEvaluationData< FEMDegree >::DownSampleStart[cx] + LeftDownSampleRadius; + int _jj = jj + BSplineEvaluationData< FEMDegree >::DownSampleStart[cy] + LeftDownSampleRadius; + for( int kk=0 ; kk::DownSampleSize[cz] ; kk++ ) + { + int _kk = kk + BSplineEvaluationData< FEMDegree >::DownSampleStart[cz] + LeftDownSampleRadius; + const TreeOctNode* _pNode = neighbors.neighbors[_ii][_jj][_kk]; + if( _pNode ) fineCoefficient += (C)( coefficients[ _pNode->nodeData.nodeIndex ] * downSampleStencils[c].values[ii][jj][kk] ); + } + } + } + else + { + double downSampleValues[3][ BSplineEvaluationData< FEMDegree >::DownSample0Size > BSplineEvaluationData< FEMDegree >::DownSample1Size ? BSplineEvaluationData< FEMDegree >::DownSample0Size : BSplineEvaluationData< FEMDegree >::DownSample1Size ]; + + for( int ii=0 ; ii::DownSampleSize[cx] ; ii++ ) downSampleValues[0][ii] = upSampleEvaluator.value( off[0] + ii + BSplineEvaluationData< FEMDegree >::DownSampleStart[cx] , 2*off[0] + cx ); + for( int ii=0 ; ii::DownSampleSize[cy] ; ii++ ) downSampleValues[1][ii] = upSampleEvaluator.value( off[1] + ii + BSplineEvaluationData< FEMDegree >::DownSampleStart[cy] , 2*off[1] + cy ); + for( int ii=0 ; ii::DownSampleSize[cz] ; ii++ ) downSampleValues[2][ii] = upSampleEvaluator.value( off[2] + ii + BSplineEvaluationData< FEMDegree >::DownSampleStart[cz] , 2*off[2] + cz ); + + for( int ii=0 ; ii::DownSampleSize[cx] ; ii++ ) for( int jj=0 ; jj::DownSampleSize[cy] ; jj++ ) + { + double dxy = downSampleValues[0][ii] * downSampleValues[1][jj]; + int _ii = ii + BSplineEvaluationData< FEMDegree >::DownSampleStart[cx] + LeftDownSampleRadius; + int _jj = jj + BSplineEvaluationData< FEMDegree >::DownSampleStart[cy] + LeftDownSampleRadius; + for( int kk=0 ; kk::DownSampleSize[cz] ; kk++ ) + { + int _kk = kk + BSplineEvaluationData< FEMDegree >::DownSampleStart[cz] + LeftDownSampleRadius; + const TreeOctNode* _pNode = neighbors.neighbors[_ii][_jj][_kk]; + if( _IsValidNode< FEMDegree >( _pNode ) ) fineCoefficient += (C)( coefficients[ _pNode->nodeData.nodeIndex ] * dxy * downSampleValues[2][kk] ); + } + } + } + } +} + +template< class Real > +template< class C , int FEMDegree > +void Octree< Real >::_UpSample( int highDepth , ConstPointer( C ) lowCoefficients , Pointer( C ) highCoefficients , bool dirichlet , int threads ) +{ + static const int LeftDownSampleRadius = -( ( BSplineEvaluationData< FEMDegree >::DownSample0Start < BSplineEvaluationData< FEMDegree >::DownSample1Start ) ? BSplineEvaluationData< FEMDegree >::DownSample0Start : BSplineEvaluationData< FEMDegree >::DownSample1Start ); + static const int RightDownSampleRadius = ( ( BSplineEvaluationData< FEMDegree >::DownSample0End > BSplineEvaluationData< FEMDegree >::DownSample1End ) ? BSplineEvaluationData< FEMDegree >::DownSample0End : BSplineEvaluationData< FEMDegree >::DownSample1End ); + typedef TreeOctNode::NeighborKey< LeftDownSampleRadius , RightDownSampleRadius > DownSampleKey; + + int lowDepth = highDepth-1; + if( lowDepth<1 ) return; + + typename BSplineEvaluationData< FEMDegree >::UpSampleEvaluator upSampleEvaluator; + BSplineEvaluationData< FEMDegree >::SetUpSampleEvaluator( upSampleEvaluator , lowDepth-1 , dirichlet ); + std::vector< DownSampleKey > neighborKeys( std::max< int >( 1 , threads ) ); + + static const int DownSampleSize = BSplineEvaluationData< FEMDegree >::DownSample0Size > BSplineEvaluationData< FEMDegree >::DownSample1Size ? BSplineEvaluationData< FEMDegree >::DownSample0Size : BSplineEvaluationData< FEMDegree >::DownSample1Size; + Stencil< double , DownSampleSize > downSampleStencils[ Cube::CORNERS ]; + int lowCenter = _Dimension< FEMDegree >( lowDepth )>>1; + for( int c=0 ; c::DownSampleSize[cx] ; ii++ ) + for( int jj=0 ; jj::DownSampleSize[cy] ; jj++ ) + for( int kk=0 ; kk::DownSampleSize[cz] ; kk++ ) + downSampleStencils[c].values[ii][jj][kk] = + upSampleEvaluator.value( lowCenter + ii + BSplineEvaluationData< FEMDegree >::DownSampleStart[cx] , 2*lowCenter + cx ) * + upSampleEvaluator.value( lowCenter + jj + BSplineEvaluationData< FEMDegree >::DownSampleStart[cy] , 2*lowCenter + cy ) * + upSampleEvaluator.value( lowCenter + kk + BSplineEvaluationData< FEMDegree >::DownSampleStart[cz] , 2*lowCenter + cz ) ; + } + int lowDim = _Dimension< FEMDegree >( lowDepth ) , highDim = _Dimension< FEMDegree >( highDepth ); + + // Iterate over all parent nodes +#pragma omp parallel for num_threads( threads ) + for( int k=0 ; k( lowDepth , i , j , k ); + + // Iterate over all the children of the parent + for( int c=0 ; c=highDim || jj<0 || jj>=highDim || kk<0 || kk>=highDim ) continue; + + C& highCoefficient = highCoefficients[ ii + jj*highDim + kk*highDim*highDim ]; + + if( isInterior ) + { + for( int ii=0 ; ii::DownSampleSize[cx] ; ii++ ) for( int jj=0 ; jj::DownSampleSize[cy] ; jj++ ) + { + int _i = i + ii + BSplineEvaluationData< FEMDegree >::DownSampleStart[cx]; + int _j = j + jj + BSplineEvaluationData< FEMDegree >::DownSampleStart[cy]; + for( int kk=0 ; kk::DownSampleSize[cz] ; kk++ ) + { + int _k = k + kk + BSplineEvaluationData< FEMDegree >::DownSampleStart[cz]; + highCoefficient += (C)( lowCoefficients[ _i + _j*lowDim + _k*lowDim*lowDim ] * downSampleStencils[c].values[ii][jj][kk] ); + } + } + } + else + { + double downSampleValues[3][ BSplineEvaluationData< FEMDegree >::DownSample0Size > BSplineEvaluationData< FEMDegree >::DownSample1Size ? BSplineEvaluationData< FEMDegree >::DownSample0Size : BSplineEvaluationData< FEMDegree >::DownSample1Size ]; + + for( int ii=0 ; ii::DownSampleSize[cx] ; ii++ ) downSampleValues[0][ii] = upSampleEvaluator.value( off[0] + ii + BSplineEvaluationData< FEMDegree >::DownSampleStart[cx] , 2*off[0] + cx ); + for( int ii=0 ; ii::DownSampleSize[cy] ; ii++ ) downSampleValues[1][ii] = upSampleEvaluator.value( off[1] + ii + BSplineEvaluationData< FEMDegree >::DownSampleStart[cy] , 2*off[1] + cy ); + for( int ii=0 ; ii::DownSampleSize[cz] ; ii++ ) downSampleValues[2][ii] = upSampleEvaluator.value( off[2] + ii + BSplineEvaluationData< FEMDegree >::DownSampleStart[cz] , 2*off[2] + cz ); + + for( int ii=0 ; ii::DownSampleSize[cx] ; ii++ ) for( int jj=0 ; jj::DownSampleSize[cy] ; jj++ ) + { + double dxy = downSampleValues[0][ii] * downSampleValues[1][jj]; + int _i = i + ii + BSplineEvaluationData< FEMDegree >::DownSampleStart[cx]; + int _j = j + jj + BSplineEvaluationData< FEMDegree >::DownSampleStart[cy]; + if( _i>=0 && _i=0 && _j::DownSampleSize[cz] ; kk++ ) + { + int _k = k + kk + BSplineEvaluationData< FEMDegree >::DownSampleStart[cz]; + if( _k>=0 && _k +template< int FEMDegree > +Real Octree< Real >::_CoarserFunctionValue( Point3D< Real > p , const PointSupportKey< FEMDegree >& neighborKey , const TreeOctNode* pointNode , const BSplineData< FEMDegree >& bsData , const DenseNodeData< Real , FEMDegree >& upSampledCoefficients ) const +{ + static const int SupportSize = BSplineEvaluationData< FEMDegree >::SupportSize; + static const int LeftSupportRadius = - BSplineEvaluationData< FEMDegree >::SupportStart; + static const int RightSupportRadius = BSplineEvaluationData< FEMDegree >::SupportEnd; + static const int LeftPointSupportRadius = BSplineEvaluationData< FEMDegree >::SupportEnd; + static const int RightPointSupportRadius = - BSplineEvaluationData< FEMDegree >::SupportStart; + + double pointValue = 0; + int depth = pointNode->depth(); + if( depth<=_minDepth ) return Real(0.); + + // Iterate over all basis functions that overlap the point at the coarser resolution + { + const typename TreeOctNode::Neighbors< SupportSize >& neighbors = neighborKey.neighbors[depth-1]; + int _d , _off[3]; + pointNode->parent->depthAndOffset( _d , _off ); + int fStart , fEnd; + BSplineData< FEMDegree >::FunctionSpan( _d-1 , fStart , fEnd ); + + double pointValues[ DIMENSION ][SupportSize]; + memset( pointValues , 0 , sizeof(double) * DIMENSION * SupportSize ); + + for( int dd=0 ; dd::FunctionIndex( _d-1 , _off[dd]+i ); + if( fIdx>=fStart && fIdx( _node ) ) _pointValue += pointValues[2][l] * double( upSampledCoefficients[_node->nodeData.nodeIndex] ); + } + pointValue += _pointValue * xyValue; + } + } + return Real( pointValue ); +} + +template< class Real > +template< int FEMDegree > +Real Octree< Real >::_FinerFunctionValue( Point3D< Real > p , const PointSupportKey< FEMDegree >& neighborKey , const TreeOctNode* pointNode , const BSplineData< FEMDegree >& bsData , const DenseNodeData< Real , FEMDegree >& finerCoefficients ) const +{ + typename TreeOctNode::Neighbors< BSplineEvaluationData< FEMDegree >::SupportSize > childNeighbors; + static const int LeftPointSupportRadius = BSplineEvaluationData< FEMDegree >::SupportEnd; + static const int RightPointSupportRadius = -BSplineEvaluationData< FEMDegree >::SupportStart; + static const int LeftSupportRadius = -BSplineEvaluationData< FEMDegree >::SupportStart; + static const int RightSupportRadius = BSplineEvaluationData< FEMDegree >::SupportEnd; + + double pointValue = 0; + int depth = pointNode->depth(); + neighborKey.template getChildNeighbors< false >( p , depth , childNeighbors ); + for( int j=-LeftPointSupportRadius ; j<=RightPointSupportRadius ; j++ ) + for( int k=-LeftPointSupportRadius ; k<=RightPointSupportRadius ; k++ ) + for( int l=-LeftPointSupportRadius ; l<=RightPointSupportRadius ; l++ ) + { + const TreeOctNode* _node = childNeighbors.neighbors[j+LeftPointSupportRadius][k+LeftPointSupportRadius][l+LeftPointSupportRadius]; + if( _IsValidNode< FEMDegree >( _node ) ) + { + int fIdx[3]; + FunctionIndex< FEMDegree >( _node , fIdx ); + pointValue += + bsData.baseBSplines[ fIdx[0] ][LeftSupportRadius-j]( p[0] ) * + bsData.baseBSplines[ fIdx[1] ][LeftSupportRadius-k]( p[1] ) * + bsData.baseBSplines[ fIdx[2] ][LeftSupportRadius-l]( p[2] ) * + double( finerCoefficients[ _node->nodeData.nodeIndex ] ); + } + } + return Real( pointValue ); +} + +template< class Real > +template< int FEMDegree > +void Octree< Real >::_SetPointValuesFromCoarser( SparseNodeData< PointData< Real > , 0 >& pointInfo , int highDepth , const BSplineData< FEMDegree >& bsData , const DenseNodeData< Real , FEMDegree >& upSampledCoefficients ) +{ + int lowDepth = highDepth-1; + if( lowDepth<_minDepth ) return; + std::vector< PointData< Real > >& points = pointInfo.data; + std::vector< PointSupportKey< FEMDegree > > neighborKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i( _sNodes.treeNodes[i] ) ) + { + PointSupportKey< FEMDegree >& neighborKey = neighborKeys[ omp_get_thread_num() ]; + int pIdx = pointInfo.index( _sNodes.treeNodes[i] ); + if( pIdx!=-1 ) + { + neighborKey.template getNeighbors< false >( _sNodes.treeNodes[i]->parent ); + points[ pIdx ].weightedCoarserDValue = (Real)( _CoarserFunctionValue( points[pIdx].position , neighborKey , _sNodes.treeNodes[i] , bsData , upSampledCoefficients ) - 0.5 ) * points[pIdx].weight; + } + } +} + +template< class Real > +template< int FEMDegree > +void Octree< Real >::_SetPointConstraintsFromFiner( const SparseNodeData< PointData< Real > , 0 >& pointInfo , int highDepth , const BSplineData< FEMDegree >& bsData , const DenseNodeData< Real , FEMDegree >& finerCoefficients , DenseNodeData< Real , FEMDegree >& coarserConstraints ) const +{ + static const int SupportSize = BSplineEvaluationData< FEMDegree >::SupportSize; + static const int LeftPointSupportRadius = BSplineEvaluationData< FEMDegree >::SupportEnd; + static const int RightPointSupportRadius = -BSplineEvaluationData< FEMDegree >::SupportStart; + static const int LeftSupportRadius = -BSplineEvaluationData< FEMDegree >::SupportStart; + static const int RightSupportRadius = BSplineEvaluationData< FEMDegree >::SupportEnd; + + const std::vector< PointData< Real > >& points = pointInfo.data; + // Note: We can't iterate over the finer point nodes as the point weights might be + // scaled incorrectly, due to the adaptive exponent. So instead, we will iterate + // over the coarser nodes and evaluate the finer solution at the associated points. + int lowDepth = highDepth-1; + if( lowDepth<_minDepth ) return; + size_t start = _sNodes.begin(lowDepth) , end = _sNodes.end(lowDepth) , range = end-start; + memset( coarserConstraints.data+start , 0 , sizeof( Real ) * range ); + std::vector< PointSupportKey< FEMDegree > > neighborKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i( _sNodes.treeNodes[i] ) ) + { + PointSupportKey< FEMDegree >& neighborKey = neighborKeys[ omp_get_thread_num() ]; + int pIdx = pointInfo.index( _sNodes.treeNodes[i] ); + if( pIdx!=-1 ) + { + typename TreeOctNode::Neighbors< SupportSize >& neighbors = neighborKey.template getNeighbors< false >( _sNodes.treeNodes[i] ); + // Evaluate the solution @( depth ) at the current point @( depth-1 ) + { + Real finerPointDValue = (Real)( _FinerFunctionValue( points[pIdx].position , neighborKey , _sNodes.treeNodes[i] , bsData , finerCoefficients ) - 0.5 ) * points[pIdx].weight; + Point3D< Real > p = points[ pIdx ].position; + // Update constraints for all nodes @( depth-1 ) that overlap the point + int d , idx[3]; + neighbors.neighbors[LeftPointSupportRadius][LeftPointSupportRadius][LeftPointSupportRadius]->depthAndOffset( d, idx ); + // Set the (offset) index to the top-left-front corner of the 3x3x3 block of b-splines + // overlapping the point. + idx[0] = BinaryNode::CenterIndex( d , idx[0] ); + idx[1] = BinaryNode::CenterIndex( d , idx[1] ); + idx[2] = BinaryNode::CenterIndex( d , idx[2] ); + for( int x=-LeftPointSupportRadius ; x<=RightPointSupportRadius ; x++ ) + for( int y=-LeftPointSupportRadius ; y<=RightPointSupportRadius ; y++ ) + for( int z=-LeftPointSupportRadius ; z<=RightPointSupportRadius ; z++ ) + if( _IsValidNode< FEMDegree >( neighbors.neighbors[x+LeftPointSupportRadius][y+LeftPointSupportRadius][z+LeftPointSupportRadius] ) ) + { +#pragma omp atomic + coarserConstraints[ neighbors.neighbors[x+LeftPointSupportRadius][y+LeftPointSupportRadius][z+LeftPointSupportRadius]->nodeData.nodeIndex - _sNodes.begin(lowDepth) ] += + Real( + bsData.baseBSplines[idx[0]+x][LeftSupportRadius-x]( p[0] ) * + bsData.baseBSplines[idx[1]+y][LeftSupportRadius-y]( p[1] ) * + bsData.baseBSplines[idx[2]+z][LeftSupportRadius-z]( p[2] ) * + finerPointDValue + ); + } + } + } + } +} + +template< class Real > +template< int FEMDegree > +int Octree< Real >::_SetMatrixRow( const SparseNodeData< PointData< Real > , 0 >& pointInfo , const typename TreeOctNode::Neighbors< BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize >& neighbors , Pointer( MatrixEntry< Real > ) row , int offset , const typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::Integrator& integrator , const Stencil< double , BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize >& stencil , const BSplineData< FEMDegree >& bsData ) const +{ + static const int SupportSize = BSplineEvaluationData< FEMDegree >::SupportSize; + static const int OverlapRadius = - BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapStart; + static const int OverlapSize = BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize; + static const int LeftSupportRadius = -BSplineEvaluationData< FEMDegree >::SupportStart; + static const int RightSupportRadius = BSplineEvaluationData< FEMDegree >::SupportEnd; + static const int LeftPointSupportRadius = BSplineEvaluationData< FEMDegree >::SupportEnd; + static const int RightPointSupportRadius = -BSplineEvaluationData< FEMDegree >::SupportStart; + + const std::vector< PointData< Real > >& points = pointInfo.data; + bool hasYZPoints[SupportSize] , hasZPoints[SupportSize][SupportSize]; + Real diagonal = 0; + // Given a node: + // -- for each node in its support: + // ---- if the supporting node contains a point: + // ------ evaluate the x, y, and z B-splines of the nodes supporting the point + // splineValues \in [-LeftSupportRadius,RightSupportRadius] x [-LeftSupportRadius,RightSupportRadius] x [-LeftSupportRadius,RightSupportRadius] x [0,Dimension) x [-LeftPointSupportRadius,RightPointSupportRadius] + Real splineValues[SupportSize][SupportSize][SupportSize][DIMENSION][SupportSize]; + memset( splineValues , 0 , sizeof( Real ) * SupportSize * SupportSize * SupportSize * DIMENSION *SupportSize ); + + int count = 0; + const TreeOctNode* node = neighbors.neighbors[OverlapRadius][OverlapRadius][OverlapRadius]; + int d , off[3]; + node->depthAndOffset( d , off ); + int fStart , fEnd; + BSplineData< FEMDegree >::FunctionSpan( d-1 , fStart , fEnd ); + bool isInterior = _IsInteriorlyOverlapped< FEMDegree , FEMDegree >( node ); + + if( _constrainValues ) + { + // Iterate over all neighboring nodes that may have a constraining point + // -- For each one, compute the values of the spline functions supported on the point + for( int j=0 ; j( _node ) && pointInfo.index( _node )!=-1 ) + { + int pOff[] = { off[0]+j , off[1]+k , off[2]+l }; + hasYZPoints[jj] = hasZPoints[jj][kk] = true; + const PointData< Real >& pData = points[ pointInfo.index( _node ) ]; + Real (*_splineValues)[SupportSize] = splineValues[jj][kk][ll]; + Real weight = pData.weight; + Point3D< Real > p = pData.position; + // Evaluate the point p at all the nodes whose functions have it in their support + for( int s=-LeftPointSupportRadius ; s<=RightPointSupportRadius ; s++ ) for( int dd=0 ; dd::FunctionIndex( d-1 , pOff[dd]+s ); + if( fIdx>=fStart && fIdx( _node ) && pointInfo.index( _node )!=-1 ) + // Iterate over all neighbors whose support contains the point and accumulate the mutual integral + for( int ii=-LeftPointSupportRadius ; ii<=RightPointSupportRadius ; ii++ ) + for( int jj=-LeftPointSupportRadius ; jj<=RightPointSupportRadius ; jj++ ) + for( int kk=-LeftPointSupportRadius ; kk<=RightPointSupportRadius ; kk++ ) + { + TreeOctNode* _node = neighbors.neighbors[i+ii+OverlapRadius][j+jj+OverlapRadius][k+kk+OverlapRadius]; + if( _IsValidNode< FEMDegree >( _node ) ) + pointValues[i+ii+OverlapRadius][j+jj+OverlapRadius][k+kk+OverlapRadius] += + _splineValues[0][ii+LeftPointSupportRadius ] * _splineValues[1][jj+LeftPointSupportRadius ] * _splineValues[2][kk+LeftPointSupportRadius ]; + } + } + } + pointValues[OverlapRadius][OverlapRadius][OverlapRadius] = diagonal; + int nodeIndex = neighbors.neighbors[OverlapRadius][OverlapRadius][OverlapRadius]->nodeData.nodeIndex; + if( isInterior ) // General case, so try to make fast + { + const TreeOctNode* const * _nodes = &neighbors.neighbors[0][0][0]; + const double* _stencil = &stencil.values[0][0][0]; + Real* _values = &pointValues[0][0][0]; + const static int CenterIndex = OverlapSize*OverlapSize*OverlapRadius + OverlapSize*OverlapRadius + OverlapRadius; + if( _constrainValues ) for( int i=0 ; i( nodeIndex-offset , _values[CenterIndex] ); + for( int i=0 ; i( _nodes[i]->nodeData.nodeIndex-offset , _values[i] ); + } + else + { + int d , off[3]; + node->depthAndOffset( d , off ); + Real temp = Real( SystemCoefficients< FEMDegree , FEMDegree >::GetLaplacian( integrator , off , off ) ); + if( _constrainValues ) temp += pointValues[OverlapRadius][OverlapRadius][OverlapRadius]; + row[count++] = MatrixEntry< Real >( nodeIndex-offset , temp ); + for( int x=0 ; x( neighbors.neighbors[x][y][z] ) ) + { + const TreeOctNode* _node = neighbors.neighbors[x][y][z]; + int _d , _off[3]; + _node->depthAndOffset( _d , _off ); + Real temp = Real( SystemCoefficients< FEMDegree , FEMDegree >::GetLaplacian( integrator , _off , off ) ); + if( _constrainValues ) temp += pointValues[x][y][z]; + row[count++] = MatrixEntry< Real >( _node->nodeData.nodeIndex-offset , temp ); + } + } + return count; +} + +template< class Real > +template< int FEMDegree > +int Octree< Real >::_GetMatrixAndUpdateConstraints( const SparseNodeData< PointData< Real > , 0 >& pointInfo , SparseMatrix< Real >& matrix , DenseNodeData< Real , FEMDegree >& constraints , typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::Integrator& integrator , typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::ChildIntegrator& childIntegrator , const BSplineData< FEMDegree >& bsData , int depth , const DenseNodeData< Real , FEMDegree >* metSolution , bool coarseToFine ) +{ + static const int OverlapRadius = - BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapStart; + static const int OverlapSize = BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize; + + size_t start = _sNodes.begin(depth) , end = _sNodes.end(depth) , range = end-start; + Stencil< double , OverlapSize > stencil , stencils[2][2][2]; + SystemCoefficients< FEMDegree , FEMDegree >::SetCentralLaplacianStencil ( integrator , stencil ); + SystemCoefficients< FEMDegree , FEMDegree >::SetCentralLaplacianStencils( childIntegrator , stencils ); + matrix.Resize( (int)range ); + std::vector< AdjacenctNodeKey > neighborKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i( _sNodes.treeNodes[i+start] ) ) + { + AdjacenctNodeKey& neighborKey = neighborKeys[ omp_get_thread_num() ]; + TreeOctNode* node = _sNodes.treeNodes[i+start]; + // Get the matrix row size + typename TreeOctNode::Neighbors< OverlapSize > neighbors; + neighborKey.template getNeighbors< false , OverlapRadius , OverlapRadius >( node , neighbors ); + int count = _GetMatrixRowSize< FEMDegree >( neighbors ); + + // Allocate memory for the row +#pragma omp critical (matrix_set_row_size) + matrix.SetRowSize( i , count ); + + // Set the row entries + matrix.rowSizes[i] = _SetMatrixRow( pointInfo , neighbors , matrix[i] , (int)start , integrator , stencil , bsData ); + if( depth>_minDepth ) + { + // Offset the constraints using the solution from lower resolutions. + int x , y , z , c; + if( node->parent ) + { + c = int( node - node->parent->children ); + Cube::FactorCornerIndex( c , x , y , z ); + } + else x = y = z = 0; + if( coarseToFine ) + { + typename TreeOctNode::Neighbors< OverlapSize > pNeighbors; + neighborKey.template getNeighbors< false , OverlapRadius , OverlapRadius >( node->parent , pNeighbors ); + _UpdateConstraintsFromCoarser( pointInfo , neighbors , pNeighbors , node , constraints , *metSolution , childIntegrator , stencils[x][y][z] , bsData ); + } + } + } + return 1; +} + +template< class Real > +template< int FEMDegree > +int Octree< Real >::_GetSliceMatrixAndUpdateConstraints( const SparseNodeData< PointData< Real > , 0 >& pointInfo , SparseMatrix< Real >& matrix , DenseNodeData< Real , FEMDegree >& constraints , typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::Integrator& integrator , typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::ChildIntegrator& childIntegrator , const BSplineData< FEMDegree >& bsData , int depth , int slice , const DenseNodeData< Real , FEMDegree >& metSolution , bool coarseToFine ) +{ + static const int OverlapSize = BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize; + static const int OverlapRadius = -BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapStart; + + int nStart = _sNodes.begin( depth , slice ) , nEnd = _sNodes.end( depth , slice ); + size_t range = nEnd-nStart; + Stencil< double , OverlapSize > stencil , stencils[2][2][2]; + SystemCoefficients< FEMDegree , FEMDegree >::SetCentralLaplacianStencil ( integrator , stencil ); + SystemCoefficients< FEMDegree , FEMDegree >::SetCentralLaplacianStencils( childIntegrator , stencils ); + + matrix.Resize( (int)range ); + std::vector< AdjacenctNodeKey > neighborKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i( _sNodes.treeNodes[i+nStart] ) ) + { + AdjacenctNodeKey& neighborKey = neighborKeys[ omp_get_thread_num() ]; + TreeOctNode* node = _sNodes.treeNodes[i+nStart]; + // Get the matrix row size + typename TreeOctNode::Neighbors< OverlapSize > neighbors; + neighborKey.template getNeighbors< false , OverlapRadius , OverlapRadius >( node , neighbors ); + int count = _GetMatrixRowSize< FEMDegree >( neighbors ); + + // Allocate memory for the row +#pragma omp critical (matrix_set_row_size) + { + matrix.SetRowSize( i , count ); + } + + // Set the row entries + matrix.rowSizes[i] = _SetMatrixRow( pointInfo , neighbors , matrix[i] , _sNodes.begin(depth,slice) , integrator , stencil , bsData ); + + + if( depth>_minDepth ) + { + // Offset the constraints using the solution from lower resolutions. + int x , y , z , c; + if( node->parent ) + { + c = int( node - node->parent->children ); + Cube::FactorCornerIndex( c , x , y , z ); + } + else x = y = z = 0; + if( coarseToFine ) + { + typename TreeOctNode::Neighbors< OverlapSize > pNeighbors; + neighborKey.template getNeighbors< false, OverlapRadius , OverlapRadius >( node->parent , pNeighbors ); + _UpdateConstraintsFromCoarser( pointInfo , neighbors , pNeighbors , node , constraints , metSolution , childIntegrator , stencils[x][y][z] , bsData ); + } + } + } + return 1; +} + +template< class Real > +template< int FEMDegree > +int Octree< Real >::_SolveSystemGS( const BSplineData< FEMDegree >& bsData , SparseNodeData< PointData< Real > , 0 >& pointInfo , int depth , DenseNodeData< Real , FEMDegree >& solution , DenseNodeData< Real , FEMDegree >& constraints , DenseNodeData< Real , FEMDegree >& metSolutionConstraints , int iters , bool coarseToFine , bool showResidual , double* bNorm2 , double* inRNorm2 , double* outRNorm2 , bool forceSilent ) +{ + const int OverlapRadius = -BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapStart; + typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::Integrator integrator; + typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::ChildIntegrator childIntegrator; + BSplineIntegrationData< FEMDegree , FEMDegree >::SetIntegrator( integrator , depth-1 , _dirichlet , _dirichlet ); + if( depth>_minDepth ) BSplineIntegrationData< FEMDegree , FEMDegree >::SetChildIntegrator( childIntegrator , depth-2 , _dirichlet , _dirichlet ); + + DenseNodeData< Real , FEMDegree > metSolution , metConstraints; + if( coarseToFine ) metSolution = metSolutionConstraints; // This stores the up-sampled solution up to depth-2 + else metConstraints = metSolutionConstraints; // This stores the down-sampled constraints up to depth + + double _maxMemoryUsage = maxMemoryUsage; + maxMemoryUsage = 0; + int slices = _Dimension< FEMDegree >(depth); + double systemTime=0. , solveTime=0. , updateTime=0. , evaluateTime = 0.; + + if( coarseToFine ) + { + if( depth>_minDepth ) + { + // Up-sample the cumulative change in solution @(depth-2) into the cumulative change in solution @(depth-1) + if( depth-2>=_minDepth ) _UpSample( depth-1 , metSolution ); + // Add in the change in solution @(depth-1) +#pragma omp parallel for num_threads( threads ) + for( int i=_sNodes.begin(depth-1) ; i<_sNodes.end(depth-1) ; i++ ) metSolution[i] += solution[i]; + // Evaluate the points @(depth) using the cumulative change in solution @(depth-1) + if( _constrainValues ) + { + evaluateTime = Time(); + _SetPointValuesFromCoarser( pointInfo , depth , bsData , metSolution ); + evaluateTime = Time() - evaluateTime; + } + } + } + else if( depth<_sNodes.levels()-1 ) + for( int i=_sNodes.begin(depth) ; i<_sNodes.end(depth) ; i++ ) constraints[i] -= metConstraints[i]; + double bNorm=0 , inRNorm=0 , outRNorm=0; + if( depth>=_minDepth ) + { + // Add padding space if we are computing residuals + int frontOffset = ( showResidual || inRNorm2 ) ? OverlapRadius : 0; + int backOffset = ( showResidual || outRNorm2 ) ? OverlapRadius : 0; + // Set the number of in-memory slices required for a temporally blocked solver + int solveSlices = std::min< int >( OverlapRadius*iters - (OverlapRadius-1) , slices ) , matrixSlices = std::max< int >( 1 , std::min< int >( solveSlices+frontOffset+backOffset , slices ) ); + // The list of matrices for each in-memory slices + std::vector< SparseMatrix< Real > > _M( matrixSlices ); + // The list of multi-colored indices for each in-memory slice + std::vector< std::vector< std::vector< int > > > __mcIndices( std::max< int >( 0 , solveSlices ) ); + + int dir = coarseToFine ? -1 : 1 , start = coarseToFine ? slices-1 : 0 , end = coarseToFine ? -1 : slices; + for( int frontSlice=start-frontOffset*dir , backSlice = frontSlice-OverlapRadius*(iters-1)*dir ; backSlice!=end+backOffset*dir ; frontSlice+=dir , backSlice+=dir ) + { + double t; + if( frontSlice+frontOffset*dir>=0 && frontSlice+frontOffset*dir ) start = _M[_s][j]; + ConstPointer( MatrixEntry< Real > ) end = start + _M[_s].rowSizes[j]; + ConstPointer( MatrixEntry< Real > ) e; + for( e=start ; e!=end ; e++ ) temp += X[ e->N ] * e->Value; + bNorm += B[j]*B[j]; + inRNorm += (temp-B[j]) * (temp-B[j]); + } + else if( bNorm2 ) +#pragma omp parallel for num_threads( threads ) reduction( + : bNorm ) + for( int j=0 ; j<_M[_s].rows ; j++ ) bNorm += B[j]*B[j]; + } + t = Time(); + // Compute the multicolor indices + if( iters && frontSlice>=0 && frontSlice( _sNodes.begin(depth,s) , _sNodes.end(depth,s) , __mcIndices[__s] ); + } + // Advance through the in-memory slices, taking an appropriately sized stride + for( int slice=frontSlice ; slice*dir>=backSlice*dir ; slice-=OverlapRadius*dir ) + if( slice>=0 && slice::SolveGS( __mcIndices[__s] , _M[_s] , B , X , !coarseToFine , threads ); + } + solveTime += Time() - t; + // Compute residuals + if( (showResidual || outRNorm2) && backSlice-backOffset*dir>=0 && backSlice-backOffset*dir ) start = _M[_s][j]; + ConstPointer( MatrixEntry< Real > ) end = start + _M[_s].rowSizes[j]; + ConstPointer( MatrixEntry< Real > ) e; + for( e=start ; e!=end ; e++ ) temp += X[ e->N ] * e->Value; + outRNorm += (temp-B[j]) * (temp-B[j]); + } + } + } + } + + if( bNorm2 ) bNorm2[depth] = bNorm; + if( inRNorm2 ) inRNorm2[depth] = inRNorm; + if( outRNorm2 ) outRNorm2[depth] = outRNorm; + if( showResidual && iters ) + { + for( int i=0 ; i %.4e -> %.4e (%.2e) [%d]\n" , sqrt( bNorm ) , sqrt( inRNorm ) , sqrt( outRNorm ) , sqrt( outRNorm/bNorm ) , iters ); + } + + if( !coarseToFine && depth>_minDepth ) + { + // Explicitly compute the restriction of the met solution onto the coarser nodes + // and down-sample the previous accumulation + { + _UpdateConstraintsFromFiner( childIntegrator , bsData , depth , solution , metConstraints ); + if( _constrainValues ) _SetPointConstraintsFromFiner( pointInfo , depth , bsData , solution , metConstraints ); + if( depth<_sNodes.levels()-1 ) _DownSample( depth , metConstraints ); + } + } + MemoryUsage(); + if( !forceSilent ) DumpOutput( "\tEvaluated / Got / Solved in: %6.3f / %6.3f / %6.3f\t(%.3f MB)\n" , evaluateTime , systemTime , solveTime , float( maxMemoryUsage ) ); + maxMemoryUsage = std::max< double >( maxMemoryUsage , _maxMemoryUsage ); + + return iters; +} + +template< class Real > +template< int FEMDegree > +int Octree< Real >::_SolveSystemCG( const BSplineData< FEMDegree >& bsData , SparseNodeData< PointData< Real > , 0 >& pointInfo , int depth , DenseNodeData< Real , FEMDegree >& solution , DenseNodeData< Real , FEMDegree >& constraints , DenseNodeData< Real , FEMDegree >& metSolutionConstraints , int iters , bool coarseToFine , bool showResidual , double* bNorm2 , double* inRNorm2 , double* outRNorm2 , double accuracy ) +{ + typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::Integrator integrator; + typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::ChildIntegrator childIntegrator; + BSplineIntegrationData< FEMDegree , FEMDegree >::SetIntegrator( integrator , depth-1 , _dirichlet , _dirichlet ); + if( depth>_minDepth ) BSplineIntegrationData< FEMDegree , FEMDegree >::SetChildIntegrator( childIntegrator , depth-2 , _dirichlet , _dirichlet ); + + DenseNodeData< Real , FEMDegree > metSolution , metConstraints; + if( coarseToFine ) metSolution = metSolutionConstraints; // This stores the up-sampled solution up to depth-2 + else metConstraints = metSolutionConstraints; // This stores the down-sampled constraints up to depth + double _maxMemoryUsage = maxMemoryUsage; + maxMemoryUsage = 0; + int iter = 0; + Pointer( Real ) X = solution.data + _sNodes.begin( depth ); + Pointer( Real ) B = constraints.data + _sNodes.begin( depth ); + SparseMatrix< Real > M; + double systemTime=0. , solveTime=0. , updateTime=0. , evaluateTime = 0.; + + if( coarseToFine ) + { + if( depth>_minDepth ) + { + // Up-sample the cumulative change in solution @(depth-2) into the cumulative change in solution @(depth-1) + if( depth-2>=_minDepth ) _UpSample( depth-1 , metSolution ); + // Add in the change in solution @(depth-1) +#pragma omp parallel for num_threads( threads ) + for( int i=_sNodes.begin(depth-1) ; i<_sNodes.end(depth-1) ; i++ ) metSolution[i] += solution[i]; + // Evaluate the points @(depth) using the cumulative change in solution @(depth-1) + if( _constrainValues ) + { + evaluateTime = Time(); + _SetPointValuesFromCoarser( pointInfo , depth , bsData , metSolution ); + evaluateTime = Time() - evaluateTime; + } + } + } + else if( depth<_sNodes.levels()-1 ) + for( int i=_sNodes.begin(depth) ; i<_sNodes.end(depth) ; i++ ) constraints[i] -= metConstraints[i]; + + // Get the system matrix (and adjust the right-hand-side based on the coarser solution if prolonging) + systemTime = Time(); + _GetMatrixAndUpdateConstraints( pointInfo , M , constraints , integrator , childIntegrator , bsData , depth , coarseToFine ? &metSolution : NULL , coarseToFine ); + systemTime = Time()-systemTime; + + solveTime = Time(); + // Solve the linear system + accuracy = Real( accuracy / 100000 ) * M.rows; + int dim = _Dimension< FEMDegree >( depth ); + int nonZeroRows = 0; + for( int i=0 ; i( _sNodes.size(depth) ); + if( addDCTerm ) M.MultiplyAndAddAverage( ( ConstPointer( Real ) )X , temp , threads ); + else M.Multiply( ( ConstPointer( Real ) )X , temp , threads ); +#pragma omp parallel for num_threads( threads ) + for( int i=0 ; i<_sNodes.size(depth) ; i++ ) temp[i] -= B[i]; +#pragma omp parallel for num_threads( threads ) reduction( + : inRNorm ) + for( int i=0 ; i<_sNodes.size(depth) ; i++ ) inRNorm += temp[i] * temp[i]; + FreePointer( temp ); + } + + iters = std::min< int >( nonZeroRows , iters ); + if( iters ) iter += SparseMatrix< Real >::SolveCG( M , ( ConstPointer( Real ) )B , iters , X , Real( accuracy ) , 0 , addDCTerm , false , threads ); + solveTime = Time()-solveTime; + if( showResidual || outRNorm2 ) + { + outRNorm = 0; + Pointer( Real ) temp = AllocPointer< Real >( _sNodes.size(depth) ); + if( addDCTerm ) M.MultiplyAndAddAverage( ( ConstPointer( Real ) )X , temp , threads ); + else M.Multiply( ( ConstPointer( Real ) )X , temp , threads ); +#pragma omp parallel for num_threads( threads ) + for( int i=0 ; i<_sNodes.size(depth) ; i++ ) temp[i] -= B[i]; +#pragma omp parallel for num_threads( threads ) reduction( + : outRNorm ) + for( int i=0 ; i<_sNodes.size(depth) ; i++ ) outRNorm += temp[i] * temp[i]; + FreePointer( temp ); + } + if( bNorm2 ) bNorm2[depth] = bNorm * bNorm; + if( inRNorm2 ) inRNorm2[depth] = inRNorm * inRNorm; + if( outRNorm2 ) outRNorm2[depth] = outRNorm * outRNorm; + if( showResidual && iters ) + { + for( int i=0 ; i %.4e -> %.4e (%.2e) [%d]\n" , bNorm , inRNorm , outRNorm , outRNorm/bNorm , iter ); + } + + if( !coarseToFine && depth>_minDepth ) + { + // Explicitly compute the restriction of the met solution onto the coarser nodes + // and down-sample the previous accumulation + { + _UpdateConstraintsFromFiner( childIntegrator , bsData , depth , solution , metConstraints ); + if( _constrainValues ) _SetPointConstraintsFromFiner( pointInfo , depth , bsData , solution , metConstraints ); + if( depth<_sNodes.levels()-1 ) _DownSample( depth , metConstraints ); + } + } + + MemoryUsage(); + DumpOutput( "\tEvaluated / Got / Solved in: %6.3f / %6.3f / %6.3f\t(%.3f MB)\n" , evaluateTime , systemTime , solveTime , float( maxMemoryUsage ) ); + maxMemoryUsage = std::max< double >( maxMemoryUsage , _maxMemoryUsage ); + return iter; +} + +template< class Real > +template< int FEMDegree > +int Octree< Real >::_GetMatrixRowSize( const typename TreeOctNode::Neighbors< BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize >& neighbors ) const +{ + static const int OverlapSize = BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize; + static const int OverlapRadius = - BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapStart; + + int count = 0; + int nodeIndex = neighbors.neighbors[OverlapRadius][OverlapRadius][OverlapRadius]->nodeData.nodeIndex; + const TreeOctNode* const * _nodes = &neighbors.neighbors[0][0][0]; + for( int i=0 ; i( _nodes[i] ) ) count++; + return count; +} + + +template< class Real > +template< int FEMDegree1 , int FEMDegree2 > +void Octree< Real >::_SetParentOverlapBounds( const TreeOctNode* node , int& startX , int& endX , int& startY , int& endY , int& startZ , int& endZ ) +{ + const int OverlapStart = BSplineIntegrationData< FEMDegree1 , FEMDegree2 >::OverlapStart; + + if( node->parent ) + { + int x , y , z , c = int( node - node->parent->children ); + Cube::FactorCornerIndex( c , x , y , z ); + startX = BSplineIntegrationData< FEMDegree1 , FEMDegree2 >::ParentOverlapStart[x]-OverlapStart , endX = BSplineIntegrationData< FEMDegree1 , FEMDegree2 >::ParentOverlapEnd[x]-OverlapStart+1; + startY = BSplineIntegrationData< FEMDegree1 , FEMDegree2 >::ParentOverlapStart[y]-OverlapStart , endY = BSplineIntegrationData< FEMDegree1 , FEMDegree2 >::ParentOverlapEnd[y]-OverlapStart+1; + startZ = BSplineIntegrationData< FEMDegree1 , FEMDegree2 >::ParentOverlapStart[z]-OverlapStart , endZ = BSplineIntegrationData< FEMDegree1 , FEMDegree2 >::ParentOverlapEnd[z]-OverlapStart+1; + } +} + +// It is assumed that at this point, the evaluationg of the current depth's points, using the coarser resolution solution +// has already happened +template< class Real > +template< int FEMDegree > +void Octree< Real >::_UpdateConstraintsFromCoarser( const SparseNodeData< PointData< Real > , 0 >& pointInfo , const typename TreeOctNode::Neighbors< BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize >& neighbors , const typename TreeOctNode::Neighbors< BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize >& pNeighbors , TreeOctNode* node , DenseNodeData< Real , FEMDegree >& constraints , const DenseNodeData< Real , FEMDegree >& metSolution , const typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::ChildIntegrator& childIntegrator , const Stencil< double , BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize >& lapStencil , const BSplineData< FEMDegree >& bsData ) const +{ + static const int OverlapSize = BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapStart; + static const int LeftSupportRadius = -BSplineEvaluationData< FEMDegree >::SupportStart; + static const int RightSupportRadius = BSplineEvaluationData< FEMDegree >::SupportEnd; + static const int OverlapRadius = - BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapStart; + + const std::vector< PointData< Real > >& points = pointInfo.data; + if( node->depth()<=_minDepth ) return; + // This is a conservative estimate as we only need to make sure that the parent nodes don't overlap the child (not the parent itself) + bool isInterior = _IsInteriorlyOverlapped< FEMDegree , FEMDegree >( node->parent ); + int d , off[3]; + node->depthAndOffset( d , off ); + Real constraint = Real( 0 ); + // Offset the constraints using the solution from lower resolutions. + int startX , endX , startY , endY , startZ , endZ; + _SetParentOverlapBounds< FEMDegree , FEMDegree >( node , startX , endX , startY , endY , startZ , endZ ); + + for( int x=startX ; x( pNeighbors.neighbors[x][y][z] ) ) + { + const TreeOctNode* _node = pNeighbors.neighbors[x][y][z]; + Real _solution = metSolution[ _node->nodeData.nodeIndex ]; + { + if( isInterior ) constraints[ node->nodeData.nodeIndex ] -= Real( lapStencil.values[x][y][z] * _solution ); + else + { + int _d , _off[3]; + _node->depthAndOffset( _d , _off ); + constraints[ node->nodeData.nodeIndex ] -= Real( SystemCoefficients< FEMDegree , FEMDegree >::GetLaplacian( childIntegrator , _off , off ) * _solution ); + } + } + } + if( _constrainValues ) + { + double constraint = 0; + int fIdx[3]; + FunctionIndex< FEMDegree >( node , fIdx ); + // Evaluate the current node's basis function at adjacent points + for( int x=-LeftSupportRadius ; x<=RightSupportRadius ; x++ ) for( int y=-LeftSupportRadius ; y<=RightSupportRadius ; y++ ) for( int z=-LeftSupportRadius ; z<=RightSupportRadius ; z++ ) + { + const TreeOctNode* _node = neighbors.neighbors[x+OverlapRadius][y+OverlapRadius][z+OverlapRadius]; + if( _IsValidNode< 0 >( _node ) && pointInfo.index( _node )!=-1 ) + { + const PointData< Real >& pData = points[ pointInfo.index( _node ) ]; + Point3D< Real > p = pData.position; + constraint += + bsData.baseBSplines[ fIdx[0] ][x+LeftSupportRadius]( p[0] ) * + bsData.baseBSplines[ fIdx[1] ][y+LeftSupportRadius]( p[1] ) * + bsData.baseBSplines[ fIdx[2] ][z+LeftSupportRadius]( p[2] ) * + pData.weightedCoarserDValue; + } + } + constraints[ node->nodeData.nodeIndex ] -= Real( constraint ); + } +} + +// Given the solution @( depth ) add to the met constraints @( depth-1 ) +template< class Real > +template< int FEMDegree > +void Octree< Real >::_UpdateConstraintsFromFiner( const typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::ChildIntegrator& childIntegrator , const BSplineData< FEMDegree >& bsData , int depth , const DenseNodeData< Real , FEMDegree >& fineSolution , DenseNodeData< Real , FEMDegree >& coarseConstraints ) const +{ + static const int OverlapSize = BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize; + static const int OverlapRadius = - BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapStart; + typedef typename TreeOctNode::NeighborKey< -BSplineEvaluationData< FEMDegree >::SupportStart , BSplineEvaluationData< FEMDegree >::SupportEnd >SupportKey; + + if( depth<=_minDepth ) return; + // Get the stencil describing the Laplacian relating coefficients @(depth) with coefficients @(depth-1) + Stencil< double , OverlapSize > stencils[2][2][2]; + SystemCoefficients< FEMDegree , FEMDegree >::SetCentralLaplacianStencils( childIntegrator , stencils ); + size_t start = _sNodes.begin(depth) , end = _sNodes.end(depth) , range = end-start; + int lStart = _sNodes.begin(depth-1); + memset( coarseConstraints.data + _sNodes.begin(depth-1) , 0 , sizeof(Real)*_sNodes.size(depth-1) ); + + // Iterate over the nodes @( depth ) + std::vector< SupportKey > neighborKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i( _sNodes.treeNodes[i] ) ) + { + SupportKey& neighborKey = neighborKeys[ omp_get_thread_num() ]; + TreeOctNode* node = _sNodes.treeNodes[i]; + + // Offset the coarser constraints using the solution from the current resolutions. + int x , y , z , c; + c = int( node - node->parent->children ); + Cube::FactorCornerIndex( c , x , y , z ); + { + typename TreeOctNode::Neighbors< OverlapSize > pNeighbors; + neighborKey.template getNeighbors< false , OverlapRadius , OverlapRadius >( node->parent , pNeighbors ); + const Stencil< double , OverlapSize >& lapStencil = stencils[x][y][z]; + + bool isInterior = _IsInteriorlyOverlapped< FEMDegree , FEMDegree >( node->parent ); + int d , off[3]; + node->depthAndOffset( d , off ); + + // Offset the constraints using the solution from finer resolutions. + int startX , endX , startY , endY , startZ , endZ; + _SetParentOverlapBounds< FEMDegree , FEMDegree >( node , startX , endX , startY , endY , startZ , endZ ); + + Real solution = fineSolution[ node->nodeData.nodeIndex ]; + for( int x=startX ; x( pNeighbors.neighbors[x][y][z] ) ) + { + const TreeOctNode* _node = pNeighbors.neighbors[x][y][z]; + if( isInterior ) +#pragma omp atomic + coarseConstraints[ _node->nodeData.nodeIndex ] += Real( lapStencil.values[x][y][z] * solution ); + else + { + int _d , _off[3]; + _node->depthAndOffset( _d , _off ); +#pragma omp atomic + coarseConstraints[ _node->nodeData.nodeIndex ] += Real( SystemCoefficients< FEMDegree , FEMDegree >::GetLaplacian( childIntegrator , _off , off ) * solution ); + } + } + } + } +} + + +template< class Real > +template< int FEMDegree > +DenseNodeData< Real , FEMDegree > Octree< Real >::SolveSystem( SparseNodeData< PointData< Real > , 0 >& pointInfo , DenseNodeData< Real , FEMDegree >& constraints , bool showResidual , int iters , int maxSolveDepth , int cgDepth , double accuracy ) +{ + BSplineData< FEMDegree > bsData; + bsData.set( maxSolveDepth , _dirichlet ); + + maxSolveDepth++; + int iter=0; + iters = std::max< int >( 0 , iters ); + + DenseNodeData< Real , FEMDegree > solution( _sNodes.size() ); + memset( solution.data , 0 , sizeof(Real)*_sNodes.size() ); + + solution[0] = 0; + + DenseNodeData< Real , FEMDegree > metSolution( _sNodes.end( _sNodes.levels()-2 ) ); + memset( metSolution.data , 0 , sizeof(Real)*_sNodes.end( _sNodes.levels()-2 ) ); + for( int d=_minDepth ; d<_sNodes.levels() ; d++ ) + { + DumpOutput( "Depth[%d/%d]: %d\n" , d-1 , _sNodes.levels()-2 , _sNodes.size( d ) ); + if( d==_minDepth ) _SolveSystemCG( bsData , pointInfo , d , solution , constraints , metSolution , _sNodes.size(_minDepth) , true , showResidual , NULL , NULL , NULL ); + else + { + if( d>cgDepth ) iter += _SolveSystemGS( bsData , pointInfo , d , solution , constraints , metSolution , d>maxSolveDepth ? 0 : iters , true , showResidual , NULL , NULL , NULL ); + else iter += _SolveSystemCG( bsData , pointInfo , d , solution , constraints , metSolution , d>maxSolveDepth ? 0 : iters , true , showResidual , NULL , NULL , NULL , accuracy ); + } + } + metSolution.resize( 0 ); + return solution; +} + +template< class Real > +template< int FEMDegree , int NormalDegree > +DenseNodeData< Real , FEMDegree > Octree< Real >::SetLaplacianConstraints( const SparseNodeData< Point3D< Real > , NormalDegree >& normalInfo ) +{ + typedef typename TreeOctNode::NeighborKey< -BSplineEvaluationData< FEMDegree >::SupportStart , BSplineEvaluationData< FEMDegree >::SupportEnd > SupportKey; + const int OverlapSize = BSplineIntegrationData< NormalDegree , FEMDegree >::OverlapSize; + const int LeftNormalFEMOverlapRadius = -BSplineIntegrationData< NormalDegree , FEMDegree >::OverlapStart; + const int RightNormalFEMOverlapRadius = BSplineIntegrationData< NormalDegree , FEMDegree >::OverlapEnd; + const int LeftFEMNormalOverlapRadius = -BSplineIntegrationData< FEMDegree , NormalDegree >::OverlapStart; + const int RightFEMNormalOverlapRadius = BSplineIntegrationData< FEMDegree , NormalDegree >::OverlapEnd; + + // To set the Laplacian constraints, we iterate over the + // splatted normals and compute the dot-product of the + // divergence of the normal field with all the basis functions. + // Within the same depth: set directly as a gather + // Coarser depths + int maxDepth = _sNodes.levels()-1; + DenseNodeData< Real , FEMDegree > constraints( _sNodes.size() ) , _constraints( _sNodes.end( maxDepth-1 ) ); + memset( constraints.data , 0 , sizeof(Real)*_sNodes.size() ); + memset( _constraints.data , 0 , sizeof(Real)*( _sNodes.end(maxDepth-1) ) ); + MemoryUsage(); + + for( int d=maxDepth ; d>=_minDepth ; d-- ) + { + int offset = d>0 ? _sNodes.begin(d-1) : 0; + Stencil< Point3D< double > , OverlapSize > stencil , stencils[2][2][2]; + typename BSplineIntegrationData< NormalDegree , FEMDegree >::FunctionIntegrator::Integrator integrator; + typename BSplineIntegrationData< FEMDegree , NormalDegree >::FunctionIntegrator::ChildIntegrator childIntegrator; + BSplineIntegrationData< NormalDegree , FEMDegree >::SetIntegrator( integrator , d-1 , _dirichlet , _dirichlet ); + if( d>_minDepth ) BSplineIntegrationData< FEMDegree , NormalDegree >::SetChildIntegrator( childIntegrator , d-2 , _dirichlet , _dirichlet ); + SystemCoefficients< NormalDegree , FEMDegree >::SetCentralDivergenceStencil ( integrator , stencil , false ); + SystemCoefficients< FEMDegree , NormalDegree >::SetCentralDivergenceStencils( childIntegrator , stencils , true ); + + std::vector< SupportKey > neighborKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; idepth(); + typename TreeOctNode::Neighbors< OverlapSize > neighbors; + neighborKey.template getNeighbors< false , LeftFEMNormalOverlapRadius , RightFEMNormalOverlapRadius >( node , neighbors ); + bool isInterior = _IsInteriorlyOverlapped< FEMDegree , NormalDegree >( node ) , isInterior2 = _IsInteriorlyOverlapped< NormalDegree , FEMDegree >( node->parent ); + + int cx , cy , cz; + if( d>_minDepth ) Cube::FactorCornerIndex( (int)( node-node->parent->children) , cx , cy ,cz ); + else cx = cy = cz = 0; + Stencil< Point3D< double > , OverlapSize >& _stencil = stencils[cx][cy][cz]; + int d , off[3]; + node->depthAndOffset( d , off ); + // Set constraints from current depth + // Gather the constraints from the vector-field at _node into the constraint stored with node + if( _IsValidNode< FEMDegree >( node ) ) + { + for( int x=startX ; x( _node ) ) + { + int _idx = normalInfo.index( _node ); + if( _idx>=0 ) + if( isInterior ) constraints[i] += Point3D< Real >::Dot( stencil.values[x][y][z] , normalInfo.data[ _idx ] ); + else + { + int _d , _off[3]; + _node->depthAndOffset( _d , _off ); + constraints[i] += Real( SystemCoefficients< NormalDegree , FEMDegree >::GetDivergence2( integrator , _off , off , normalInfo.data[ _idx ] ) ); + } + } + } + _SetParentOverlapBounds< NormalDegree , FEMDegree >( node , startX , endX , startY , endY , startZ , endZ ); + } + if( !_IsValidNode< NormalDegree >( node ) ) continue; + int idx = normalInfo.index( node ); + if( idx<0 ) continue; + const Point3D< Real >& normal = normalInfo.data[ idx ]; + if( normal[0]==0 && normal[1]==0 && normal[2]==0 ) continue; + + // Set the _constraints for the parents + if( depth>_minDepth ) + { + neighborKey.template getNeighbors< false , LeftNormalFEMOverlapRadius , RightNormalFEMOverlapRadius >( node->parent , neighbors ); + + for( int x=startX ; x( _node ) ) ) + { + TreeOctNode* _node = neighbors.neighbors[x][y][z]; + Real c; + if( isInterior2 ) c = Point3D< Real >::Dot( _stencil.values[x][y][z] , normal ); + else + { + int _d , _off[3]; + _node->depthAndOffset( _d , _off ); + c = Real( SystemCoefficients< FEMDegree , NormalDegree >::GetDivergence1( childIntegrator , _off , off , normal ) ); + } +#pragma omp atomic + _constraints[ _node->nodeData.nodeIndex ] += c; + } + } + } + } + MemoryUsage(); + } + + // Fine-to-coarse down-sampling of constraints + for( int d=maxDepth-1 ; d>_minDepth ; d-- ) _DownSample( d , _constraints ); + + // Add the accumulated constraints from all finer depths +#pragma omp parallel for num_threads( threads ) + for( int i=0 ; i<_sNodes.end(maxDepth-1) ; i++ ) constraints[i] += _constraints[i]; + + _constraints.resize( 0 ); + + DenseNodeData< Point3D< Real > , NormalDegree > coefficients( _sNodes.end( maxDepth-1 ) ); + for( int d=maxDepth-1 ; d>=_minDepth ; d-- ) + { +#pragma omp parallel for num_threads( threads ) + for( int i=_sNodes.begin(d) ; i<_sNodes.end(d) ; i++ ) if( _IsValidNode< NormalDegree >( _sNodes.treeNodes[i] ) ) + { + int idx = normalInfo.index( _sNodes.treeNodes[i] ); + if( idx<0 ) continue; + coefficients[i] = normalInfo.data[ idx ]; + } + } + + // Coarse-to-fine up-sampling of coefficients + for( int d=_minDepth+1 ; d , OverlapSize > stencils[2][2][2]; + typename BSplineIntegrationData< NormalDegree , FEMDegree >::FunctionIntegrator::ChildIntegrator childIntegrator; + if( d>_minDepth ) BSplineIntegrationData< NormalDegree , FEMDegree >::SetChildIntegrator( childIntegrator , d-2 , _dirichlet , _dirichlet ); + SystemCoefficients< NormalDegree , FEMDegree >::SetCentralDivergenceStencils( childIntegrator , stencils , false ); + std::vector< SupportKey > neighborKeys( std::max< int >( 1 , threads ) ); + for( size_t i=0 ; i( _sNodes.treeNodes[i] ) ) + { + SupportKey& neighborKey = neighborKeys[ omp_get_thread_num() ]; + TreeOctNode* node = _sNodes.treeNodes[i]; + int depth = node->depth(); + if( !depth ) continue; + int startX , endX , startY , endY , startZ , endZ; + _SetParentOverlapBounds< FEMDegree , NormalDegree >( node , startX , endX , startY , endY , startZ , endZ ); + typename TreeOctNode::Neighbors< OverlapSize > neighbors; + neighborKey.template getNeighbors< false , LeftFEMNormalOverlapRadius , RightFEMNormalOverlapRadius >( node->parent , neighbors ); + + bool isInterior = _IsInteriorlyOverlapped< FEMDegree , NormalDegree >( node->parent ); + int cx , cy , cz; + if( d ) + { + int c = int( node - node->parent->children ); + Cube::FactorCornerIndex( c , cx , cy , cz ); + } + else cx = cy = cz = 0; + Stencil< Point3D< double > , OverlapSize >& _stencil = stencils[cx][cy][cz]; + + Real constraint = Real(0); + int d , off[3]; + node->depthAndOffset( d , off ); + for( int x=startX ; x( _node ) ) + { + int _i = _node->nodeData.nodeIndex; + if( isInterior ) constraint += Point3D< Real >::Dot( coefficients[_i] , _stencil.values[x][y][z] ); + else + { + int _d , _off[3]; + _node->depthAndOffset( _d , _off ); + constraint += Real( SystemCoefficients< NormalDegree , FEMDegree >::GetDivergence2( childIntegrator , _off , off , coefficients[_i] ) ); + } + } + } + constraints[ node->nodeData.nodeIndex ] += constraint; + } + } + MemoryUsage(); + coefficients.resize( 0 ); + + return constraints; +} diff --git a/colmap/include/colmap/lib/PoissonRecon/MultiGridOctreeData.WeightedSamples.inl b/colmap/include/colmap/lib/PoissonRecon/MultiGridOctreeData.WeightedSamples.inl new file mode 100644 index 0000000000000000000000000000000000000000..aa94e1cf2db432c3a53ce91066307b7f7c3ed3df --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/MultiGridOctreeData.WeightedSamples.inl @@ -0,0 +1,590 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +// Evaluate the result of splatting along a plane and then evaluating at a point on the plane. +template< int Degree > double GetScaleValue( void ) +{ + double centerValues[Degree+1]; + Polynomial< Degree >::BSplineComponentValues( 0.5 , centerValues ); + double scaleValue = 0; + for( int i=0 ; i<=Degree ; i++ ) scaleValue += centerValues[i] * centerValues[i]; + return 1./ scaleValue; +} +template< class Real > +template< int WeightDegree > +void Octree< Real >::_AddWeightContribution( SparseNodeData< Real , WeightDegree >& densityWeights , TreeOctNode* node , Point3D< Real > position , PointSupportKey< WeightDegree >& weightKey , Real weight ) +{ + static const double ScaleValue = GetScaleValue< WeightDegree >(); + double dx[ DIMENSION ][ PointSupportKey< WeightDegree >::Size ]; + typename TreeOctNode::Neighbors< PointSupportKey< WeightDegree >::Size >& neighbors = weightKey.template getNeighbors< true >( node ); + + if( densityWeights.indices.size() start; + Real w; + _StartAndWidth( node , start , w ); + + for( int dim=0 ; dim::BSplineComponentValues( ( position[dim]-start[dim] ) / w , dx[dim] ); + + weight *= (Real)ScaleValue; + + for( int i=0 ; i::Size ; i++ ) for( int j=0 ; j::Size ; j++ ) + { + double dxdy = dx[0][i] * dx[1][j] * weight; + TreeOctNode** _neighbors = neighbors.neighbors[i][j]; + for( int k=0 ; k::Size ; k++ ) if( _neighbors[k] ) + { + int idx = densityWeights.index( _neighbors[k] ); + if( idx<0 ) + { + densityWeights.indices[ _neighbors[k]->nodeData.nodeIndex ] = (int)densityWeights.data.size(); + densityWeights.data.push_back( (Real)( dxdy * dx[2][k] ) ); + } + else densityWeights.data[idx] += Real( dxdy * dx[2][k] ); + } + } +} + +template< class Real > +template< int WeightDegree > +Real Octree< Real >::_GetSamplesPerNode( const SparseNodeData< Real , WeightDegree >& densityWeights , TreeOctNode* node , Point3D< Real > position , PointSupportKey< WeightDegree >& weightKey ) +{ + Real weight = 0; + double dx[ DIMENSION ][ PointSupportKey< WeightDegree >::Size ]; + typename TreeOctNode::Neighbors< PointSupportKey< WeightDegree >::Size >& neighbors = weightKey.template getNeighbors< true >( node ); + + Point3D< Real > start; + Real w; + _StartAndWidth( node , start , w ); + + for( int dim=0 ; dim::BSplineComponentValues( ( position[dim]-start[dim] ) / w , dx[dim] ); + + for( int i=0 ; i::Size ; i++ ) for( int j=0 ; j::Size ; j++ ) + { + double dxdy = dx[0][i] * dx[1][j]; + for( int k=0 ; k::Size ; k++ ) if( neighbors.neighbors[i][j][k] ) + { + int idx = densityWeights.index( neighbors.neighbors[i][j][k] ); + if( idx>=0 ) weight += Real( dxdy * dx[2][k] * densityWeights.data[idx] ); + } + } + return weight; +} +template< class Real > +template< int WeightDegree > +Real Octree< Real >::_GetSamplesPerNode( const SparseNodeData< Real , WeightDegree >& densityWeights , const TreeOctNode* node , Point3D< Real > position , ConstPointSupportKey< WeightDegree >& weightKey ) const +{ + Real weight = 0; + double dx[ DIMENSION ][ PointSupportKey< WeightDegree >::Size ]; + typename TreeOctNode::ConstNeighbors< PointSupportKey< WeightDegree >::Size >& neighbors = weightKey.getNeighbors( node ); + + Point3D< Real > start; + Real w; + _StartAndWidth( node , start , w ); + + for( int dim=0 ; dim::BSplineComponentValues( ( position[dim]-start[dim] ) / w , dx[dim] ); + + for( int i=0 ; i::Size ; i++ ) for( int j=0 ; j::Size ; j++ ) + { + double dxdy = dx[0][i] * dx[1][j]; + for( int k=0 ; k::Size ; k++ ) if( neighbors.neighbors[i][j][k] ) + { + int idx = densityWeights.index( neighbors.neighbors[i][j][k] ); + if( idx>=0 ) weight += Real( dxdy * dx[2][k] * densityWeights.data[idx] ); + } + } + return weight; +} +template< class Real > +template< int WeightDegree > +void Octree< Real >::_GetSampleDepthAndWeight( const SparseNodeData< Real , WeightDegree >& densityWeights , const TreeOctNode* node , Point3D< Real > position , ConstPointSupportKey< WeightDegree >& weightKey , Real& depth , Real& weight ) const +{ + const TreeOctNode* temp = node; + weight = _GetSamplesPerNode( densityWeights , temp , position , weightKey ); + if( weight>=(Real)1. ) depth = Real( _Depth( temp ) + log( weight ) / log(double(1<<(DIMENSION-1))) ); + else + { + Real oldWeight , newWeight; + oldWeight = newWeight = weight; + while( newWeight<(Real)1. && temp->parent ) + { + temp=temp->parent; + oldWeight = newWeight; + newWeight = _GetSamplesPerNode( densityWeights , temp , position , weightKey ); + } + depth = Real( _Depth( temp ) + log( newWeight ) / log( newWeight / oldWeight ) ); + } + weight = Real( pow( double(1<<(DIMENSION-1)) , -double(depth) ) ); +} +template< class Real > +template< int WeightDegree > +void Octree< Real >::_GetSampleDepthAndWeight( const SparseNodeData< Real , WeightDegree >& densityWeights , TreeOctNode* node , Point3D< Real > position , PointSupportKey< WeightDegree >& weightKey , Real& depth , Real& weight ) +{ + TreeOctNode* temp = node; + weight = _GetSamplesPerNode( densityWeights , temp , position , weightKey ); + if( weight>=(Real)1. ) depth = Real( _Depth( temp ) + log( weight ) / log(double(1<<(DIMENSION-1))) ); + else + { + Real oldWeight , newWeight; + oldWeight = newWeight = weight; + while( newWeight<(Real)1. && temp->parent ) + { + temp=temp->parent; + oldWeight = newWeight; + newWeight = _GetSamplesPerNode( densityWeights , temp , position, weightKey ); + } + depth = Real( _Depth( temp ) + log( newWeight ) / log( newWeight / oldWeight ) ); + } + weight = Real( pow( double(1<<(DIMENSION-1)) , -double(depth) ) ); +} +template< class Real > +template< int WeightDegree > +void Octree< Real >::_GetSampleDepthAndWeight( const SparseNodeData< Real , WeightDegree >& densityWeights , Point3D< Real > position , ConstPointSupportKey< WeightDegree >& weightKey , Real& depth , Real& weight ) +{ + TreeOctNode* temp; + Point3D< Real > myCenter( (Real)0.5 , (Real)0.5 , (Real)0.5 ); + Real myWidth = Real( 1. ); + + // Get the finest node with depth less than or equal to the splat depth that contains the point + temp = _spaceRoot; + while( _Depth( temp )<_splatDepth ) + { + if( !temp->children ) break;// fprintf( stderr , "[ERROR] Octree::GetSampleDepthAndWeight\n" ) , exit( 0 ); + int cIndex = TreeOctNode::CornerIndex( myCenter , position ); + temp = &temp->children[cIndex]; + myWidth /= 2; + if( cIndex&1 ) myCenter[0] += myWidth/2; + else myCenter[0] -= myWidth/2; + if( cIndex&2 ) myCenter[1] += myWidth/2; + else myCenter[1] -= myWidth/2; + if( cIndex&4 ) myCenter[2] += myWidth/2; + else myCenter[2] -= myWidth/2; + } + return _GetSampleDepthAndWeight( densityWeights , temp , position , weightKey , depth , weight ); +} +template< class Real > +template< int WeightDegree > +void Octree< Real >::_GetSampleDepthAndWeight( const SparseNodeData< Real , WeightDegree >& densityWeights , Point3D< Real > position , PointSupportKey< WeightDegree >& weightKey , Real& depth , Real& weight ) +{ + TreeOctNode* temp; + Point3D< Real > myCenter( (Real)0.5 , (Real)0.5 , (Real)0.5 ); + Real myWidth = Real( 1. ); + + // Get the finest node with depth less than or equal to the splat depth that contains the point + temp = _spaceRoot; + while( _Depth( temp )<_splatDepth ) + { + if( !temp->children ) break;// fprintf( stderr , "[ERROR] Octree::GetSampleDepthAndWeight\n" ) , exit( 0 ); + int cIndex = TreeOctNode::CornerIndex( myCenter , position ); + temp = &temp->children[cIndex]; + myWidth /= 2; + if( cIndex&1 ) myCenter[0] += myWidth/2; + else myCenter[0] -= myWidth/2; + if( cIndex&2 ) myCenter[1] += myWidth/2; + else myCenter[1] -= myWidth/2; + if( cIndex&4 ) myCenter[2] += myWidth/2; + else myCenter[2] -= myWidth/2; + } + return _GetSampleDepthAndWeight( densityWeights , temp , position , weightKey , depth , weight ); +} + + +template< class Real > +template< int DataDegree , class V > +void Octree< Real >::_SplatPointData( TreeOctNode* node , Point3D< Real > position , V v , SparseNodeData< V , DataDegree >& dataInfo , PointSupportKey< DataDegree >& dataKey ) +{ + double dx[ DIMENSION ][ PointSupportKey< DataDegree >::Size ]; + typename TreeOctNode::Neighbors< PointSupportKey< DataDegree >::Size >& neighbors = dataKey.template getNeighbors< true >( node ); + + Point3D< Real > start; + Real w; + _StartAndWidth( node , start , w ); + + for( int dd=0 ; dd::BSplineComponentValues( ( position[dd]-start[dd] ) / w , dx[dd] ); + + for( int i=0 ; i::Size ; i++ ) for( int j=0 ; j::Size ; j++ ) + { + double dxdy = dx[0][i] * dx[1][j]; + for( int k=0 ; k::Size ; k++ ) + if( neighbors.neighbors[i][j][k] ) + { + TreeOctNode* _node = neighbors.neighbors[i][j][k]; + + double dxdydz = dxdy * dx[2][k]; + if( (int)dataInfo.indices.size()nodeData.nodeIndex ] = (int)dataInfo.data.size(); + dataInfo.data.push_back( v * Real(dxdydz) ); + } + else dataInfo.data[idx] += v * Real( dxdydz ); + } + } +} +template< class Real > +template< int WeightDegree , int DataDegree , class V > +Real Octree< Real >::_SplatPointData( const SparseNodeData< Real , WeightDegree >& densityWeights , Point3D< Real > position , V v , SparseNodeData< V , DataDegree >& dataInfo , PointSupportKey< WeightDegree >& weightKey , PointSupportKey< DataDegree >& dataKey , int minDepth , int maxDepth , int dim ) +{ + double dx; + V _v; + TreeOctNode* temp; + int cnt=0; + double width; + Point3D< Real > myCenter( (Real)0.5 , (Real)0.5 , (Real)0.5 ); + Real myWidth = (Real)1.; + + temp = _spaceRoot; + while( _Depth( temp )<_splatDepth ) + { + if( !temp->children ) fprintf( stderr , "[ERROR] Octree::SplatPointData\n" ) , exit( 0 ); + int cIndex = TreeOctNode::CornerIndex( myCenter , position ); + temp = &temp->children[cIndex]; + myWidth /= 2; + if( cIndex&1 ) myCenter[0] += myWidth/2; + else myCenter[0] -= myWidth/2; + if( cIndex&2 ) myCenter[1] += myWidth/2; + else myCenter[1] -= myWidth/2; + if( cIndex&4 ) myCenter[2] += myWidth/2; + else myCenter[2] -= myWidth/2; + } + Real weight , depth; + _GetSampleDepthAndWeight( densityWeights , temp , position , weightKey , depth , weight ); + + if( depthmaxDepth ) depth = Real(maxDepth); + int topDepth = int(ceil(depth)); + + dx = 1.0-(topDepth-depth); + if ( topDepth<=minDepth ) topDepth = minDepth , dx = 1; + else if( topDepth> maxDepth ) topDepth = maxDepth , dx = 1; + + while( _Depth( temp )>topDepth ) temp=temp->parent; + while( _Depth( temp )children ) temp->initChildren(); + int cIndex = TreeOctNode::CornerIndex( myCenter , position ); + temp = &temp->children[cIndex]; + myWidth/=2; + if( cIndex&1 ) myCenter[0] += myWidth/2; + else myCenter[0] -= myWidth/2; + if( cIndex&2 ) myCenter[1] += myWidth/2; + else myCenter[1] -= myWidth/2; + if( cIndex&4 ) myCenter[2] += myWidth/2; + else myCenter[2] -= myWidth/2; + } + width = 1.0 / ( ( 1<<( _Depth( temp ) ) ) ); + _v = v * weight / Real( pow( width , dim ) ) * Real( dx ); + _SplatPointData( temp , position , _v , dataInfo , dataKey ); + if( fabs(1.0-dx) > EPSILON ) + { + dx = Real(1.0-dx); + temp = temp->parent; + width = 1.0 / ( ( 1<<( _Depth( temp ) ) ) ); + + _v = v * weight / Real( pow( width , dim ) ) * Real( dx ); + _SplatPointData( temp , position , _v , dataInfo , dataKey ); + } + return weight; +} +template< class Real > +template< int WeightDegree , int DataDegree , class V > +void Octree< Real >::_MultiSplatPointData( const SparseNodeData< Real , WeightDegree >* densityWeights , Point3D< Real > position , V v , SparseNodeData< V , DataDegree >& dataInfo , PointSupportKey< WeightDegree >& weightKey , PointSupportKey< DataDegree >& dataKey , int maxDepth , int dim ) +{ + Real _depth , weight; + if( densityWeights ) _GetSampleDepthAndWeight( *densityWeights , position , weightKey , _depth , weight ); + else weight = (Real)1. , _depth = (Real)maxDepth; + int depth = std::min< int >( maxDepth , (int)ceil( _depth ) ); + V _v = v * weight; + + Point3D< Real > myCenter( (Real)0.5 , (Real)0.5 , (Real)0.5 ); + Real myWidth = (Real)1.; + + TreeOctNode* temp = _spaceRoot; + while( _Depth( temp )<=depth ) + { + _SplatPointData( temp , position , _v * Real( pow( 1<<_Depth( temp ) , dim ) ) , dataInfo , dataKey ); + if( _Depth( temp )children ) temp->initChildren(); + int cIndex = TreeOctNode::CornerIndex( myCenter , position ); + temp = &temp->children[cIndex]; + myWidth /= 2; + if( cIndex&1 ) myCenter[0] += myWidth/2; + else myCenter[0] -= myWidth/2; + if( cIndex&2 ) myCenter[1] += myWidth/2; + else myCenter[1] -= myWidth/2; + if( cIndex&4 ) myCenter[2] += myWidth/2; + else myCenter[2] -= myWidth/2; + } + else break; + } +} +template< class Real > +template< class V , int DataDegree > +V Octree< Real >::_Evaluate( const DenseNodeData< V , DataDegree >& coefficients , Point3D< Real > p , const BSplineData< DataDegree >& bsData , const ConstPointSupportKey< DataDegree >& neighborKey ) const +{ + V value = V(0); + + for( int d=0 ; d<=neighborKey.depth() ; d++ ) for( int i=0 ; i::Size ; i++ ) for( int j=0 ; j::Size ; j++ ) for( int k=0 ; k::Size ; k++ ) + { + const TreeOctNode* n = neighborKey.neighbors[d].neighbors[i][j][k]; + if( _IsValidNode< DataDegree >( n ) ) + { + int fIdx[3]; + FunctionIndex< DataDegree >( n , fIdx ); + value += + ( + coefficients[ n->nodeData.nodeIndex ] * + (Real) + ( + bsData.baseBSplines[ fIdx[0] ][PointSupportKey< DataDegree >::Size-1-i]( p[0] ) * + bsData.baseBSplines[ fIdx[1] ][PointSupportKey< DataDegree >::Size-1-j]( p[1] ) * + bsData.baseBSplines[ fIdx[2] ][PointSupportKey< DataDegree >::Size-1-k]( p[2] ) + ) + ); + } + } + + return value; +} +template< class Real > +template< class V , int DataDegree > +V Octree< Real >::_Evaluate( const SparseNodeData< V , DataDegree >& coefficients , Point3D< Real > p , const BSplineData< DataDegree >& bsData , const ConstPointSupportKey< DataDegree >& dataKey ) const +{ + V value = V(0); + + for( int d=0 ; d<=dataKey.depth() ; d++ ) + { + double dx[ DIMENSION ][ PointSupportKey< DataDegree >::Size ]; + memset( dx , 0 , sizeof( double ) * DIMENSION * PointSupportKey< DataDegree >::Size ); + { + const TreeOctNode* n = dataKey.neighbors[d].neighbors[ PointSupportKey< DataDegree >::LeftRadius ][ PointSupportKey< DataDegree >::LeftRadius ][ PointSupportKey< DataDegree >::LeftRadius ]; + if( !n ) fprintf( stderr , "[ERROR] Point is not centered on a node\n" ) , exit( 0 ); + int fIdx[3]; + FunctionIndex< DataDegree >( n , fIdx ); + int fStart , fEnd; + BSplineData< DataDegree >::FunctionSpan( d-1 , fStart , fEnd ); + for( int dd=0 ; dd::LeftRadius ; i<=PointSupportKey< DataDegree >::RightRadius ; i++ ) + if( fIdx[dd]+i>=fStart && fIdx[dd]+i::RightRadius ]( p[dd] ); + } + for( int i=0 ; i::Size ; i++ ) for( int j=0 ; j::Size ; j++ ) for( int k=0 ; k::Size ; k++ ) + { + const TreeOctNode* n = dataKey.neighbors[d].neighbors[i][j][k]; + if( _IsValidNode< DataDegree >( n ) ) + { + int idx = coefficients.index( n ); + if( idx>=0 ) value += coefficients.data[ idx ] * (Real) ( dx[0][i] * dx[1][j] * dx[2][k] ); + } + } + } + + return value; +} + +template< class Real > +template< class V , int DataDegree > +V Octree< Real >::Evaluate( const DenseNodeData< V , DataDegree >& coefficients , Point3D< Real > p , const BSplineData< DataDegree >& bsData ) const +{ + static const int SupportSize = BSplineEvaluationData< DataDegree >::SupportSize; + static const int LeftSupportRadius = -BSplineEvaluationData< DataDegree >::SupportStart; + static const int RightSupportRadius = BSplineEvaluationData< DataDegree >::SupportEnd; + V value = V(0); + + // [WARNING] This is required because the B-Spline components are not continuous at the domain boundaries + // so we need to nudge things inward a tiny bit. + for( int dd=0 ; dd<3 ; dd++ ) + if ( p[dd]==0 ) p[dd] = 0.+1e-6; + else if( p[dd]==1 ) p[dd] = 1.-1e-6; + + const TreeOctNode* n = _tree.nextNode(); + while( n ) + { + Point3D< Real > s; + Real w; + _StartAndWidth( n , s , w ); + double left = (LeftSupportRadius+0.)*w , right = (RightSupportRadius+1.)*w; + if( p[0]<=s[0]-left || p[0]>=s[0]+right || p[1]<=s[1]-left || p[1]>=s[1]+right || p[2]<=s[2]-left || p[2]>=s[2]+right ) + { + n = _tree.nextBranch( n ); + continue; + } + if( _IsValidNode< DataDegree >( n ) ) + { + int d , fIdx[3] , pIdx[3]; + _DepthAndOffset( n , d , fIdx ); + for( int dd=0 ; dd<3 ; dd++ ) pIdx[dd] = std::max< int >( 0 , std::min< int >( SupportSize-1 , LeftSupportRadius + (int)floor( ( p[dd]-s[dd] ) / w ) ) ); + value += + coefficients[ n->nodeData.nodeIndex ] * + (Real) + ( + bsData.baseBSplines[ BSplineData< DataDegree >::FunctionIndex( d , fIdx[0] ) ][ pIdx[0] ]( p[0] ) * + bsData.baseBSplines[ BSplineData< DataDegree >::FunctionIndex( d , fIdx[1] ) ][ pIdx[1] ]( p[1] ) * + bsData.baseBSplines[ BSplineData< DataDegree >::FunctionIndex( d , fIdx[2] ) ][ pIdx[2] ]( p[2] ) + ); + } + n = _tree.nextNode( n ); + } + return value; +} +template< class Real > +template< class V , int DataDegree > +V Octree< Real >::Evaluate( const SparseNodeData< V , DataDegree >& coefficients , Point3D< Real > p , const BSplineData< DataDegree >& bsData ) const +{ + V value = V(0); + + const TreeOctNode* n = _tree.nextNode(); + while( n ) + { + Point3D< Real > s; + Real w; + _StartAndWidth( n , s , w ); + if( !_IsValidNode< DataDegree >( n ) || + p[0]::SupportStart*w || p[0]>s[0]+(BSplineData< DataDegree >::SupportEnd+1.0)*w || + p[1]::SupportStart*w || p[1]>s[1]+(BSplineData< DataDegree >::SupportEnd+1.0)*w || + p[2]::SupportStart*w || p[2]>s[2]+(BSplineData< DataDegree >::SupportEnd+1.0)*w ) + { + n = _tree.nextBranch( n ); + continue; + } + + int idx = coefficients.index( n ); + if( idx>=0 ) + { + int d , off[3] , pIdx[3]; + _DepthAndOffset( n , d , off ); + for( int dd=0 ; dd<3 ; dd++ ) pIdx[dd] = std::max< int >( 0 , std::min< int >( BSplineData< DataDegree >::SupportSize-1 , -BSplineData< DataDegree >::SupportStart + (int)floor( ( p[dd]-s[dd] ) / w ) ) ); + value += + coefficients.data[idx] * + (Real) + ( + bsData.baseBSplines[ BSplineData< DataDegree >::FunctionIndex( d , off[0] ) ][ pIdx[0] ]( p[0] ) * + bsData.baseBSplines[ BSplineData< DataDegree >::FunctionIndex( d , off[1] ) ][ pIdx[1] ]( p[1] ) * + bsData.baseBSplines[ BSplineData< DataDegree >::FunctionIndex( d , off[2] ) ][ pIdx[2] ]( p[2] ) + ); + } + n = _tree.nextNode( n ); + } + return value; +} +template< class Real > +template< class V , int DataDegree > +Pointer( V ) Octree< Real >::Evaluate( const DenseNodeData< V , DataDegree >& coefficients , int& res , Real isoValue , int depth , bool primal ) +{ + int dim; + if( depth>=0 ) depth++; + int maxDepth = _tree.maxDepth(); + if( depth<=0 || depth>maxDepth ) depth = maxDepth; + + BSplineData< DataDegree > fData; + fData.set( depth , _dirichlet ); + + // Initialize the coefficients at the coarsest level + Pointer( V ) _coefficients = NullPointer( V ); + { + int d = _minDepth; + dim = _Dimension< DataDegree >( d ); + _coefficients = NewPointer< V >( dim * dim * dim ); + memset( _coefficients , 0 , sizeof( V ) * dim * dim * dim ); +#pragma omp parallel for num_threads( threads ) + for( int i=_sNodes.begin(d) ; i<_sNodes.end(d) ; i++ ) if( _IsValidNode< DataDegree >( _sNodes.treeNodes[i] ) ) + { + int _d , _off[3]; + _sNodes.treeNodes[i]->depthAndOffset( _d , _off ); + _coefficients[ _off[0] + _off[1]*dim + _off[2]*dim*dim ] = coefficients[i]; + } + } + + // Up-sample and add in the existing coefficients + for( int d=_minDepth+1 ; d<=depth ; d++ ) + { + dim = _Dimension< DataDegree >( d ); + Pointer( V ) __coefficients = NewPointer< V >( dim * dim *dim ); + memset( __coefficients , 0 , sizeof( V ) * dim * dim * dim ); +#pragma omp parallel for num_threads( threads ) + for( int i=_sNodes.begin(d) ; i<_sNodes.end(d) ; i++ ) if( _IsValidNode< DataDegree >( _sNodes.treeNodes[i] ) ) + { + int _d , _off[3]; + _sNodes.treeNodes[i]->depthAndOffset( _d , _off ); + __coefficients[ _off[0] + _off[1]*dim + _off[2]*dim*dim ] = coefficients[i]; + } + _UpSample< V , DataDegree >( d , ( ConstPointer(V) )_coefficients , __coefficients , _dirichlet , threads ); + DeletePointer( _coefficients ); + _coefficients = __coefficients; + } + + res = 1<<(depth-1); + if( primal ) res++; + Pointer( V ) values = NewPointer< V >( res*res*res ); + memset( values , 0 , sizeof(V)*res*res*res ); + + if( primal ) + { + // Evaluate at the cell corners + typename BSplineEvaluationData< DataDegree >::CornerEvaluator::Evaluator evaluator; + BSplineEvaluationData< DataDegree >::SetCornerEvaluator( evaluator , depth-1 , _dirichlet ); +#pragma omp parallel for num_threads( threads ) + for( int k=0 ; k::CornerEnd ; kk<=-BSplineEvaluationData< DataDegree >::CornerStart ; kk++ ) if( k+kk>=0 && k+kk::CornerEnd ; jj<=-BSplineEvaluationData< DataDegree >::CornerStart ; jj++ ) if( j+jj>=0 && j+jj::CornerEnd ; ii<=-BSplineEvaluationData< DataDegree >::CornerStart ; ii++ ) if( i+ii>=0 && i+ii::CenterEvaluator::Evaluator evaluator; + BSplineEvaluationData< DataDegree >::SetCenterEvaluator( evaluator , depth-1 , _dirichlet ); +#pragma omp parallel for num_threads( threads ) + for( int k=0 ; k::SupportEnd ; kk<=-BSplineEvaluationData< DataDegree >::SupportStart ; kk++ ) if( k+kk>=0 && k+kk::SupportEnd ; jj<=-BSplineEvaluationData< DataDegree >::SupportStart ; jj++ ) if( j+jj>=0 && j+jj::SupportEnd ; ii<=-BSplineEvaluationData< DataDegree >::SupportStart ; ii++ ) if( i+ii>=0 && i+ii + // b_1[i] = < \nabla B_i(p) , V(p) > + // 2] Formulate this as a Poisson equation: + // \sum_i x_i \Delta B_i(p) = \nabla \cdot V(p) + // which is solved by the system A_2x = b_2 where: + // A_2[i,j] = - < \Delta B_i(p) , B_j(p) > + // b_2[i] = - < B_i(p) , \nabla \cdot V(p) > + // Although the two system matrices should be the same (assuming that the B_i satisfy dirichlet/neumann boundary conditions) + // the constraint vectors can differ when V does not satisfy the Neumann boundary conditions: + // A_1[i,j] = \int_R < \nabla B_i(p) , \nabla B_j(p) > + // = \int_R [ \nabla \cdot ( B_i(p) \nabla B_j(p) ) - B_i(p) \Delta B_j(p) ] + // = \int_dR < N(p) , B_i(p) \nabla B_j(p) > + A_2[i,j] + // and the first integral is zero if either f_i is zero on the boundary dR or the derivative of B_i across the boundary is zero. + // However, for the constraints we have: + // b_1(i) = \int_R < \nabla B_i(p) , V(p) > + // = \int_R [ \nabla \cdot ( B_i(p) V(p) ) - B_i(p) \nabla \cdot V(p) ] + // = \int_dR < N(p) , B_i(p) V(p) > + b_2[i] + // In particular, this implies that if the B_i satisfy the Neumann boundary conditions (rather than Dirichlet), + // and V is not zero across the boundary, then the two constraints are different. + // Forcing the < V(p) , N(p) > = 0 on the boundary, by killing off the component of the vector-field in the normal direction + // (FORCE_NEUMANN_FIELD), makes the two systems equal, and the value of this flag should be immaterial. + // Note that under interpretation 1, we have: + // \sum_i b_1(i) = < \nabla \sum_ i B_i(p) , V(p) > = 0 + // because the B_i's sum to one. However, in general, we could have + // \sum_i b_2(i) \neq 0. + // This could cause trouble because the constant functions are in the kernel of the matrix A, so CG will misbehave if the constraint + // has a non-zero DC term. (Again, forcing < V(p) , N(p) > = 0 along the boundary resolves this problem.) + +#define FORCE_NEUMANN_FIELD 1 // This flag forces the normal component across the boundary of the integration domain to be zero. + // This should be enabled if GRADIENT_DOMAIN_SOLUTION is not, so that CG doesn't run into trouble. + +#if !FORCE_NEUMANN_FIELD +#pragma message( "[WARNING] Not zeroing out normal component on boundary" ) +#endif // !FORCE_NEUMANN_FIELD + +#include "Hash.h" +#include "BSplineData.h" +#include "PointStream.h" + +#ifndef _OPENMP +int omp_get_num_procs( void ){ return 1; } +int omp_get_thread_num( void ){ return 0; } +#endif // _OPENMP + +class TreeNodeData +{ +public: + static size_t NodeCount; + int nodeIndex; + char flags; + + TreeNodeData( void ); + ~TreeNodeData( void ); +}; + +class VertexData +{ + typedef OctNode< TreeNodeData > TreeOctNode; +public: + static const int VERTEX_COORDINATE_SHIFT = ( sizeof( long long ) * 8 ) / 3; + static long long EdgeIndex( const TreeOctNode* node , int eIndex , int maxDepth , int index[DIMENSION] ); + static long long EdgeIndex( const TreeOctNode* node , int eIndex , int maxDepth ); + static long long FaceIndex( const TreeOctNode* node , int fIndex , int maxDepth,int index[DIMENSION] ); + static long long FaceIndex( const TreeOctNode* node , int fIndex , int maxDepth ); + static long long CornerIndex( const TreeOctNode* node , int cIndex , int maxDepth , int index[DIMENSION] ); + static long long CornerIndex( const TreeOctNode* node , int cIndex , int maxDepth ); + static long long CenterIndex( const TreeOctNode* node , int maxDepth , int index[DIMENSION] ); + static long long CenterIndex( const TreeOctNode* node , int maxDepth ); + static long long CornerIndex( int depth , const int offSet[DIMENSION] , int cIndex , int maxDepth , int index[DIMENSION] ); + static long long CenterIndex( int depth , const int offSet[DIMENSION] , int maxDepth , int index[DIMENSION] ); + static long long CornerIndexKey( const int index[DIMENSION] ); +}; + +// This class stores the octree nodes, sorted by depth and then by z-slice. +// To support primal representations, the initializer takes a function that +// determines if a node should be included/indexed in the sorted list. +class SortedTreeNodes +{ + typedef OctNode< TreeNodeData > TreeOctNode; +protected: + Pointer( Pointer( int ) ) _sliceStart; + int _levels; +public: + Pointer( TreeOctNode* ) treeNodes; + int begin( int depth ) const{ return _sliceStart[depth][0]; } + int end( int depth ) const{ return _sliceStart[depth][(size_t)1<=_levels||slice<0||slice>=(1<=_levels) printf( "uhoh\n" ); return _sliceStart[depth][(size_t)1<* map ); + void set( TreeOctNode& root ); + + template< int Indices > + struct _Indices + { + int idx[Indices]; + _Indices( void ){ memset( idx , -1 , sizeof( int ) * Indices ); } + int& operator[] ( int i ) { return idx[i]; } + const int& operator[] ( int i ) const { return idx[i]; } + }; + typedef _Indices< Square::CORNERS > SquareCornerIndices; + typedef _Indices< Square::EDGES > SquareEdgeIndices; + typedef _Indices< Square::FACES > SquareFaceIndices; + + struct SliceTableData + { + Pointer( SquareCornerIndices ) cTable; + Pointer( SquareEdgeIndices ) eTable; + Pointer( SquareFaceIndices ) fTable; + int cCount , eCount , fCount , nodeOffset , nodeCount; + SliceTableData( void ){ fCount = eCount = cCount = 0 , cTable = NullPointer( SquareCornerIndices ) , eTable = NullPointer( SquareEdgeIndices ) , fTable = NullPointer( SquareFaceIndices ) , _cMap = _eMap = _fMap = NullPointer( int ); } + ~SliceTableData( void ){ clear(); } + void clear( void ){ DeletePointer( cTable ) ; DeletePointer( eTable ) ; DeletePointer( fTable ) ; fCount = eCount = cCount = 0; } + SquareCornerIndices& cornerIndices( const TreeOctNode* node ); + SquareCornerIndices& cornerIndices( int idx ); + const SquareCornerIndices& cornerIndices( const TreeOctNode* node ) const; + const SquareCornerIndices& cornerIndices( int idx ) const; + SquareEdgeIndices& edgeIndices( const TreeOctNode* node ); + SquareEdgeIndices& edgeIndices( int idx ); + const SquareEdgeIndices& edgeIndices( const TreeOctNode* node ) const; + const SquareEdgeIndices& edgeIndices( int idx ) const; + SquareFaceIndices& faceIndices( const TreeOctNode* node ); + SquareFaceIndices& faceIndices( int idx ); + const SquareFaceIndices& faceIndices( const TreeOctNode* node ) const; + const SquareFaceIndices& faceIndices( int idx ) const; + protected: + Pointer( int ) _cMap; + Pointer( int ) _eMap; + Pointer( int ) _fMap; + friend class SortedTreeNodes; + }; + struct XSliceTableData + { + Pointer( SquareCornerIndices ) eTable; + Pointer( SquareEdgeIndices ) fTable; + int fCount , eCount , nodeOffset , nodeCount; + XSliceTableData( void ){ fCount = eCount = 0 , eTable = NullPointer( SquareCornerIndices ) , fTable = NullPointer( SquareEdgeIndices ) , _eMap = _fMap = NullPointer( int ); } + ~XSliceTableData( void ){ clear(); } + void clear( void ) { DeletePointer( fTable ) ; DeletePointer( eTable ) ; fCount = eCount = 0; } + SquareCornerIndices& edgeIndices( const TreeOctNode* node ); + SquareCornerIndices& edgeIndices( int idx ); + const SquareCornerIndices& edgeIndices( const TreeOctNode* node ) const; + const SquareCornerIndices& edgeIndices( int idx ) const; + SquareEdgeIndices& faceIndices( const TreeOctNode* node ); + SquareEdgeIndices& faceIndices( int idx ); + const SquareEdgeIndices& faceIndices( const TreeOctNode* node ) const; + const SquareEdgeIndices& faceIndices( int idx ) const; + protected: + Pointer( int ) _eMap; + Pointer( int ) _fMap; + friend class SortedTreeNodes; + }; + void setSliceTableData ( SliceTableData& sData , int depth , int offset , int threads ) const; + void setXSliceTableData( XSliceTableData& sData , int depth , int offset , int threads ) const; +}; + +template< int Degree > +struct PointSupportKey : public OctNode< TreeNodeData >::NeighborKey< BSplineEvaluationData< Degree >::SupportEnd , -BSplineEvaluationData< Degree >::SupportStart > +{ + static const int LeftRadius = BSplineEvaluationData< Degree >::SupportEnd; + static const int RightRadius = -BSplineEvaluationData< Degree >::SupportStart; + static const int Size = LeftRadius + RightRadius + 1; +}; +template< int Degree > +struct ConstPointSupportKey : public OctNode< TreeNodeData >::ConstNeighborKey< BSplineEvaluationData< Degree >::SupportEnd , -BSplineEvaluationData< Degree >::SupportStart > +{ + static const int LeftRadius = BSplineEvaluationData< Degree >::SupportEnd; + static const int RightRadius = -BSplineEvaluationData< Degree >::SupportStart; + static const int Size = LeftRadius + RightRadius + 1; +}; + +template< class Real > +struct PointData +{ + Point3D< Real > position; + Real weightedCoarserDValue; + Real weight; + PointData( Point3D< Real > p=Point3D< Real >() , Real w=0 ) { position = p , weight = w , weightedCoarserDValue = Real(0); } +}; +template< class Data , int Degree > +struct SparseNodeData +{ + std::vector< int > indices; + std::vector< Data > data; + template< class TreeNodeData > + int index( const OctNode< TreeNodeData >* node ) const { return ( !node || node->nodeData.nodeIndex<0 || node->nodeData.nodeIndex>=(int)indices.size() ) ? -1 : indices[ node->nodeData.nodeIndex ]; } +#if NEW_NEW_CODE + int index( int nodeIndex ) const { return ( nodeIndex<0 || nodeIndex>=(int)indices.size() ) ? -1 : indices[ nodeIndex ]; } +#endif // NEW_NEW_CODE + void resize( size_t sz ){ indices.resize( sz , -1 ); } + void remapIndices( const std::vector< int >& map ) + { + std::vector< int > temp = indices; + indices.resize( map.size() ); + for( size_t i=0 ; i +struct DenseNodeData +{ + Pointer( Data ) data; + DenseNodeData( void ) { data = NullPointer( Data ); } + DenseNodeData( size_t sz ){ if( sz ) data = NewPointer< Data >( sz ) ; else data = NullPointer( Data ); } + void resize( size_t sz ){ DeletePointer( data ) ; if( sz ) data = NewPointer< Data >( sz ) ; else data = NullPointer( Data ); } + Data& operator[] ( int idx ) { return data[idx]; } + const Data& operator[] ( int idx ) const { return data[idx]; } +}; + +template< class C , int N > struct Stencil{ C values[N][N][N]; }; + +template< int Degree1 , int Degree2 > +class SystemCoefficients +{ + typedef typename BSplineIntegrationData< Degree1 , Degree2 >::FunctionIntegrator FunctionIntegrator; + static const int OverlapSize = BSplineIntegrationData< Degree1 , Degree2 >::OverlapSize; + static const int OverlapStart = BSplineIntegrationData< Degree1 , Degree2 >::OverlapStart; + static const int OverlapEnd = BSplineIntegrationData< Degree1 , Degree2 >::OverlapEnd; +public: + static double GetLaplacian ( const typename FunctionIntegrator:: Integrator& integrator , const int off1[3] , const int off2[3] ); + static double GetLaplacian ( const typename FunctionIntegrator::ChildIntegrator& integrator , const int off1[3] , const int off2[3] ); + static double GetDivergence1( const typename FunctionIntegrator:: Integrator& integrator , const int off1[3] , const int off2[3] , Point3D< double > normal1 ); + static double GetDivergence1( const typename FunctionIntegrator::ChildIntegrator& integrator , const int off1[3] , const int off2[3] , Point3D< double > normal1 ); + static double GetDivergence2( const typename FunctionIntegrator:: Integrator& integrator , const int off1[3] , const int off2[3] , Point3D< double > normal2 ); + static double GetDivergence2( const typename FunctionIntegrator::ChildIntegrator& integrator , const int off1[3] , const int off2[3] , Point3D< double > normal2 ); + static Point3D< double > GetDivergence1 ( const typename FunctionIntegrator:: Integrator& integrator , const int off1[3] , const int off2[3] ); + static Point3D< double > GetDivergence1 ( const typename FunctionIntegrator::ChildIntegrator& integrator , const int off1[3] , const int off2[3] ); + static Point3D< double > GetDivergence2 ( const typename FunctionIntegrator:: Integrator& integrator , const int off1[3] , const int off2[3] ); + static Point3D< double > GetDivergence2 ( const typename FunctionIntegrator::ChildIntegrator& integrator , const int off1[3] , const int off2[3] ); + static void SetCentralDivergenceStencil ( const typename FunctionIntegrator:: Integrator& integrator , Stencil< Point3D< double > , OverlapSize >& stencil , bool scatter ); + static void SetCentralDivergenceStencils( const typename FunctionIntegrator::ChildIntegrator& integrator , Stencil< Point3D< double > , OverlapSize > stencil[2][2][2] , bool scatter ); + static void SetCentralLaplacianStencil ( const typename FunctionIntegrator:: Integrator& integrator , Stencil< double , OverlapSize >& stencil ); + static void SetCentralLaplacianStencils ( const typename FunctionIntegrator::ChildIntegrator& integrator , Stencil< double , OverlapSize > stencil[2][2][2] ); +}; + +// Note that throughout this code, the "depth" parameter refers to the depth in the octree, not the corresponding depth +// of the B-Spline element +template< class Real > +class Octree +{ + typedef OctNode< TreeNodeData > TreeOctNode; +public: + template< int FEMDegree > static void FunctionIndex( const TreeOctNode* node , int idx[3] ); + + typedef typename TreeOctNode:: NeighborKey< 1 , 1 > AdjacenctNodeKey; + typedef typename TreeOctNode::ConstNeighborKey< 1 , 1 > ConstAdjacenctNodeKey; + + template< class V > + struct ProjectiveData + { + V v; + Real w; + ProjectiveData( V vv=V(0) , Real ww=Real(0) ) : v(vv) , w(ww) { } + operator V (){ return w!=0 ? v/w : v*w; } + ProjectiveData& operator += ( const ProjectiveData& p ){ v += p.v , w += p.w ; return *this; } + ProjectiveData& operator -= ( const ProjectiveData& p ){ v -= p.v , w -= p.w ; return *this; } + ProjectiveData& operator *= ( Real s ){ v *= s , w *= s ; return *this; } + ProjectiveData& operator /= ( Real s ){ v /= s , w /= s ; return *this; } + ProjectiveData operator + ( const ProjectiveData& p ) const { return ProjectiveData( v+p.v , w+p.w ); } + ProjectiveData operator - ( const ProjectiveData& p ) const { return ProjectiveData( v-p.v , w-p.w ); } + ProjectiveData operator * ( Real s ) const { return ProjectiveData( v*s , w*s ); } + ProjectiveData operator / ( Real s ) const { return ProjectiveData( v/s , w/s ); } + }; + template< int FEMDegree > static bool IsValidNode( const TreeOctNode* node , bool dirichlet ); +protected: + template< int FEMDegree > bool _IsValidNode( const TreeOctNode* node ) const { return node && ( node->nodeData.flags & ( 1<<( FEMDegree&1 ) ) ) ; } + + TreeOctNode _tree; + TreeOctNode* _spaceRoot; + SortedTreeNodes _sNodes; + int _splatDepth; + int _maxDepth; + int _minDepth; + int _fullDepth; + bool _constrainValues; + bool _dirichlet; + Real _scale; + Point3D< Real > _center; + int _multigridDegree; + + bool _InBounds( Point3D< Real > ) const; + template< int FEMDegree > static int _Dimension( int depth ){ return BSplineData< FEMDegree >::Dimension( depth-1 ); } + static int _Resolution( int depth ){ return 1<<(depth-1); } + template< int FEMDegree > static bool _IsInteriorlySupported( int d , int x , int y , int z ) + { + if( d-1>=0 ) + { + int begin , end; + BSplineEvaluationData< FEMDegree >::InteriorSupportedSpan( d-1 , begin , end ); + return ( x>=begin && x=begin && y=begin && z static bool _IsInteriorlySupported( const TreeOctNode* node ) + { + if( !node ) return false; + int d , off[3]; + node->depthAndOffset( d , off ); + return _IsInteriorlySupported< FEMDegree >( d , off[0] , off[1] , off[2] ); + } + template< int FEMDegree1 , int FEMDegree2 > static bool _IsInteriorlyOverlapped( int d , int x , int y , int z ) + { + if( d-1>=0 ) + { + int begin , end; + BSplineIntegrationData< FEMDegree1 , FEMDegree2 >::InteriorOverlappedSpan( d-1 , begin , end ); + return ( x>=begin && x=begin && y=begin && z static bool _IsInteriorlyOverlapped( const TreeOctNode* node ) + { + if( !node ) return false; + int d , off[3]; + node->depthAndOffset( d , off ); + return _IsInteriorlyOverlapped< FEMDegree1 , FEMDegree2 >( d , off[0] , off[1] , off[2] ); + } + static void _DepthAndOffset( const TreeOctNode* node , int& d , int off[3] ){ node->depthAndOffset( d , off ) ; d -= 1; } + static int _Depth( const TreeOctNode* node ){ return node->depth()-1; } + static void _StartAndWidth( const TreeOctNode* node , Point3D< Real >& start , Real& width ) + { + int d , off[3]; + _DepthAndOffset( node , d , off ); + if( d>=0 ) width = Real( 1.0 / (1<< d ) ); + else width = Real( 1.0 * (1<<(-d)) ); + for( int dd=0 ; dd& center , Real& width ) + { + int d , off[3]; + _DepthAndOffset( node , d , off ); + width = Real( 1.0 / (1< + static typename TreeOctNode::ConstNeighbors< LeftRadius + RightRadius + 1 >& _Neighbors( TreeOctNode::ConstNeighborKey< LeftRadius , RightRadius >& key , int depth ){ return key.neighbors[ depth + 1 ]; } + template< int LeftRadius , int RightRadius > + static typename TreeOctNode::Neighbors< LeftRadius + RightRadius + 1 >& _Neighbors( TreeOctNode::NeighborKey< LeftRadius , RightRadius >& key , int depth ){ return key.neighbors[ depth + 1 ]; } + template< int LeftRadius , int RightRadius > + static const typename TreeOctNode::template Neighbors< LeftRadius + RightRadius + 1 >& _Neighbors( const typename TreeOctNode::template NeighborKey< LeftRadius , RightRadius >& key , int depth ){ return key.neighbors[ depth + 1 ]; } + template< int LeftRadius , int RightRadius > + static const typename TreeOctNode::template ConstNeighbors< LeftRadius + RightRadius + 1 >& _Neighbors( const typename TreeOctNode::template ConstNeighborKey< LeftRadius , RightRadius >& key , int depth ){ return key.neighbors[ depth + 1 ]; } + + static void _SetFullDepth( TreeOctNode* node , int depth ); + void _setFullDepth( int depth ); + + //////////////////////////////////// + // System construction code // + // MultiGridOctreeData.System.inl // + //////////////////////////////////// + template< int FEMDegree > + void _setMultiColorIndices( int start , int end , std::vector< std::vector< int > >& indices ) const; + template< int FEMDegree > + int _SolveSystemGS( const BSplineData< FEMDegree >& bsData , SparseNodeData< PointData< Real > , 0 >& pointInfo , int depth , DenseNodeData< Real , FEMDegree >& solution , DenseNodeData< Real , FEMDegree >& constraints , DenseNodeData< Real , FEMDegree >& metSolutionConstraints , int iters , bool coarseToFine , bool showResidual=false , double* bNorm2=NULL , double* inRNorm2=NULL , double* outRNorm2=NULL , bool forceSilent=false ); + template< int FEMDegree > + int _SolveSystemCG( const BSplineData< FEMDegree >& bsData , SparseNodeData< PointData< Real > , 0 >& pointInfo , int depth , DenseNodeData< Real , FEMDegree >& solution , DenseNodeData< Real , FEMDegree >& constraints , DenseNodeData< Real , FEMDegree >& metSolutionConstraints , int iters , bool coarseToFine , bool showResidual=false , double* bNorm2=NULL , double* inRNorm2=NULL , double* outRNorm2=NULL , double accuracy=0 ); + template< int FEMDegree > + int _SetMatrixRow( const SparseNodeData< PointData< Real > , 0 >& pointInfo , const typename TreeOctNode::Neighbors< BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize >& neighbors , Pointer( MatrixEntry< Real > ) row , int offset , const typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::Integrator& integrator , const Stencil< double , BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize >& stencil , const BSplineData< FEMDegree >& bsData ) const; + template< int FEMDegree > + int _GetMatrixRowSize( const typename TreeOctNode::Neighbors< BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize >& neighbors ) const; + + template< int FEMDegree1 , int FEMDegree2 > static void _SetParentOverlapBounds( const TreeOctNode* node , int& startX , int& endX , int& startY , int& endY , int& startZ , int& endZ ); + template< int FEMDegree > + void _UpdateConstraintsFromCoarser( const SparseNodeData< PointData< Real > , 0 >& pointInfo , const typename TreeOctNode::Neighbors< BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize >& neighbors , const typename TreeOctNode::Neighbors< BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize >& pNeighbors , TreeOctNode* node , DenseNodeData< Real , FEMDegree >& constraints , const DenseNodeData< Real , FEMDegree >& metSolution , const typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::ChildIntegrator& childIntegrator , const Stencil< double , BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapSize >& stencil , const BSplineData< FEMDegree >& bsData ) const; + // Updates the constraints @(depth-1) based on the solution coefficients @(depth) + template< int FEMDegree > + void _UpdateConstraintsFromFiner( const typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::ChildIntegrator& childIntegrator , const BSplineData< FEMDegree >& bsData , int highDepth , const DenseNodeData< Real , FEMDegree >& fineSolution , DenseNodeData< Real , FEMDegree >& coarseConstraints ) const; + // Evaluate the points @(depth) using coefficients @(depth-1) + template< int FEMDegree > + void _SetPointValuesFromCoarser( SparseNodeData< PointData< Real > , 0 >& pointInfo , int highDepth , const BSplineData< FEMDegree >& bsData , const DenseNodeData< Real , FEMDegree >& upSampledCoefficients ); + // Evalutes the solution @(depth) at the points @(depth-1) and updates the met constraints @(depth-1) + template< int FEMDegree > + void _SetPointConstraintsFromFiner( const SparseNodeData< PointData< Real > , 0 >& pointInfo , int highDepth , const BSplineData< FEMDegree >& bsData , const DenseNodeData< Real , FEMDegree >& finerCoefficients , DenseNodeData< Real , FEMDegree >& metConstraints ) const; + template< int FEMDegree > + Real _CoarserFunctionValue( Point3D< Real > p , const PointSupportKey< FEMDegree >& neighborKey , const TreeOctNode* node , const BSplineData< FEMDegree >& bsData , const DenseNodeData< Real , FEMDegree >& upSampledCoefficients ) const; + template< int FEMDegree > + Real _FinerFunctionValue ( Point3D< Real > p , const PointSupportKey< FEMDegree >& neighborKey , const TreeOctNode* node , const BSplineData< FEMDegree >& bsData , const DenseNodeData< Real , FEMDegree >& coefficients ) const; + template< int FEMDegree > + int _GetSliceMatrixAndUpdateConstraints( const SparseNodeData< PointData< Real > , 0 >& pointInfo , SparseMatrix< Real >& matrix , DenseNodeData< Real , FEMDegree >& constraints , typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::Integrator& integrator , typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::ChildIntegrator& childIntegrator , const BSplineData< FEMDegree >& bsData , int depth , int slice , const DenseNodeData< Real , FEMDegree >& metSolution , bool coarseToFine ); + template< int FEMDegree > + int _GetMatrixAndUpdateConstraints( const SparseNodeData< PointData< Real > , 0 >& pointInfo , SparseMatrix< Real >& matrix , DenseNodeData< Real , FEMDegree >& constraints , typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::Integrator& integrator , typename BSplineIntegrationData< FEMDegree , FEMDegree >::FunctionIntegrator::ChildIntegrator& childIntegrator , const BSplineData< FEMDegree >& bsData , int depth , const DenseNodeData< Real , FEMDegree >* metSolution , bool coarseToFine ); + + // Down samples constraints @(depth) to constraints @(depth-1) + template< class C , int FEMDegree > void _DownSample( int highDepth , DenseNodeData< C , FEMDegree >& constraints ) const; + // Up samples coefficients @(depth-1) to coefficients @(depth) + template< class C , int FEMDegree > void _UpSample( int highDepth , DenseNodeData< C , FEMDegree >& coefficients ) const; + template< class C , int FEMDegree > static void _UpSample( int highDepth , ConstPointer( C ) lowCoefficients , Pointer( C ) highCoefficients , bool dirichlet , int threads ); + + ///////////////////////////////////////////// + // Code for splatting point-sample data // + // MultiGridOctreeData.WeightedSamples.inl // + ///////////////////////////////////////////// + template< int WeightDegree > + void _AddWeightContribution( SparseNodeData< Real , WeightDegree >& densityWeights , TreeOctNode* node , Point3D< Real > position , PointSupportKey< WeightDegree >& weightKey , Real weight=Real(1.0) ); + template< int WeightDegree > + Real _GetSamplesPerNode( const SparseNodeData< Real , WeightDegree >& densityWeights , const TreeOctNode* node , Point3D< Real > position , ConstPointSupportKey< WeightDegree >& weightKey ) const; + template< int WeightDegree > + Real _GetSamplesPerNode( const SparseNodeData< Real , WeightDegree >& densityWeights , TreeOctNode* node , Point3D< Real > position , PointSupportKey< WeightDegree >& weightKey ); + template< int WeightDegree > + void _GetSampleDepthAndWeight( const SparseNodeData< Real , WeightDegree >& densityWeights , const TreeOctNode* node , Point3D< Real > position , ConstPointSupportKey< WeightDegree >& weightKey , Real& depth , Real& weight ) const; + template< int WeightDegree > + void _GetSampleDepthAndWeight( const SparseNodeData< Real , WeightDegree >& densityWeights , TreeOctNode* node , Point3D< Real > position , PointSupportKey< WeightDegree >& weightKey , Real& depth , Real& weight ); +public: + template< int WeightDegree > + void _GetSampleDepthAndWeight( const SparseNodeData< Real , WeightDegree >& densityWeights , Point3D< Real > position , PointSupportKey< WeightDegree >& weightKey , Real& depth , Real& weight ); + template< int WeightDegree > + void _GetSampleDepthAndWeight( const SparseNodeData< Real , WeightDegree >& densityWeights , Point3D< Real > position , ConstPointSupportKey< WeightDegree >& weightKey , Real& depth , Real& weight ); +protected: + template< int DataDegree , class V > void _SplatPointData( TreeOctNode* node , Point3D< Real > point , V v , SparseNodeData< V , DataDegree >& data , PointSupportKey< DataDegree >& dataKey ); + template< int WeightDegree , int DataDegree , class V > Real _SplatPointData( const SparseNodeData< Real , WeightDegree >& densityWeights , Point3D< Real > point , V v , SparseNodeData< V , DataDegree >& data , PointSupportKey< WeightDegree >& weightKey , PointSupportKey< DataDegree >& dataKey , int minDepth , int maxDepth , int dim=DIMENSION ); + template< int WeightDegree , int DataDegree , class V > void _MultiSplatPointData( const SparseNodeData< Real , WeightDegree >* densityWeights , Point3D< Real > point , V v , SparseNodeData< V , DataDegree >& data , PointSupportKey< WeightDegree >& weightKey , PointSupportKey< DataDegree >& dataKey , int maxDepth , int dim=DIMENSION ); + template< class V , int DataDegree > V _Evaluate( const DenseNodeData< V , DataDegree >& coefficients , Point3D< Real > p , const BSplineData< DataDegree >& bsData , const ConstPointSupportKey< DataDegree >& neighborKey ) const; + template< class V , int DataDegree > V _Evaluate( const SparseNodeData< V , DataDegree >& coefficients , Point3D< Real > p , const BSplineData< DataDegree >& bsData , const ConstPointSupportKey< DataDegree >& dataKey ) const; +public: + template< class V , int DataDegree > V Evaluate( const DenseNodeData< V , DataDegree >& coefficients , Point3D< Real > p , const BSplineData< DataDegree >& bsData ) const; + template< class V , int DataDegree > V Evaluate( const SparseNodeData< V , DataDegree >& coefficients , Point3D< Real > p , const BSplineData< DataDegree >& bsData ) const; + template< class V , int DataDegree > Pointer( V ) Evaluate( const DenseNodeData< V , DataDegree >& coefficients , int& res , Real isoValue=0.f , int depth=-1 , bool primal=false ); + + template< int NormalDegree > int _HasNormals( TreeOctNode* node , const SparseNodeData< Point3D< Real > , NormalDegree >& normalInfo ); + void _MakeComplete( void ); + void _SetValidityFlags( void ); + template< int NormalDegree > void _ClipTree( const SparseNodeData< Point3D< Real > , NormalDegree >& normalInfo ); + + //////////////////////////////////// + // Evaluation Methods // + // MultiGridOctreeData.Evaluation // + //////////////////////////////////// + static const int CHILDREN = Cube::CORNERS; + template< int FEMDegree > + struct _Evaluator + { + typename BSplineEvaluationData< FEMDegree >::Evaluator evaluator; + typename BSplineEvaluationData< FEMDegree >::ChildEvaluator childEvaluator; + Stencil< double , BSplineEvaluationData< FEMDegree >::SupportSize > cellStencil; + Stencil< double , BSplineEvaluationData< FEMDegree >::SupportSize > cellStencils [CHILDREN]; + Stencil< double , BSplineEvaluationData< FEMDegree >::SupportSize > edgeStencil [Cube::EDGES ]; + Stencil< double , BSplineEvaluationData< FEMDegree >::SupportSize > edgeStencils [CHILDREN][Cube::EDGES ]; + Stencil< double , BSplineEvaluationData< FEMDegree >::SupportSize > faceStencil [Cube::FACES ]; + Stencil< double , BSplineEvaluationData< FEMDegree >::SupportSize > faceStencils [CHILDREN][Cube::FACES ]; + Stencil< double , BSplineEvaluationData< FEMDegree >::SupportSize > cornerStencil [Cube::CORNERS]; + Stencil< double , BSplineEvaluationData< FEMDegree >::SupportSize > cornerStencils[CHILDREN][Cube::CORNERS]; + + Stencil< Point3D< double > , BSplineEvaluationData< FEMDegree >::SupportSize > dCellStencil; + Stencil< Point3D< double > , BSplineEvaluationData< FEMDegree >::SupportSize > dCellStencils [CHILDREN]; + Stencil< Point3D< double > , BSplineEvaluationData< FEMDegree >::SupportSize > dEdgeStencil [Cube::EDGES ]; + Stencil< Point3D< double > , BSplineEvaluationData< FEMDegree >::SupportSize > dEdgeStencils [CHILDREN][Cube::EDGES ]; + Stencil< Point3D< double > , BSplineEvaluationData< FEMDegree >::SupportSize > dFaceStencil [Cube::FACES ]; + Stencil< Point3D< double > , BSplineEvaluationData< FEMDegree >::SupportSize > dFaceStencils [CHILDREN][Cube::FACES ]; + Stencil< Point3D< double > , BSplineEvaluationData< FEMDegree >::SupportSize > dCornerStencil [Cube::CORNERS]; + Stencil< Point3D< double > , BSplineEvaluationData< FEMDegree >::SupportSize > dCornerStencils[CHILDREN][Cube::CORNERS]; + void set( int depth , bool dirichlet ); + }; + template< class V , int FEMDegree > + V _getCenterValue( const ConstPointSupportKey< FEMDegree >& neighborKey , const TreeOctNode* node , const DenseNodeData< V , FEMDegree >& solution , const DenseNodeData< V , FEMDegree >& metSolution , const _Evaluator< FEMDegree >& evaluator , bool isInterior ) const; + template< class V , int FEMDegree > + V _getCornerValue( const ConstPointSupportKey< FEMDegree >& neighborKey , const TreeOctNode* node , int corner , const DenseNodeData< V , FEMDegree >& solution , const DenseNodeData< V , FEMDegree >& metSolution , const _Evaluator< FEMDegree >& evaluator , bool isInterior ) const; + template< class V , int FEMDegree > + V _getEdgeValue ( const ConstPointSupportKey< FEMDegree >& neighborKey , const TreeOctNode* node , int edge , const DenseNodeData< V , FEMDegree >& solution , const DenseNodeData< V , FEMDegree >& metSolution , const _Evaluator< FEMDegree >& evaluator , bool isInterior ) const; + + template< int FEMDegree > + std::pair< Real , Point3D< Real > > _getCornerValueAndGradient( const ConstPointSupportKey< FEMDegree >& neighborKey , const TreeOctNode* node , int corner , const DenseNodeData< Real , FEMDegree >& solution , const DenseNodeData< Real , FEMDegree >& metSolution , const _Evaluator< FEMDegree >& evaluator , bool isInterior ) const; + template< int FEMDegree > + std::pair< Real , Point3D< Real > > _getEdgeValueAndGradient ( const ConstPointSupportKey< FEMDegree >& neighborKey , const TreeOctNode* node , int edge , const DenseNodeData< Real , FEMDegree >& solution , const DenseNodeData< Real , FEMDegree >& metSolution , const _Evaluator< FEMDegree >& evaluator , bool isInterior ) const; + + //////////////////////////////////////// + // Iso-Surfacing Methods // + // MultiGridOctreeData.IsoSurface.inl // + //////////////////////////////////////// + struct IsoEdge + { + long long edges[2]; + IsoEdge( void ){ edges[0] = edges[1] = 0; } + IsoEdge( long long v1 , long long v2 ){ edges[0] = v1 , edges[1] = v2; } + long long& operator[]( int idx ){ return edges[idx]; } + const long long& operator[]( int idx ) const { return edges[idx]; } + }; + struct FaceEdges + { + IsoEdge edges[2]; + int count; + }; + template< class Vertex > + struct SliceValues + { + typename SortedTreeNodes::SliceTableData sliceData; + Pointer( Real ) cornerValues ; Pointer( Point3D< Real > ) cornerGradients ; Pointer( char ) cornerSet; + Pointer( long long ) edgeKeys ; Pointer( char ) edgeSet; + Pointer( FaceEdges ) faceEdges ; Pointer( char ) faceSet; + Pointer( char ) mcIndices; + hash_map< long long , std::vector< IsoEdge > > faceEdgeMap; + hash_map< long long , std::pair< int , Vertex > > edgeVertexMap; + hash_map< long long , long long > vertexPairMap; + + SliceValues( void ); + ~SliceValues( void ); + void reset( bool nonLinearFit ); + protected: + int _oldCCount , _oldECount , _oldFCount , _oldNCount; + }; + template< class Vertex > + struct XSliceValues + { + typename SortedTreeNodes::XSliceTableData xSliceData; + Pointer( long long ) edgeKeys ; Pointer( char ) edgeSet; + Pointer( FaceEdges ) faceEdges ; Pointer( char ) faceSet; + hash_map< long long , std::vector< IsoEdge > > faceEdgeMap; + hash_map< long long , std::pair< int , Vertex > > edgeVertexMap; + hash_map< long long , long long > vertexPairMap; + + XSliceValues( void ); + ~XSliceValues( void ); + void reset( void ); + protected: + int _oldECount , _oldFCount; + }; + template< class Vertex > + struct SlabValues + { + XSliceValues< Vertex > _xSliceValues[2]; + SliceValues< Vertex > _sliceValues[2]; + SliceValues< Vertex >& sliceValues( int idx ){ return _sliceValues[idx&1]; } + const SliceValues< Vertex >& sliceValues( int idx ) const { return _sliceValues[idx&1]; } + XSliceValues< Vertex >& xSliceValues( int idx ){ return _xSliceValues[idx&1]; } + const XSliceValues< Vertex >& xSliceValues( int idx ) const { return _xSliceValues[idx&1]; } + }; + template< class Vertex , int FEMDegree > + void SetSliceIsoCorners( const DenseNodeData< Real , FEMDegree >& solution , const DenseNodeData< Real , FEMDegree >& coarseSolution , Real isoValue , int depth , int slice , std::vector< SlabValues< Vertex > >& sValues , const _Evaluator< FEMDegree >& evaluator , int threads ); + template< class Vertex , int FEMDegree > + void SetSliceIsoCorners( const DenseNodeData< Real , FEMDegree >& solution , const DenseNodeData< Real , FEMDegree >& coarseSolution , Real isoValue , int depth , int slice , int z , std::vector< SlabValues< Vertex > >& sValues , const _Evaluator< FEMDegree >& evaluator , int threads ); + template< int WeightDegree , int ColorDegree , class Vertex > + void SetSliceIsoVertices( const BSplineData< ColorDegree >* colorBSData , const SparseNodeData< Real , WeightDegree >* densityWeights , const SparseNodeData< ProjectiveData< Point3D< Real > > , ColorDegree >* colorData , Real isoValue , int depth , int slice , int& vOffset , CoredMeshData< Vertex >& mesh , std::vector< SlabValues< Vertex > >& sValues , int threads ); + template< int WeightDegree , int ColorDegree , class Vertex > + void SetSliceIsoVertices( const BSplineData< ColorDegree >* colorBSData , const SparseNodeData< Real , WeightDegree >* densityWeights , const SparseNodeData< ProjectiveData< Point3D< Real > > , ColorDegree >* colorData , Real isoValue , int depth , int slice , int z , int& vOffset , CoredMeshData< Vertex >& mesh , std::vector< SlabValues< Vertex > >& sValues , int threads ); + template< int WeightDegree , int ColorDegree , class Vertex > + void SetXSliceIsoVertices( const BSplineData< ColorDegree >* colorBSData , const SparseNodeData< Real , WeightDegree >* densityWeights , const SparseNodeData< ProjectiveData< Point3D< Real > > , ColorDegree >* colorData , Real isoValue , int depth , int slab , int& vOffset , CoredMeshData< Vertex >& mesh , std::vector< SlabValues< Vertex > >& sValues , int threads ); + template< class Vertex > + void CopyFinerSliceIsoEdgeKeys( int depth , int slice , std::vector< SlabValues< Vertex > >& sValues , int threads ); + template< class Vertex > + void CopyFinerSliceIsoEdgeKeys( int depth , int slice , int z , std::vector< SlabValues< Vertex > >& sValues , int threads ); + template< class Vertex > + void CopyFinerXSliceIsoEdgeKeys( int depth , int slab , std::vector< SlabValues< Vertex > >& sValues , int threads ); + template< class Vertex > + void SetSliceIsoEdges( int depth , int slice , std::vector< SlabValues< Vertex > >& slabValues , int threads ); + template< class Vertex > + void SetSliceIsoEdges( int depth , int slice , int z , std::vector< SlabValues< Vertex > >& slabValues , int threads ); + template< class Vertex > + void SetXSliceIsoEdges( int depth , int slice , std::vector< SlabValues< Vertex > >& slabValues , int threads ); + + template< class Vertex > + void SetIsoSurface( int depth , int offset , const SliceValues< Vertex >& bValues , const SliceValues< Vertex >& fValues , const XSliceValues< Vertex >& xValues , CoredMeshData< Vertex >& mesh , bool polygonMesh , bool addBarycenter , int& vOffset , int threads ); + + template< class Vertex > + static int AddIsoPolygons( CoredMeshData< Vertex >& mesh , std::vector< std::pair< int , Vertex > >& polygon , bool polygonMesh , bool addBarycenter , int& vOffset ); + + template< int WeightDegree , int ColorDegree , class Vertex > + bool GetIsoVertex( const BSplineData< ColorDegree >* colorBSData , const SparseNodeData< Real , WeightDegree >* densityWeights , const SparseNodeData< ProjectiveData< Point3D< Real > > , ColorDegree >* colorData , Real isoValue , ConstPointSupportKey< WeightDegree >& weightKey , ConstPointSupportKey< ColorDegree >& colorKey , const TreeOctNode* node , int edgeIndex , int z , const SliceValues< Vertex >& sValues , Vertex& vertex ); + template< int WeightDegree , int ColorDegree , class Vertex > + bool GetIsoVertex( const BSplineData< ColorDegree >* colorBSData , const SparseNodeData< Real , WeightDegree >* densityWeights , const SparseNodeData< ProjectiveData< Point3D< Real > > , ColorDegree >* colorData , Real isoValue , ConstPointSupportKey< WeightDegree >& weightKey , ConstPointSupportKey< ColorDegree >& colorKey , const TreeOctNode* node , int cornerIndex , const SliceValues< Vertex >& bValues , const SliceValues< Vertex >& fValues , Vertex& vertex ); + +public: + static double maxMemoryUsage; + int threads; + + static double MemoryUsage( void ); + Octree( void ); + + // After calling set tree, the indices of the octree node will be stored by depth, and within depth they will be sorted by slice + template< class PointReal , int NormalDegree , int WeightDegree , int DataDegree , class Data , class _Data > + int SetTree( OrientedPointStream< PointReal >* pointStream , int minDepth , int maxDepth , int fullDepth , int splatDepth , Real samplesPerNode , + Real scaleFactor , bool useConfidence , bool useNormalWeight , + Real constraintWeight , int adaptiveExponent , + SparseNodeData< Real , WeightDegree >& densityWeights , SparseNodeData< PointData< Real > , 0 >& pointInfo , SparseNodeData< Point3D< Real > , NormalDegree >& normalInfo , SparseNodeData< Real , NormalDegree >& nodeWeights , + SparseNodeData< ProjectiveData< _Data > , DataDegree >* dataValues , + XForm4x4< Real >& xForm , bool dirichlet=false , bool makeComplete=false ); + + template< int FEMDegree > void EnableMultigrid( std::vector< int >* map ); + + template< int FEMDegree , int NormalDegree > + DenseNodeData< Real , FEMDegree > SetLaplacianConstraints( const SparseNodeData< Point3D< Real > , NormalDegree >& normalInfo ); + template< int FEMDegree > + DenseNodeData< Real , FEMDegree > SolveSystem( SparseNodeData< PointData< Real > , 0 >& pointInfo , DenseNodeData< Real , FEMDegree >& constraints , bool showResidual , int iters , int maxSolveDepth , int cgDepth=0 , double cgAccuracy=0 ); + + template< int FEMDegree , int NormalDegree > + Real GetIsoValue( const DenseNodeData< Real , FEMDegree >& solution , const SparseNodeData< Real , NormalDegree >& nodeWeights ); + template< int FEMDegree , int WeightDegree , int ColorDegree , class Vertex > + void GetMCIsoSurface( const SparseNodeData< Real , WeightDegree >* densityWeights , const SparseNodeData< ProjectiveData< Point3D< Real > > , ColorDegree >* colorData , const DenseNodeData< Real , FEMDegree >& solution , Real isoValue , CoredMeshData< Vertex >& mesh , bool nonLinearFit=true , bool addBarycenter=false , bool polygonMesh=false ); + + const TreeOctNode& tree( void ) const{ return _tree; } + size_t leaves( void ) const { return _tree.leaves(); } + size_t nodes( void ) const { return _tree.nodes(); } +}; + +template< class Real > +void Reset( void ) +{ + TreeNodeData::NodeCount=0; + Octree< Real >::maxMemoryUsage = 0; +} + +#include "MultiGridOctreeData.inl" +#include "MultiGridOctreeData.SortedTreeNodes.inl" +#include "MultiGridOctreeData.WeightedSamples.inl" +#include "MultiGridOctreeData.System.inl" +#include "MultiGridOctreeData.IsoSurface.inl" +#include "MultiGridOctreeData.Evaluation.inl" +#endif // MULTI_GRID_OCTREE_DATA_INCLUDED diff --git a/colmap/include/colmap/lib/PoissonRecon/MultiGridOctreeData.inl b/colmap/include/colmap/lib/PoissonRecon/MultiGridOctreeData.inl new file mode 100644 index 0000000000000000000000000000000000000000..e88193414fe6c85b3031b3181e6b7f2005cdebdc --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/MultiGridOctreeData.inl @@ -0,0 +1,557 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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 "PointStream.h" + +#define ITERATION_POWER 1.0/3 +#define MEMORY_ALLOCATOR_BLOCK_SIZE 1<<12 + + +const double MATRIX_ENTRY_EPSILON = 0; +const double EPSILON = 1e-6; +const double ROUND_EPS = 1e-5; + + + +////////////////// +// TreeNodeData // +////////////////// +size_t TreeNodeData::NodeCount = 0; +TreeNodeData::TreeNodeData( void ){ nodeIndex = (int)NodeCount ; NodeCount++; } +TreeNodeData::~TreeNodeData( void ) { } + + +//////////// +// Octree // +//////////// +template< class Real > double Octree< Real >::maxMemoryUsage=0; + +template< class Real > +double Octree< Real >::MemoryUsage( void ) +{ + double mem = double( MemoryInfo::Usage() ) / (1<<20); + if( mem>maxMemoryUsage ) maxMemoryUsage=mem; + return mem; +} + +template< class Real > +Octree< Real >::Octree( void ) +{ + threads = 1; + _constrainValues = false; + _multigridDegree = 0; +} +template< class Real > +template< int FEMDegree > +void Octree< Real >::FunctionIndex( const TreeOctNode* node , int idx[3] ) +{ + int d; + node->depthAndOffset( d , idx ); + for( int dd=0 ; dd::FunctionIndex( d-1 , idx[dd] ); +} + +template< class Real > bool Octree< Real >::_InBounds( Point3D< Real > p ) const { return p[0]>=Real(0.) && p[0]<=Real(1.0) && p[1]>=Real(0.) && p[1]<=Real(1.0) && p[2]>=Real(0.) && p[2]<=Real(1.0); } +template< class Real > +template< int FEMDegree > +bool Octree< Real >::IsValidNode( const TreeOctNode* node , bool dirichlet ) +{ + if( !node || node->depth()<1 ) return false; + int d , off[3]; + node->depthAndOffset( d , off ); + int dim = BSplineData< FEMDegree >::Dimension( d-1 ); + if( FEMDegree&1 && dirichlet ) return !( off[0]<=0 || off[1]<=0 || off[2]<=0 || off[0]>=dim-1 || off[1]>=dim-1 || off[2]>=dim-1 ); + else return !( off[0]< 0 || off[1]< 0 || off[2]< 0 || off[0]> dim-1 || off[1]> dim-1 || off[2]> dim-1 ); +} +template< class Real > +void Octree< Real >::_SetFullDepth( TreeOctNode* node , int depth ) +{ + if( node->depth()==0 || _Depth( node )children ) node->initChildren(); + for( int c=0 ; cchildren+c , depth ); + } +} +template< class Real > +void Octree< Real >::_setFullDepth( int depth ) +{ + if( !_tree.children ) _tree.initChildren(); + for( int c=0 ; c +template< class PointReal , int NormalDegree , int WeightDegree , int DataDegree , class Data , class _Data > +int Octree< Real >::SetTree( OrientedPointStream< PointReal >* pointStream , int minDepth , int maxDepth , int fullDepth , + int splatDepth , Real samplesPerNode , Real scaleFactor , + bool useConfidence , bool useNormalWeights , Real constraintWeight , int adaptiveExponent , + SparseNodeData< Real , WeightDegree >& densityWeights , SparseNodeData< PointData< Real > , 0 >& pointInfo , SparseNodeData< Point3D< Real > , NormalDegree >& normalInfo , SparseNodeData< Real , NormalDegree >& nodeWeights , + SparseNodeData< ProjectiveData< _Data > , DataDegree >* dataValues , + XForm4x4< Real >& xForm , bool dirichlet , bool makeComplete ) +{ + OrientedPointStreamWithData< PointReal , Data >* pointStreamWithData = ( OrientedPointStreamWithData< PointReal , Data >* )pointStream; + _tree.initChildren() , _spaceRoot = _tree.children; + splatDepth = std::max< int >( 0 , std::min< int >( splatDepth , maxDepth ) ); + + _dirichlet = dirichlet; + _constrainValues = (constraintWeight>0); + + XForm3x3< Real > xFormN; + for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) xFormN(i,j) = xForm(i,j); + xFormN = xFormN.transpose().inverse(); + minDepth = std::max< int >( 0 , std::min< int >( minDepth , maxDepth ) ); // 0<=minDepth <= maxDepth + fullDepth = std::max< int >( minDepth , std::min< int >( fullDepth , maxDepth ) ); // minDepth <= fullDepth <= maxDepth + +#if 0 + // For Neumann constraints, the function at depth 0 is constant so the system matrix is zero if there is no screening. + if( !_dirichlet && !_constrainValues ) minDepth = std::max< int >( minDepth , 1 ); +#endif + minDepth++ , maxDepth++; + + _minDepth = minDepth; + _fullDepth = fullDepth; + _splatDepth = splatDepth; + double pointWeightSum = 0; + int i , cnt=0; + + PointSupportKey< WeightDegree > weightKey; + PointSupportKey< DataDegree > dataKey; + PointSupportKey< NormalDegree > normalKey; + weightKey.set( maxDepth ) , dataKey.set( maxDepth ) , normalKey.set( maxDepth ); + + _setFullDepth( _fullDepth ); + + // Read through once to get the center and scale + { + Point3D< Real > min , max; + double t = Time(); + Point3D< Real > p; + OrientedPoint3D< PointReal > _p; + while( pointStream->nextPoint( _p ) ) + { + p = xForm * Point3D< Real >(_p.p); + for( i=0 ; imax[i] ) max[i] = p[i]; + } + cnt++; + } + + _scale = std::max< Real >( max[0]-min[0] , std::max< Real >( max[1]-min[1] , max[2]-min[2] ) ); + _center = ( max+min ) /2; + } + + _scale *= scaleFactor; + for( i=0 ; i trans = XForm4x4< Real >::Identity() , scale = XForm4x4< Real >::Identity(); + for( int i=0 ; i<3 ; i++ ) scale(i,i) = (Real)(1./_scale ) , trans(3,i) = -_center[i]; + xForm = scale * trans * xForm; + } + + { + double t = Time(); + cnt = 0; + pointStream->reset(); + Point3D< Real > p , n; + OrientedPoint3D< PointReal > _p; + while( pointStream->nextPoint( _p ) ) + { + p = xForm * Point3D< Real >(_p.p) , n = xFormN * Point3D< Real >(_p.n); + if( !_InBounds(p) ) continue; + Point3D< Real > myCenter = Point3D< Real >( Real(0.5) , Real(0.5) , Real(0.5) ); + Real myWidth = Real(1.0); + Real weight=Real( 1. ); + if( useConfidence ) weight = Real( Length(n) ); + if( samplesPerNode>0 ) weight /= (Real)samplesPerNode; + TreeOctNode* temp = _spaceRoot; + int d=0; + while( dchildren ) temp->initChildren(); + int cIndex=TreeOctNode::CornerIndex( myCenter , p ); + temp = temp->children + cIndex; + myWidth/=2; + if( cIndex&1 ) myCenter[0] += myWidth/2; + else myCenter[0] -= myWidth/2; + if( cIndex&2 ) myCenter[1] += myWidth/2; + else myCenter[1] -= myWidth/2; + if( cIndex&4 ) myCenter[2] += myWidth/2; + else myCenter[2] -= myWidth/2; + d++; + } + _AddWeightContribution( densityWeights , temp , p , weightKey , weight ); + cnt++; + } + } + + std::vector< PointData< Real > >& points = pointInfo.data; + + cnt = 0; + pointStream->reset(); + Point3D< Real > p , n; + OrientedPoint3D< PointReal > _p; + Data _d; + while( ( dataValues ? pointStreamWithData->nextPoint( _p , _d ) : pointStream->nextPoint( _p ) ) ) + { + p = xForm * Point3D< Real >(_p.p) , n = xFormN * Point3D< Real >(_p.n); + n *= Real(-1.); + if( !_InBounds(p) ) continue; + Real normalLength = Real( Length( n ) ); + if(std::isnan( normalLength ) || !std::isfinite( normalLength ) || normalLength<=EPSILON ) continue; + if( !useConfidence ) n /= normalLength; + + Real pointWeight = Real(1.f); + if( samplesPerNode>0 ) + { + if( dataValues ) _MultiSplatPointData( &densityWeights , p , ProjectiveData< _Data >( _Data( _d ) , (Real)1. ) , *dataValues , weightKey , dataKey , maxDepth-1 , 2 ); + pointWeight = _SplatPointData( densityWeights , p , n , normalInfo , weightKey , normalKey , _minDepth-1 , maxDepth-1 , 3 ); + } + else + { + if( dataValues ) _MultiSplatPointData( ( SparseNodeData< Real , WeightDegree >* )NULL , p , ProjectiveData< _Data >( _Data( _d ) , (Real)1. ) , *dataValues , weightKey , dataKey , maxDepth-1 , 2 ); + + Point3D< Real > myCenter = Point3D< Real >( Real(0.5) , Real(0.5) , Real(0.5) ); + Real myWidth = Real(1.0); + TreeOctNode* temp = _spaceRoot; + int d=0; + if( splatDepth ) + { + while( dchildren[cIndex]; + myWidth /= 2; + if(cIndex&1) myCenter[0] += myWidth/2; + else myCenter[0] -= myWidth/2; + if(cIndex&2) myCenter[1] += myWidth/2; + else myCenter[1] -= myWidth/2; + if(cIndex&4) myCenter[2] += myWidth/2; + else myCenter[2] -= myWidth/2; + d++; + } + pointWeight = (Real)1.0/_GetSamplesPerNode( densityWeights , temp , p , weightKey ); + } + for( i=0 ; ichildren ) temp->initChildren(); + int cIndex=TreeOctNode::CornerIndex( myCenter , p ); + temp=&temp->children[cIndex]; + myWidth/=2; + if(cIndex&1) myCenter[0] += myWidth/2; + else myCenter[0] -= myWidth/2; + if(cIndex&2) myCenter[1] += myWidth/2; + else myCenter[1] -= myWidth/2; + if(cIndex&4) myCenter[2] += myWidth/2; + else myCenter[2] -= myWidth/2; + d++; + } + _SplatPointData( temp , p , n , normalInfo , normalKey ); + } + pointWeightSum += pointWeight; + if( _constrainValues ) + { + Real pointScreeningWeight = useNormalWeights ? Real( normalLength ) : Real(1.f); + TreeOctNode* temp = _spaceRoot; + Point3D< Real > myCenter = Point3D< Real >( Real(0.5) , Real(0.5) , Real(0.5) ); + Real myWidth = Real(1.0); + while( 1 ) + { + if( (int)pointInfo.indices.size()( p*pointScreeningWeight , pointScreeningWeight ) ); + pointInfo.indices[ temp->nodeData.nodeIndex ] = idx; + } + else + { + points[idx].position += p*pointScreeningWeight; + points[idx].weight += pointScreeningWeight; + } + + int cIndex = TreeOctNode::CornerIndex( myCenter , p ); + if( !temp->children ) break; + temp = &temp->children[cIndex]; + myWidth /= 2; + if( cIndex&1 ) myCenter[0] += myWidth/2; + else myCenter[0] -= myWidth/2; + if( cIndex&2 ) myCenter[1] += myWidth/2; + else myCenter[1] -= myWidth/2; + if( cIndex&4 ) myCenter[2] += myWidth/2; + else myCenter[2] -= myWidth/2; + } + } + cnt++; + } + + constraintWeight *= Real( pointWeightSum ); + constraintWeight /= cnt; + + MemoryUsage( ); + if( _constrainValues ) + // Set the average position and scale the weights + for( TreeOctNode* node=_tree.nextNode() ; node ; node=_tree.nextNode(node) ) + if( pointInfo.index( node )!=-1 ) + { + int idx = pointInfo.index( node ); + points[idx].position /= points[idx].weight; + int e = _Depth( node ) * adaptiveExponent - ( maxDepth - 1 ) * (adaptiveExponent-1); + if( e<0 ) points[idx].weight /= Real( 1<<(-e) ); + else points[idx].weight *= Real( 1<< e ); + points[idx].weight *= Real( constraintWeight ); + } +#if FORCE_NEUMANN_FIELD +// #pragma message( "[WARNING] This is likely wrong as it only forces the normal component of the coefficient to be zero, not the actual vector-field" ) + if( !_dirichlet ) + for( TreeOctNode* node=_tree.nextNode() ; node ; node=_tree.nextNode( node ) ) + { + int d , off[3] , res; + node->depthAndOffset( d , off ); + res = 1<& normal = normalInfo.data[ idx ]; + for( int d=0 ; d<3 ; d++ ) if( off[d]==0 || off[d]==res-1 ) normal[d] = 0; + } +#endif // FORCE_NEUMANN_FIELD + nodeWeights.resize( TreeNodeData::NodeCount ); + // Set the point weights for evaluating the iso-value + for( TreeOctNode* node=_tree.nextNode() ; node ; node=_tree.nextNode(node) ) + { + int nIdx = normalInfo.index( node ); + if( nIdx>=0 ) + { + Real l = Real( Length( normalInfo.data[ nIdx ] ) ); + if( l ) + { + int nIdx = nodeWeights.index( node ); + if( nIdx<0 ) + { + nodeWeights.indices[ node->nodeData.nodeIndex ] = (int)nodeWeights.data.size(); + nodeWeights.data.push_back( l ); + } + else nodeWeights.data[ nIdx ] = l; + } + } + } + MemoryUsage(); + if( makeComplete ) _MakeComplete( ); + else _ClipTree< NormalDegree >( normalInfo ); + _maxDepth = _tree.maxDepth(); + return cnt; +} + +template< class Real > +void Octree< Real >::_SetValidityFlags( void ) +{ + for( int i=0 ; i<_sNodes.end( _sNodes.levels()-1 ) ; i++ ) + { + _sNodes.treeNodes[i]->nodeData.flags = 0; + if( IsValidNode< 0 >( _sNodes.treeNodes[i] , _dirichlet ) ) _sNodes.treeNodes[i]->nodeData.flags |= (1<<0); + if( IsValidNode< 1 >( _sNodes.treeNodes[i] , _dirichlet ) ) _sNodes.treeNodes[i]->nodeData.flags |= (1<<1); + } +} +template< class Real > void Octree< Real >::_MakeComplete( void ){ _tree.setFullDepth( _spaceRoot->maxDepth() ) ; MemoryUsage(); } +// Trim off the branches of the tree (finer than _fullDepth) that don't contain normal points +template< class Real > +template< int NormalDegree > +void Octree< Real >::_ClipTree( const SparseNodeData< Point3D< Real > , NormalDegree >& normalInfo ) +{ +#if NEW_NEW_CODE +#define ABS_INDEX( idx ) ( (idx<0) ? -(idx) : (idx) ) + static const int SupportSize = BSplineEvaluationData< NormalDegree >::SupportSize; + static const int LeftSupportRadius = -BSplineEvaluationData< NormalDegree >::SupportStart; + static const int RightSupportRadius = BSplineEvaluationData< NormalDegree >::SupportEnd; + int maxDepth = _tree.maxDepth(); + typename TreeOctNode::NeighborKey< LeftSupportRadius , RightSupportRadius > neighborKey; + neighborKey.set( maxDepth ); + + // Set all nodes to invalid (negative indices) + for( TreeOctNode* node=_tree.nextNode() ; node ; node=_tree.nextNode(node) ) node->nodeData.nodeIndex = -node->nodeData.nodeIndex; + + // Iterate over the nodes and, if they contain a normal, make sure that the supported nodes exist and are set to valid + for( TreeOctNode* node=_tree.nextNode() ; node ; node=_tree.nextNode(node) ) if( normalInfo.index( ABS_INDEX( node->nodeData.nodeIndex ) )>=0 ) + { + int depth = node->depth(); + neighborKey.getNeighbors< true >( node ); + for( int d=0 ; d<=depth ; d++ ) + { + TreeOctNode::template Neighbors< SupportSize >& neighbors = neighborKey.neighbors[d]; + for( int i=0 ; inodeData.nodeIndex = ABS_INDEX( neighbors.neighbors[i][j][k]->nodeData.nodeIndex ); + } + } + + // Remove the invalid nodes + for( TreeOctNode* node=_tree.nextNode() ; node ; node=_tree.nextNode( node ) ) + { + if( node->children && _Depth(node)>=_fullDepth ) + { + bool hasValidChildren = false; + for( int c=0 ; cchildren[c].nodeData.nodeIndex>0 ); + if( !hasValidChildren ) node->children = NULL; + } + node->nodeData.nodeIndex = ABS_INDEX( node->nodeData.nodeIndex ); + } + + MemoryUsage(); +#undef ABS_INDEX +#else // !NEW_NEW_CODE + int maxDepth = _tree.maxDepth(); + for( TreeOctNode* temp=_tree.nextNode() ; temp ; temp=_tree.nextNode(temp) ) + if( temp->children && _Depth( temp )>=_fullDepth ) + { + int hasNormals=0; + for( int i=0 ; ichildren[i] , normalInfo ); + if( !hasNormals ) temp->children=NULL; + } + MemoryUsage(); +#endif // NEW_NEW_CODE +} + +template< class Real > +template< int FEMDegree > +void Octree< Real >::EnableMultigrid( std::vector< int >* map ) +{ + if( FEMDegree<=_multigridDegree ) return; + _multigridDegree = FEMDegree; + const int OverlapRadius = -BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapStart; + int maxDepth = _tree.maxDepth( ); + typename TreeOctNode::NeighborKey< OverlapRadius , OverlapRadius > neighborKey; + neighborKey.set( maxDepth-1 ); + for( int d=maxDepth-1 ; d>=0 ; d-- ) + for( TreeOctNode* node=_tree.nextNode() ; node ; node=_tree.nextNode( node ) ) if( node->depth()==d && node->children ) + neighborKey.template getNeighbors< true >( node ); + _sNodes.set( _tree , map ); + _SetValidityFlags(); +} + +template< class Real > +template< int NormalDegree > +int Octree< Real >::_HasNormals( TreeOctNode* node , const SparseNodeData< Point3D< Real > , NormalDegree >& normalInfo ) +{ + int idx = normalInfo.index( node ); + if( idx>=0 ) + { + const Point3D< Real >& normal = normalInfo.data[ idx ]; + if( normal[0]!=0 || normal[1]!=0 || normal[2]!=0 ) return 1; + } + if( node->children ) for( int i=0 ; ichildren[i] , normalInfo ) ) return 1; + return 0; +} + +//////////////// +// VertexData // +//////////////// +long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth) +{ + int idx[DIMENSION]; + return CenterIndex(node,maxDepth,idx); +} +long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth,int idx[DIMENSION]) +{ + int d,o[3]; + node->depthAndOffset(d,o); + for(int i=0;idepthAndOffset( d , o ); + for( int i=0 ; idepthAndOffset(d,o); + for(int i=0;idepthAndOffset( d ,off ); + Cube::FactorEdgeIndex( eIndex , o , i1 , i2 ); + for( int i=0 ; i +#include +#ifndef _WIN32 +#include +#endif // _WIN32 + +inline double Time( void ) +{ +#ifdef _WIN32 + struct _timeb t; + _ftime( &t ); + return double( t.time ) + double( t.millitm ) / 1000.0; +#else // _WIN32 + struct timeval t; + gettimeofday( &t , NULL ); + return t.tv_sec + double( t.tv_usec ) / 1000000; +#endif // _WIN32 +} + +#endif // MY_TIME_INCLUDED diff --git a/colmap/include/colmap/lib/PoissonRecon/Octree.h b/colmap/include/colmap/lib/PoissonRecon/Octree.h new file mode 100644 index 0000000000000000000000000000000000000000..9d76975415ac5c84bb1327e5d8de84ff90d32c20 --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/Octree.h @@ -0,0 +1,175 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +#ifndef OCT_NODE_INCLUDED +#define OCT_NODE_INCLUDED + +#include "Allocator.h" +#include "BinaryNode.h" +#include "MarchingCubes.h" + +#define NEW_OCTNODE_CODE 1 + +#define DIMENSION 3 + +template< class NodeData > +class OctNode +{ +private: + static int UseAlloc; + unsigned long long _depthAndOffset; + + const OctNode* __faceNeighbor( int dir , int off ) const; + const OctNode* __edgeNeighbor( int o , const int i[2] , const int idx[2] ) const; + OctNode* __faceNeighbor( int dir , int off , int forceChildren ); + OctNode* __edgeNeighbor( int o , const int i[2] , const int idx[2] , int forceChildren); +public: + static const int DepthShift , OffsetShift , OffsetShift1 , OffsetShift2 , OffsetShift3; + static const int DepthMask , OffsetMask; + + static Allocator< OctNode > NodeAllocator; + static int UseAllocator( void ); + static void SetAllocator( int blockSize ); + + OctNode* parent; + OctNode* children; + NodeData nodeData; + + OctNode( void ); + ~OctNode( void ); + int initChildren( void ); + + void depthAndOffset( int& depth , int offset[DIMENSION] ) const; + void centerIndex( int index[DIMENSION] ) const; + int depth( void ) const; + static inline void DepthAndOffset( const long long& index , int& depth , int offset[DIMENSION] ); + template< class Real > static inline void CenterAndWidth( const long long& index , Point3D< Real >& center , Real& width ); + template< class Real > static inline void StartAndWidth( const long long& index , Point3D< Real >& start , Real& width ); + static inline int Depth( const long long& index ); + static inline void Index( int depth , const int offset[3] , short& d , short off[DIMENSION] ); + static inline unsigned long long Index( int depth , const int offset[3] ); + template< class Real > void centerAndWidth( Point3D& center , Real& width ) const; + template< class Real > void startAndWidth( Point3D< Real >& start , Real& width ) const; + template< class Real > bool isInside( Point3D< Real > p ) const; + + size_t leaves( void ) const; + size_t maxDepthLeaves( int maxDepth ) const; + size_t nodes( void ) const; + int maxDepth( void ) const; + + const OctNode* root( void ) const; + + const OctNode* nextLeaf( const OctNode* currentLeaf=NULL ) const; + OctNode* nextLeaf( OctNode* currentLeaf=NULL ); + const OctNode* nextNode( const OctNode* currentNode=NULL ) const; + OctNode* nextNode( OctNode* currentNode=NULL ); + const OctNode* nextBranch( const OctNode* current ) const; + OctNode* nextBranch( OctNode* current ); + const OctNode* prevBranch( const OctNode* current ) const; + OctNode* prevBranch( OctNode* current ); + + void setFullDepth( int maxDepth ); + + void printLeaves( void ) const; + void printRange( void ) const; + + template< class Real > static int CornerIndex( const Point3D& center , const Point3D &p ); + + OctNode* faceNeighbor( int faceIndex , int forceChildren=0 ); + const OctNode* faceNeighbor( int faceIndex ) const; + OctNode* edgeNeighbor( int edgeIndex , int forceChildren=0 ); + const OctNode* edgeNeighbor( int edgeIndex ) const; + OctNode* cornerNeighbor( int cornerIndex , int forceChildren=0 ); + const OctNode* cornerNeighbor( int cornerIndex ) const; + + int write( const char* fileName ) const; + int write( FILE* fp ) const; + int read( const char* fileName ); + int read( FILE* fp ); + + template< unsigned int Width > + struct Neighbors + { + OctNode* neighbors[Width][Width][Width]; + Neighbors( void ); + void clear( void ); + }; + template< unsigned int Width > + struct ConstNeighbors + { + const OctNode* neighbors[Width][Width][Width]; + ConstNeighbors( void ); + void clear( void ); + }; + + template< unsigned int LeftRadius , unsigned int RightRadius > + class NeighborKey + { + int _depth; + public: + static const int Width = LeftRadius + RightRadius + 1; + Neighbors< Width >* neighbors; + + NeighborKey( void ); + NeighborKey( const NeighborKey& key ); + ~NeighborKey( void ); + int depth( void ) const { return _depth; } + + void set( int depth ); + template< bool CreateNodes > typename OctNode< NodeData >::template Neighbors< LeftRadius+RightRadius+1 >& getNeighbors( OctNode* node ); + template< bool CreateNodes , unsigned int _LeftRadius , unsigned int _RightRadius > void getNeighbors( OctNode* node , Neighbors< _LeftRadius + _RightRadius + 1 >& neighbors ); + template< bool CreateNodes > bool getChildNeighbors( int cIdx , int d , Neighbors< Width >& childNeighbors ) const; + template< bool CreateNodes , class Real > bool getChildNeighbors( Point3D< Real > p , int d , Neighbors< Width >& childNeighbors ) const; + }; + + template< unsigned int LeftRadius , unsigned int RightRadius > + class ConstNeighborKey + { + int _depth; + public: + static const int Width = LeftRadius + RightRadius + 1; + ConstNeighbors* neighbors; + + ConstNeighborKey( void ); + ConstNeighborKey( const ConstNeighborKey& key ); + ~ConstNeighborKey( void ); + int depth( void ) const { return _depth; } + + void set( int depth ); + typename OctNode< NodeData >::template ConstNeighbors< LeftRadius+RightRadius+1 >& getNeighbors( const OctNode* node ); + template< unsigned int _LeftRadius , unsigned int _RightRadius > void getNeighbors( const OctNode* node , ConstNeighbors< _LeftRadius + _RightRadius + 1 >& neighbors ); + }; + + void centerIndex( int maxDepth , int index[DIMENSION] ) const; + int width( int maxDepth ) const; +}; + + +#include "Octree.inl" + +#endif // OCT_NODE_INCLUDED diff --git a/colmap/include/colmap/lib/PoissonRecon/Octree.inl b/colmap/include/colmap/lib/PoissonRecon/Octree.inl new file mode 100644 index 0000000000000000000000000000000000000000..eb08cd8d00135c31226f26ed4df87eb8ac00da0f --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/Octree.inl @@ -0,0 +1,1068 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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 +#include +#include + +///////////// +// OctNode // +///////////// +template< class NodeData > const int OctNode< NodeData >::DepthShift=5; +template< class NodeData > const int OctNode< NodeData >::OffsetShift = ( sizeof(long long)*8 - DepthShift ) / 3; +template< class NodeData > const int OctNode< NodeData >::DepthMask=(1< const int OctNode< NodeData >::OffsetMask=(1< const int OctNode< NodeData >::OffsetShift1=DepthShift; +template< class NodeData > const int OctNode< NodeData >::OffsetShift2=OffsetShift1+OffsetShift; +template< class NodeData > const int OctNode< NodeData >::OffsetShift3=OffsetShift2+OffsetShift; + +template< class NodeData > int OctNode< NodeData >::UseAlloc=0; +template< class NodeData > Allocator > OctNode< NodeData >::NodeAllocator; + +template< class NodeData > +void OctNode< NodeData >::SetAllocator(int blockSize) +{ + if(blockSize>0) + { + UseAlloc=1; + NodeAllocator.set(blockSize); + } + else{UseAlloc=0;} +} +template< class NodeData > +int OctNode< NodeData >::UseAllocator(void){return UseAlloc;} + +template< class NodeData > +OctNode< NodeData >::OctNode(void){ + parent=children=NULL; + _depthAndOffset = 0; +} + +template< class NodeData > +OctNode< NodeData >::~OctNode(void){ + if(!UseAlloc){if(children){delete[] children;}} + parent=children=NULL; +} +template< class NodeData > +void OctNode< NodeData >::setFullDepth( int maxDepth ) +{ + if( maxDepth ) + { + if( !children ) initChildren(); + for( int i=0 ; i<8 ; i++ ) children[i].setFullDepth( maxDepth-1 ); + } +} + +template< class NodeData > +int OctNode< NodeData >::initChildren( void ) +{ + if( UseAlloc ) children=NodeAllocator.newElements(8); + else + { + if( children ) delete[] children; + children = NULL; + children = new OctNode[Cube::CORNERS]; + } + if( !children ) + { + fprintf(stderr,"Failed to initialize children in OctNode::initChildren\n"); + exit(0); + return 0; + } + int d , off[3]; + depthAndOffset( d , off ); + for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ ) + { + int idx=Cube::CornerIndex(i,j,k); + children[idx].parent = this; + children[idx].children = NULL; + int off2[3]; + off2[0] = (off[0]<<1)+i; + off2[1] = (off[1]<<1)+j; + off2[2] = (off[2]<<1)+k; + children[idx]._depthAndOffset = Index( d+1 , off2 ); + } + return 1; +} +template< class NodeData > +inline void OctNode< NodeData >::Index(int depth,const int offset[3],short& d,short off[3]){ + d=short(depth); + off[0]=short((1< +inline void OctNode< NodeData >::depthAndOffset( int& depth , int offset[DIMENSION] ) const +{ + depth = int( _depthAndOffset & DepthMask ); + offset[0] = int( (_depthAndOffset>>OffsetShift1) & OffsetMask ); + offset[1] = int( (_depthAndOffset>>OffsetShift2) & OffsetMask ); + offset[2] = int( (_depthAndOffset>>OffsetShift3) & OffsetMask ); +} +template< class NodeData > +inline void OctNode< NodeData >::centerIndex( int index[DIMENSION] ) const +{ + int d , off[DIMENSION]; + depthAndOffset( d , off ); + for( int i=0 ; i +inline unsigned long long OctNode< NodeData >::Index( int depth , const int offset[3] ) +{ + unsigned long long idx=0; + idx |= ( ( (unsigned long long)(depth ) ) & DepthMask ); + idx |= ( ( (unsigned long long)(offset[0]) ) & OffsetMask ) << OffsetShift1; + idx |= ( ( (unsigned long long)(offset[1]) ) & OffsetMask ) << OffsetShift2; + idx |= ( ( (unsigned long long)(offset[2]) ) & OffsetMask ) << OffsetShift3; + return idx; +} +template< class NodeData > +inline int OctNode< NodeData >::depth( void ) const {return int( _depthAndOffset & DepthMask );} +template< class NodeData > +inline void OctNode< NodeData >::DepthAndOffset(const long long& index,int& depth,int offset[3]){ + depth=int(index&DepthMask); + offset[0]=(int((index>>OffsetShift1)&OffsetMask)+1)&(~(1<>OffsetShift2)&OffsetMask)+1)&(~(1<>OffsetShift3)&OffsetMask)+1)&(~(1< +inline int OctNode< NodeData >::Depth(const long long& index){return int(index&DepthMask);} +template< class NodeData > +template< class Real > +void OctNode< NodeData >::centerAndWidth( Point3D& center , Real& width ) const +{ + int depth , offset[3]; + depthAndOffset( depth , offset ); + width = Real( 1.0 / (1< +template< class Real > +void OctNode< NodeData >::startAndWidth( Point3D& start , Real& width ) const +{ + int depth , offset[3]; + depthAndOffset( depth , offset ); + width = Real( 1.0 / (1< +template< class Real > +bool OctNode< NodeData >::isInside( Point3D< Real > p ) const +{ + Point3D< Real > c; + Real w; + centerAndWidth( c , w ); + w /= 2; + return (c[0]-w) +template< class Real > +inline void OctNode< NodeData >::CenterAndWidth(const long long& index,Point3D& center,Real& width){ + int depth,offset[3]; + depth=index&DepthMask; + offset[0]=(int((index>>OffsetShift1)&OffsetMask)+1)&(~(1<>OffsetShift2)&OffsetMask)+1)&(~(1<>OffsetShift3)&OffsetMask)+1)&(~(1< +template< class Real > +inline void OctNode< NodeData >::StartAndWidth( const long long& index , Point3D< Real >& start , Real& width ) +{ + int depth,offset[3]; + depth = index&DepthMask; + offset[0] = (int((index>>OffsetShift1)&OffsetMask)+1)&(~(1<>OffsetShift2)&OffsetMask)+1)&(~(1<>OffsetShift3)&OffsetMask)+1)&(~(1< +int OctNode< NodeData >::maxDepth(void) const{ + if(!children){return 0;} + else{ + int c,d; + for(int i=0;ic){c=d;} + } + return c+1; + } +} +template< class NodeData > +size_t OctNode< NodeData >::nodes( void ) const +{ + if( !children ) return 1; + else + { + size_t c=0; + for( int i=0 ; i +size_t OctNode< NodeData >::leaves( void ) const +{ + if( !children ) return 1; + else + { + size_t c=0; + for( int i=0 ; i +size_t OctNode< NodeData >::maxDepthLeaves( int maxDepth ) const +{ + if( depth()>maxDepth ) return 0; + if( !children ) return 1; + else + { + size_t c=0; + for( int i=0 ; i +const OctNode< NodeData >* OctNode< NodeData >::root(void) const{ + const OctNode* temp=this; + while(temp->parent){temp=temp->parent;} + return temp; +} + + +template< class NodeData > +const OctNode< NodeData >* OctNode< NodeData >::nextBranch( const OctNode* current ) const +{ + if( !current->parent || current==this ) return NULL; + if(current-current->parent->children==Cube::CORNERS-1) return nextBranch( current->parent ); + else return current+1; +} +template< class NodeData > +OctNode< NodeData >* OctNode< NodeData >::nextBranch(OctNode* current){ + if(!current->parent || current==this){return NULL;} + if(current-current->parent->children==Cube::CORNERS-1){return nextBranch(current->parent);} + else{return current+1;} +} +template< class NodeData > +const OctNode< NodeData >* OctNode< NodeData >::prevBranch( const OctNode* current ) const +{ + if( !current->parent || current==this ) return NULL; + if( current-current->parent->children==0 ) return prevBranch( current->parent ); + else return current-1; +} +template< class NodeData > +OctNode< NodeData >* OctNode< NodeData >::prevBranch( OctNode* current ) +{ + if( !current->parent || current==this ) return NULL; + if( current-current->parent->children==0 ) return prevBranch( current->parent ); + else return current-1; +} +template< class NodeData > +const OctNode< NodeData >* OctNode< NodeData >::nextLeaf(const OctNode* current) const{ + if(!current){ + const OctNode< NodeData >* temp=this; + while(temp->children){temp=&temp->children[0];} + return temp; + } + if(current->children){return current->nextLeaf();} + const OctNode* temp=nextBranch(current); + if(!temp){return NULL;} + else{return temp->nextLeaf();} +} +template< class NodeData > +OctNode< NodeData >* OctNode< NodeData >::nextLeaf(OctNode* current){ + if(!current){ + OctNode< NodeData >* temp=this; + while(temp->children){temp=&temp->children[0];} + return temp; + } + if(current->children){return current->nextLeaf();} + OctNode* temp=nextBranch(current); + if(!temp){return NULL;} + else{return temp->nextLeaf();} +} + +template< class NodeData > +const OctNode< NodeData >* OctNode< NodeData >::nextNode( const OctNode* current ) const +{ + if( !current ) return this; + else if( current->children ) return ¤t->children[0]; + else return nextBranch(current); +} +template< class NodeData > +OctNode< NodeData >* OctNode< NodeData >::nextNode( OctNode* current ) +{ + if( !current ) return this; + else if( current->children ) return ¤t->children[0]; + else return nextBranch( current ); +} + +template< class NodeData > +void OctNode< NodeData >::printRange(void) const +{ + Point3D< float > center; + float width; + centerAndWidth(center,width); + for(int dim=0;dim +template< class Real > +int OctNode< NodeData >::CornerIndex(const Point3D& center,const Point3D& p){ + int cIndex=0; + if(p.coords[0]>center.coords[0]){cIndex|=1;} + if(p.coords[1]>center.coords[1]){cIndex|=2;} + if(p.coords[2]>center.coords[2]){cIndex|=4;} + return cIndex; +} + +template< class NodeData > +OctNode< NodeData >* OctNode< NodeData >::faceNeighbor(int faceIndex,int forceChildren){return __faceNeighbor(faceIndex>>1,faceIndex&1,forceChildren);} +template< class NodeData > +const OctNode< NodeData >* OctNode< NodeData >::faceNeighbor(int faceIndex) const {return __faceNeighbor(faceIndex>>1,faceIndex&1);} +template< class NodeData > +OctNode< NodeData >* OctNode< NodeData >::__faceNeighbor(int dir,int off,int forceChildren){ + if(!parent){return NULL;} + int pIndex=int(this-parent->children); + pIndex^=(1<children[pIndex];} + else{ + OctNode* temp=parent->__faceNeighbor(dir,off,forceChildren); + if(!temp){return NULL;} + if(!temp->children){ + if(forceChildren){temp->initChildren();} + else{return temp;} + } + return &temp->children[pIndex]; + } +} +template< class NodeData > +const OctNode< NodeData >* OctNode< NodeData >::__faceNeighbor(int dir,int off) const { + if(!parent){return NULL;} + int pIndex=int(this-parent->children); + pIndex^=(1<children[pIndex];} + else{ + const OctNode* temp=parent->__faceNeighbor(dir,off); + if(!temp || !temp->children){return temp;} + else{return &temp->children[pIndex];} + } +} + +template< class NodeData > +OctNode< NodeData >* OctNode< NodeData >::edgeNeighbor(int edgeIndex,int forceChildren){ + int idx[2],o,i[2]; + Cube::FactorEdgeIndex(edgeIndex,o,i[0],i[1]); + switch(o){ + case 0: idx[0]=1; idx[1]=2; break; + case 1: idx[0]=0; idx[1]=2; break; + case 2: idx[0]=0; idx[1]=1; break; + }; + return __edgeNeighbor(o,i,idx,forceChildren); +} +template< class NodeData > +const OctNode< NodeData >* OctNode< NodeData >::edgeNeighbor(int edgeIndex) const { + int idx[2],o,i[2]; + Cube::FactorEdgeIndex(edgeIndex,o,i[0],i[1]); + switch(o){ + case 0: idx[0]=1; idx[1]=2; break; + case 1: idx[0]=0; idx[1]=2; break; + case 2: idx[0]=0; idx[1]=1; break; + }; + return __edgeNeighbor(o,i,idx); +} +template< class NodeData > +const OctNode< NodeData >* OctNode< NodeData >::__edgeNeighbor(int o,const int i[2],const int idx[2]) const{ + if(!parent){return NULL;} + int pIndex=int(this-parent->children); + int aIndex,x[DIMENSION]; + + Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]); + aIndex=(~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]])<<1))) & 3; + pIndex^=(7 ^ (1<__faceNeighbor(idx[0],i[0]); + if(!temp || !temp->children){return NULL;} + else{return &temp->children[pIndex];} + } + else if(aIndex==2) { // I can get the neighbor from the parent's face adjacent neighbor + const OctNode* temp=parent->__faceNeighbor(idx[1],i[1]); + if(!temp || !temp->children){return NULL;} + else{return &temp->children[pIndex];} + } + else if(aIndex==0) { // I can get the neighbor from the parent + return &parent->children[pIndex]; + } + else if(aIndex==3) { // I can get the neighbor from the parent's edge adjacent neighbor + const OctNode* temp=parent->__edgeNeighbor(o,i,idx); + if(!temp || !temp->children){return temp;} + else{return &temp->children[pIndex];} + } + else{return NULL;} +} +template< class NodeData > +OctNode< NodeData >* OctNode< NodeData >::__edgeNeighbor(int o,const int i[2],const int idx[2],int forceChildren){ + if(!parent){return NULL;} + int pIndex=int(this-parent->children); + int aIndex,x[DIMENSION]; + + Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]); + aIndex=(~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]])<<1))) & 3; + pIndex^=(7 ^ (1<__faceNeighbor(idx[0],i[0],0); + if(!temp || !temp->children){return NULL;} + else{return &temp->children[pIndex];} + } + else if(aIndex==2) { // I can get the neighbor from the parent's face adjacent neighbor + OctNode* temp=parent->__faceNeighbor(idx[1],i[1],0); + if(!temp || !temp->children){return NULL;} + else{return &temp->children[pIndex];} + } + else if(aIndex==0) { // I can get the neighbor from the parent + return &parent->children[pIndex]; + } + else if(aIndex==3) { // I can get the neighbor from the parent's edge adjacent neighbor + OctNode* temp=parent->__edgeNeighbor(o,i,idx,forceChildren); + if(!temp){return NULL;} + if(!temp->children){ + if(forceChildren){temp->initChildren();} + else{return temp;} + } + return &temp->children[pIndex]; + } + else{return NULL;} +} + +template< class NodeData > +const OctNode< NodeData >* OctNode< NodeData >::cornerNeighbor(int cornerIndex) const { + int pIndex,aIndex=0; + if(!parent){return NULL;} + + pIndex=int(this-parent->children); + aIndex=(cornerIndex ^ pIndex); // The disagreement bits + pIndex=(~pIndex)&7; // The antipodal point + if(aIndex==7){ // Agree on no bits + return &parent->children[pIndex]; + } + else if(aIndex==0){ // Agree on all bits + const OctNode* temp=((const OctNode*)parent)->cornerNeighbor(cornerIndex); + if(!temp || !temp->children){return temp;} + else{return &temp->children[pIndex];} + } + else if(aIndex==6){ // Agree on face 0 + const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(0,cornerIndex & 1); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==5){ // Agree on face 1 + const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(1,(cornerIndex & 2)>>1); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==3){ // Agree on face 2 + const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(2,(cornerIndex & 4)>>2); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==4){ // Agree on edge 2 + const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(8 | (cornerIndex & 1) | (cornerIndex & 2) ); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==2){ // Agree on edge 1 + const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(4 | (cornerIndex & 1) | ((cornerIndex & 4)>>1) ); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==1){ // Agree on edge 0 + const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(((cornerIndex & 2) | (cornerIndex & 4))>>1 ); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else{return NULL;} +} +template< class NodeData > +OctNode< NodeData >* OctNode< NodeData >::cornerNeighbor(int cornerIndex,int forceChildren){ + int pIndex,aIndex=0; + if(!parent){return NULL;} + + pIndex=int(this-parent->children); + aIndex=(cornerIndex ^ pIndex); // The disagreement bits + pIndex=(~pIndex)&7; // The antipodal point + if(aIndex==7){ // Agree on no bits + return &parent->children[pIndex]; + } + else if(aIndex==0){ // Agree on all bits + OctNode* temp=((OctNode*)parent)->cornerNeighbor(cornerIndex,forceChildren); + if(!temp){return NULL;} + if(!temp->children){ + if(forceChildren){temp->initChildren();} + else{return temp;} + } + return &temp->children[pIndex]; + } + else if(aIndex==6){ // Agree on face 0 + OctNode* temp=((OctNode*)parent)->__faceNeighbor(0,cornerIndex & 1,0); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==5){ // Agree on face 1 + OctNode* temp=((OctNode*)parent)->__faceNeighbor(1,(cornerIndex & 2)>>1,0); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==3){ // Agree on face 2 + OctNode* temp=((OctNode*)parent)->__faceNeighbor(2,(cornerIndex & 4)>>2,0); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==4){ // Agree on edge 2 + OctNode* temp=((OctNode*)parent)->edgeNeighbor(8 | (cornerIndex & 1) | (cornerIndex & 2) ); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==2){ // Agree on edge 1 + OctNode* temp=((OctNode*)parent)->edgeNeighbor(4 | (cornerIndex & 1) | ((cornerIndex & 4)>>1) ); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==1){ // Agree on edge 0 + OctNode* temp=((OctNode*)parent)->edgeNeighbor(((cornerIndex & 2) | (cornerIndex & 4))>>1 ); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else{return NULL;} +} + +//////////////////////// +// OctNode::Neighbors // +//////////////////////// +template< class NodeData > +template< unsigned int Width > +OctNode< NodeData >::Neighbors< Width >::Neighbors( void ){ clear(); } +template< class NodeData > +template< unsigned int Width > +void OctNode< NodeData >::Neighbors< Width >::clear( void ){ for( int i=0 ; i +template< unsigned int Width > +OctNode< NodeData >::ConstNeighbors< Width >::ConstNeighbors( void ){ clear(); } +template< class NodeData > +template< unsigned int Width > +void OctNode< NodeData >::ConstNeighbors< Width >::clear( void ){ for( int i=0 ; i +template< unsigned int LeftRadius , unsigned int RightRadius > +OctNode< NodeData >::NeighborKey< LeftRadius , RightRadius >::NeighborKey( void ){ _depth=-1 , neighbors=NULL; } +template< class NodeData > +template< unsigned int LeftRadius , unsigned int RightRadius > +OctNode< NodeData >::NeighborKey< LeftRadius , RightRadius >::NeighborKey( const NeighborKey& nKey ) +{ + _depth = 0 , neighbors = NULL; + set( nKey._depth ); + for( int d=0 ; d<=_depth ; d++ ) memcpy( &neighbors[d] , &nKey.neighbors[d] , sizeof( Neighbors< Width > ) ); +} +template< class NodeData > +template< unsigned int LeftRadius , unsigned int RightRadius > +OctNode< NodeData >::NeighborKey< LeftRadius , RightRadius >::~NeighborKey( void ) +{ + if( neighbors ) delete[] neighbors; + neighbors = NULL; +} + +template< class NodeData > +template< unsigned int LeftRadius , unsigned int RightRadius > +void OctNode< NodeData >::NeighborKey< LeftRadius , RightRadius >::set( int d ) +{ + if( neighbors ) delete[] neighbors; + neighbors = NULL; + _depth = d; + if( d<0 ) return; + neighbors = new Neighbors< Width >[d+1]; +} +template< class NodeData > +template< unsigned int LeftRadius , unsigned int RightRadius > +template< bool CreateNodes > +bool OctNode< NodeData >::NeighborKey< LeftRadius , RightRadius >::getChildNeighbors( int cIdx , int d , Neighbors< Width >& cNeighbors ) const +{ + Neighbors< Width >& pNeighbors = neighbors[d]; + // Check that we actuall have a center node + if( !pNeighbors.neighbors[LeftRadius][LeftRadius][LeftRadius] ) return false; + + // Get the indices of the child node that would contain the point (and its antipode) + int cx , cy , cz; + Cube::FactorCornerIndex( cIdx , cx , cy , cz ); + + + // Iterate over the finer neighbors and set them (if you can) + // Here: + // (x,y,z) give the position of the finer nodes relative to the center, + // (_x,_y,_z) give a positive global position, up to an even offset, and + // (px-LeftRadius,py-LeftRadius,pz-LeftRadius) give the positions of their parents relative to the parent of the center + for( int z=-(int)LeftRadius ; z<=(int)RightRadius ; z++ ) + { + int _z = (z+cz) + (LeftRadius<<1) , pz = ( _z>>1 ) , zz = z+LeftRadius; + for( int y=-(int)LeftRadius ; y<=(int)RightRadius ; y++ ) + { + int _y = (y+cy) + (LeftRadius<<1) , py = ( _y>>1 ) , yy = y+LeftRadius; + + int cornerIndex = ( (_z&1)<<2 ) | ( (_y&1)<<1 ); + for( int x=-(int)LeftRadius ; x<=(int)RightRadius ; x++ ) + { + int _x = (x+cx) + (LeftRadius<<1) , px = ( _x>>1 ) , xx = x+LeftRadius; + + if( CreateNodes ) + { + if( pNeighbors.neighbors[px][py][pz] ) + { + if( !pNeighbors.neighbors[px][py][pz]->children ) pNeighbors.neighbors[px][py][pz]->initChildren(); + cNeighbors.neighbors[xx][yy][zz] = pNeighbors.neighbors[px][py][pz]->children + ( cornerIndex | (_x&1) ); + } + else cNeighbors.neighbors[xx][yy][zz] = NULL; + } + else + { + if( pNeighbors.neighbors[px][py][pz] && pNeighbors.neighbors[px][py][pz]->children ) + cNeighbors.neighbors[xx][yy][zz] = pNeighbors.neighbors[px][py][pz]->children + ( cornerIndex | (_x&1) ); + else cNeighbors.neighbors[xx][yy][zz] = NULL; + } + } + } + } + return true; +} +template< class NodeData > +template< unsigned int LeftRadius , unsigned int RightRadius > +template< bool CreateNodes , class Real > +bool OctNode< NodeData >::NeighborKey< LeftRadius , RightRadius >::getChildNeighbors( Point3D< Real > p , int d , Neighbors< Width >& cNeighbors ) const +{ + Neighbors< Width >& pNeighbors = neighbors[d]; + // Check that we actuall have a center node + if( !pNeighbors.neighbors[LeftRadius][LeftRadius][LeftRadius] ) return false; + Point3D< Real > c; + Real w; + pNeighbors.neighbors[LeftRadius][LeftRadius][LeftRadius]->centerAndWidth( c , w ); + return getChildNeighbors< CreateNodes >( CornerIndex( c , p ) , d , cNeighbors ); +} + +template< class NodeData > +template< unsigned int LeftRadius , unsigned int RightRadius > +template< bool CreateNodes > +typename OctNode< NodeData >::template Neighbors< LeftRadius+RightRadius+1 >& OctNode< NodeData >::NeighborKey< LeftRadius , RightRadius >::getNeighbors( OctNode< NodeData >* node ) +{ + Neighbors< Width >& neighbors = this->neighbors[ node->depth() ]; + if( node==neighbors.neighbors[LeftRadius][LeftRadius][LeftRadius] ) + { + bool reset = false; + for( int i=0 ; iparent ) neighbors.neighbors[LeftRadius][LeftRadius][LeftRadius] = node; + else + { + Neighbors< Width >& pNeighbors = getNeighbors< CreateNodes >( node->parent ); + + + // Get the indices of the child node that would contain the point (and its antipode) + int cx , cy , cz; + Cube::FactorCornerIndex( (int)( node - node->parent->children ) , cx , cy , cz ); + + + // Iterate over the finer neighbors and set them (if you can) + // Here: + // (x,y,z) give the position of the finer nodes relative to the center, + // (_x,_y,_z) give a positive global position, up to an even offset, and + // (px-LeftRadius,py-LeftRadius,pz-LeftRadius) give the positions of their parents relative to the parent of the center + for( int z=-(int)LeftRadius ; z<=(int)RightRadius ; z++ ) + { + int _z = (z+cz) + (LeftRadius<<1) , pz = ( _z>>1 ) , zz = z+LeftRadius; + for( int y=-(int)LeftRadius ; y<=(int)RightRadius ; y++ ) + { + int _y = (y+cy) + (LeftRadius<<1) , py = ( _y>>1 ) , yy = y+LeftRadius; + + int cornerIndex = ( (_z&1)<<2 ) | ( (_y&1)<<1 ); + for( int x=-(int)LeftRadius ; x<=(int)RightRadius ; x++ ) + { + int _x = (x+cx) + (LeftRadius<<1) , px = ( _x>>1 ) , xx = x+LeftRadius; + if( CreateNodes ) + { + if( pNeighbors.neighbors[px][py][pz] ) + { + if( !pNeighbors.neighbors[px][py][pz]->children ) pNeighbors.neighbors[px][py][pz]->initChildren(); + neighbors.neighbors[xx][yy][zz] = pNeighbors.neighbors[px][py][pz]->children + ( cornerIndex | (_x&1) ); + } + else neighbors.neighbors[xx][yy][zz] = NULL; + } + else + { + if( pNeighbors.neighbors[px][py][pz] && pNeighbors.neighbors[px][py][pz]->children ) + neighbors.neighbors[xx][yy][zz] = pNeighbors.neighbors[px][py][pz]->children + ( cornerIndex | (_x&1) ); + else neighbors.neighbors[xx][yy][zz] = NULL; + } + } + } + } + } + } + return neighbors; +} +template< class NodeData > +template< unsigned int LeftRadius , unsigned int RightRadius > +template< bool CreateNodes , unsigned int _LeftRadius , unsigned int _RightRadius > +void OctNode< NodeData >::NeighborKey< LeftRadius , RightRadius >::getNeighbors( OctNode< NodeData >* node , Neighbors< _LeftRadius + _RightRadius + 1 >& neighbors ) +{ + neighbors.clear(); + if( !node ) return; + + // [WARNING] This estimate of the required radius is somewhat conservative if the radius is odd (depending on where the node is relative to its parent) + const unsigned int _PLeftRadius = (_LeftRadius+1)/2 , _PRightRadius = (_RightRadius+1)/2; + // If we are at the root of the tree, we are done + if( !node->parent ) neighbors.neighbors[_LeftRadius][_LeftRadius][_LeftRadius] = node; + // If we can get the data from the the key for the parent node, do that + else if( _PLeftRadius<=LeftRadius && _PRightRadius<=RightRadius ) + { + getNeighbors< CreateNodes >( node->parent ); + const Neighbors< LeftRadius + RightRadius + 1 >& pNeighbors = this->neighbors[ node->depth()-1 ]; + // Get the indices of the child node that would contain the point (and its antipode) + int cx , cy , cz; + Cube::FactorCornerIndex( (int)( node - node->parent->children ) , cx , cy , cz ); + + + // Iterate over the finer neighbors + // Here: + // (x,y,z) give the position of the finer nodes relative to the center, + // (_x,_y,_z) give a positive global position, up to an even offset, and + // (px-LeftRadius,py-LeftRadius,pz-LeftRadius) give the positions of their parents relative to the parent of the center + for( int z=-(int)_LeftRadius ; z<=(int)_RightRadius ; z++ ) + { + int _z = (z+cz) + (_LeftRadius<<1) , pz = ( _z>>1 ) - _LeftRadius + LeftRadius , zz = z + _LeftRadius; + for( int y=-(int)_LeftRadius ; y<=(int)_RightRadius ; y++ ) + { + int _y = (y+cy) + (_LeftRadius<<1) , py = ( _y>>1 ) - _LeftRadius + LeftRadius , yy = y + _LeftRadius; + + int cornerIndex = ( (_z&1)<<2 ) | ( (_y&1)<<1 ); + for( int x=-(int)_LeftRadius ; x<=(int)_RightRadius ; x++ ) + { + int _x = (x+cx) + (_LeftRadius<<1) , px = ( _x>>1 ) - _LeftRadius + LeftRadius , xx = x + _LeftRadius; + if( CreateNodes ) + { + if( pNeighbors.neighbors[px][py][pz] ) + { + if( !pNeighbors.neighbors[px][py][pz]->children ) pNeighbors.neighbors[px][py][pz]->initChildren(); + neighbors.neighbors[xx][yy][zz] = pNeighbors.neighbors[px][py][pz]->children + ( cornerIndex | (_x&1) ); + } + else neighbors.neighbors[xx][yy][zz] = NULL; + } + else + { + if( pNeighbors.neighbors[px][py][pz] && pNeighbors.neighbors[px][py][pz]->children ) + neighbors.neighbors[xx][yy][zz] = pNeighbors.neighbors[px][py][pz]->children + ( cornerIndex | (_x&1) ); + else neighbors.neighbors[xx][yy][zz] = NULL; + } + } + } + } + } + // Otherwise recurse + else + { + Neighbors< _PLeftRadius + _PRightRadius + 1 > pNeighbors; + getNeighbors< CreateNodes , _PLeftRadius , _PRightRadius >( node->parent , pNeighbors ); + + // Get the indices of the child node that would contain the point (and its antipode) + int cx , cy , cz; + Cube::FactorCornerIndex( (int)( node - node->parent->children ) , cx , cy , cz ); + + + // Iterate over the finer neighbors + // Here: + // (x,y,z) give the position of the finer nodes relative to the center, + // (_x,_y,_z) give a positive global position, up to an even offset, and + // (px-LeftRadius,py-LeftRadius,pz-LeftRadius) give the positions of their parents relative to the parent of the center + for( int z=-(int)_LeftRadius ; z<=(int)_RightRadius ; z++ ) + { + int _z = (z+cz) + (_LeftRadius<<1) , pz = ( _z>>1 ) - _LeftRadius + _PLeftRadius , zz = z + _LeftRadius; + for( int y=-(int)_LeftRadius ; y<=(int)_RightRadius ; y++ ) + { + int _y = (y+cy) + (_LeftRadius<<1) , py = ( _y>>1 ) - _LeftRadius + _PLeftRadius , yy = y + _LeftRadius; + + int cornerIndex = ( (_z&1)<<2 ) | ( (_y&1)<<1 ); + for( int x=-(int)_LeftRadius ; x<=(int)_RightRadius ; x++ ) + { + int _x = (x+cx) + (_LeftRadius<<1) , px = ( _x>>1 ) - _LeftRadius + _PLeftRadius , xx = x + _LeftRadius; + if( CreateNodes ) + { + if( pNeighbors.neighbors[px][py][pz] ) + { + if( !pNeighbors.neighbors[px][py][pz]->children ) pNeighbors.neighbors[px][py][pz]->initChildren(); + neighbors.neighbors[xx][yy][zz] = pNeighbors.neighbors[px][py][pz]->children + ( cornerIndex | (_x&1) ); + } + else neighbors.neighbors[xx][yy][zz] = NULL; + } + else + { + if( pNeighbors.neighbors[px][py][pz] && pNeighbors.neighbors[px][py][pz]->children ) + neighbors.neighbors[xx][yy][zz] = pNeighbors.neighbors[px][py][pz]->children + ( cornerIndex | (_x&1) ); + else neighbors.neighbors[xx][yy][zz] = NULL; + } + } + } + } + } +} + +/////////////////////////////// +// OctNode::ConstNeighborKey // +/////////////////////////////// +template< class NodeData > +template< unsigned int LeftRadius , unsigned int RightRadius > +OctNode< NodeData >::ConstNeighborKey< LeftRadius , RightRadius >::ConstNeighborKey( void ){ _depth=-1 , neighbors=NULL; } +template< class NodeData > +template< unsigned int LeftRadius , unsigned int RightRadius > +OctNode< NodeData >::ConstNeighborKey< LeftRadius , RightRadius >::ConstNeighborKey( const ConstNeighborKey& key ) +{ + _depth = 0 , neighbors = NULL; + set( key._depth ); + for( int d=0 ; d<=_depth ; d++ ) memcpy( &neighbors[d] , &key.neighbors[d] , sizeof( ConstNeighbors< Width > ) ); +} +template< class NodeData > +template< unsigned int LeftRadius , unsigned int RightRadius > +OctNode< NodeData >::ConstNeighborKey< LeftRadius , RightRadius >::~ConstNeighborKey( void ) +{ + if( neighbors ) delete[] neighbors; + neighbors=NULL; +} + +template< class NodeData > +template< unsigned int LeftRadius , unsigned int RightRadius > +void OctNode< NodeData >::ConstNeighborKey< LeftRadius , RightRadius >::set( int d ) +{ + if( neighbors ) delete[] neighbors; + neighbors = NULL; + _depth = d; + if( d<0 ) return; + neighbors = new ConstNeighbors< Width >[d+1]; +} +template< class NodeData > +template< unsigned int LeftRadius , unsigned int RightRadius > +typename OctNode< NodeData >::template ConstNeighbors< LeftRadius+RightRadius+1 >& OctNode< NodeData >::ConstNeighborKey< LeftRadius , RightRadius >::getNeighbors( const OctNode< NodeData >* node ) +{ + ConstNeighbors< Width >& neighbors = this->neighbors[ node->depth() ]; + if( node!=neighbors.neighbors[LeftRadius][LeftRadius][LeftRadius]) + { + neighbors.clear(); + + if( !node->parent ) neighbors.neighbors[LeftRadius][LeftRadius][LeftRadius] = node; + else + { + ConstNeighbors< Width >& pNeighbors = getNeighbors( node->parent ); + + // Get the indices of the child node that would contain the point (and its antipode) + int cx , cy , cz; + Cube::FactorCornerIndex( (int)( node - node->parent->children ) , cx , cy , cz ); + + + // Iterate over the finer neighbors and set them (if you can) + // Here: + // (x,y,z) give the position of the finer nodes relative to the center, + // (_x,_y,_z) give a positive global position, up to an even offset, and + // (px-LeftRadius,py-LeftRadius,pz-LeftRadius) give the positions of their parents relative to the parent of the center + for( int z=-(int)LeftRadius ; z<=(int)RightRadius ; z++ ) + { + int _z = (z+cz) + (LeftRadius<<1) , pz = ( _z>>1 ) , zz = z+LeftRadius; + for( int y=-(int)LeftRadius ; y<=(int)RightRadius ; y++ ) + { + int _y = (y+cy) + (LeftRadius<<1) , py = ( _y>>1 ) , yy = y+LeftRadius; + + int cornerIndex = ( (_z&1)<<2 ) | ( (_y&1)<<1 ); + for( int x=-(int)LeftRadius ; x<=(int)RightRadius ; x++ ) + { + int _x = (x+cx) + (LeftRadius<<1) , px = ( _x>>1 ) , xx = x+LeftRadius; + if( pNeighbors.neighbors[px][py][pz] && pNeighbors.neighbors[px][py][pz]->children ) + neighbors.neighbors[xx][yy][zz] = pNeighbors.neighbors[px][py][pz]->children + ( cornerIndex | (_x&1) ); + else + neighbors.neighbors[xx][yy][zz] = NULL; + } + } + } + } + } + return neighbors; +} +template< class NodeData > +template< unsigned int LeftRadius , unsigned int RightRadius > +template< unsigned int _LeftRadius , unsigned int _RightRadius > +void OctNode< NodeData >::ConstNeighborKey< LeftRadius , RightRadius >::getNeighbors( const OctNode< NodeData >* node , ConstNeighbors< _LeftRadius+_RightRadius+1 >& neighbors ) +{ + neighbors.clear(); + if( !node ) return; + + // [WARNING] This estimate of the required radius is somewhat conservative if the readius is odd (depending on where the node is relative to its parent) + const unsigned int _PLeftRadius = (_LeftRadius+1)/2 , _PRightRadius = (_RightRadius+1)/2; + // If we are at the root of the tree, we are done + if( !node->parent ) neighbors.neighbors[_LeftRadius][_LeftRadius][_LeftRadius] = node; + // If we can get the data from the the key for the parent node, do that + else if( _PLeftRadius<=LeftRadius && _PRightRadius<=RightRadius ) + { + getNeighbors( node->parent ); + const ConstNeighbors< LeftRadius + RightRadius + 1 >& pNeighbors = this->neighbors[ node->depth()-1 ]; + // Get the indices of the child node that would contain the point (and its antipode) + int cx , cy , cz; + Cube::FactorCornerIndex( (int)( node - node->parent->children ) , cx , cy , cz ); + + + // Iterate over the finer neighbors + // Here: + // (x,y,z) give the position of the finer nodes relative to the center, + // (_x,_y,_z) give a positive global position, up to an even offset, and + // (px-LeftRadius,py-LeftRadius,pz-LeftRadius) give the positions of their parents relative to the parent of the center + for( int z=-(int)_LeftRadius ; z<=(int)_RightRadius ; z++ ) + { + int _z = (z+cz) + (_LeftRadius<<1) , pz = ( _z>>1 ) - _LeftRadius + LeftRadius , zz = z + _LeftRadius; + for( int y=-(int)_LeftRadius ; y<=(int)_RightRadius ; y++ ) + { + int _y = (y+cy) + (_LeftRadius<<1) , py = ( _y>>1 ) - _LeftRadius + LeftRadius , yy = y + _LeftRadius; + + int cornerIndex = ( (_z&1)<<2 ) | ( (_y&1)<<1 ); + for( int x=-(int)_LeftRadius ; x<=(int)_RightRadius ; x++ ) + { + int _x = (x+cx) + (_LeftRadius<<1) , px = ( _x>>1 ) - _LeftRadius + LeftRadius , xx = x + _LeftRadius; + if( pNeighbors.neighbors[px][py][pz] && pNeighbors.neighbors[px][py][pz]->children ) + neighbors.neighbors[xx][yy][zz] = pNeighbors.neighbors[px][py][pz]->children + ( cornerIndex | (_x&1) ); + else + neighbors.neighbors[xx][yy][zz] = NULL; + } + } + } + } + // Otherwise recurse + else + { + ConstNeighbors< _PLeftRadius + _PRightRadius + 1 > pNeighbors; + getNeighbors< _PLeftRadius , _PRightRadius >( node->parent , pNeighbors ); + + // Get the indices of the child node that would contain the point (and its antipode) + int cx , cy , cz; + Cube::FactorCornerIndex( (int)( node - node->parent->children ) , cx , cy , cz ); + + + // Iterate over the finer neighbors + // Here: + // (x,y,z) give the position of the finer nodes relative to the center, + // (_x,_y,_z) give a positive global position, up to an even offset, and + // (px-LeftRadius,py-LeftRadius,pz-LeftRadius) give the positions of their parents relative to the parent of the center + for( int z=-(int)_LeftRadius ; z<=(int)_RightRadius ; z++ ) + { + int _z = (z+cz) + (_LeftRadius<<1) , pz = ( _z>>1 ) - _LeftRadius + _PLeftRadius , zz = z + _LeftRadius; + for( int y=-(int)_LeftRadius ; y<=(int)_RightRadius ; y++ ) + { + int _y = (y+cy) + (_LeftRadius<<1) , py = ( _y>>1 ) - _LeftRadius + _PLeftRadius , yy = y + _LeftRadius; + + int cornerIndex = ( (_z&1)<<2 ) | ( (_y&1)<<1 ); + for( int x=-(int)_LeftRadius ; x<=(int)_RightRadius ; x++ ) + { + int _x = (x+cx) + (_LeftRadius<<1) , px = ( _x>>1 ) - _LeftRadius + _PLeftRadius , xx = x + _LeftRadius; + + if( pNeighbors.neighbors[px][py][pz] && pNeighbors.neighbors[px][py][pz]->children ) + neighbors.neighbors[xx][yy][zz] = pNeighbors.neighbors[px][py][pz]->children + ( cornerIndex | (_x&1) ); + else + neighbors.neighbors[xx][yy][zz] = NULL; + } + } + } + } + return; +} + +template< class NodeData > +int OctNode< NodeData >::write(const char* fileName) const{ + FILE* fp=fopen(fileName,"wb"); + if(!fp){return 0;} + int ret=write(fp); + fclose(fp); + return ret; +} +template< class NodeData > +int OctNode< NodeData >::write(FILE* fp) const{ + fwrite(this,sizeof(OctNode< NodeData >),1,fp); + if(children){for(int i=0;i +int OctNode< NodeData >::read(const char* fileName){ + FILE* fp=fopen(fileName,"rb"); + if(!fp){return 0;} + int ret=read(fp); + fclose(fp); + return ret; +} +template< class NodeData > +int OctNode< NodeData >::read(FILE* fp){ + fread(this,sizeof(OctNode< NodeData >),1,fp); + parent=NULL; + if(children){ + children=NULL; + initChildren(); + for(int i=0;i +int OctNode< NodeData >::width(int maxDepth) const { + int d=depth(); + return 1<<(maxDepth-d); +} +template< class NodeData > +void OctNode< NodeData >::centerIndex(int maxDepth,int index[DIMENSION]) const +{ + int d,o[3]; + depthAndOffset(d,o); + for(int i=0;i +#include "Polynomial.h" +#include "Array.h" + +template< int Degree > +class StartingPolynomial +{ +public: + Polynomial< Degree > p; + double start; + + template< int Degree2 > + StartingPolynomial< Degree+Degree2 > operator * ( const StartingPolynomial< Degree2 >& p ) const; + StartingPolynomial scale( double s ) const; + StartingPolynomial shift( double t ) const; + int operator < ( const StartingPolynomial& sp ) const; + static int Compare( const void* v1 , const void* v2 ); +}; + +template< int Degree > +class PPolynomial +{ +public: + size_t polyCount; + Pointer( StartingPolynomial< Degree > ) polys; + + PPolynomial( void ); + PPolynomial( const PPolynomial& p ); + ~PPolynomial( void ); + + PPolynomial& operator = ( const PPolynomial& p ); + + int size( void ) const; + + void set( size_t size ); + // Note: this method will sort the elements in sps + void set( Pointer( StartingPolynomial ) sps , int count ); + void reset( size_t newSize ); + PPolynomial& compress( double delta=0. ); + + + double operator()( double t ) const; + double integral( double tMin , double tMax ) const; + double Integral( void ) const; + + template< int Degree2 > PPolynomial< Degree >& operator = ( const PPolynomial< Degree2 >& p ); + + PPolynomial operator + ( const PPolynomial& p ) const; + PPolynomial operator - ( const PPolynomial& p ) const; + + template< int Degree2 > PPolynomial< Degree+Degree2 > operator * ( const Polynomial< Degree2 >& p ) const; + template< int Degree2 > PPolynomial< Degree+Degree2 > operator * ( const PPolynomial< Degree2 >& p) const; + + + PPolynomial& operator += ( double s ); + PPolynomial& operator -= ( double s ); + PPolynomial& operator *= ( double s ); + PPolynomial& operator /= ( double s ); + PPolynomial operator + ( double s ) const; + PPolynomial operator - ( double s ) const; + PPolynomial operator * ( double s ) const; + PPolynomial operator / ( double s ) const; + + PPolynomial& addScaled( const PPolynomial& poly , double scale ); + + PPolynomial scale( double s ) const; + PPolynomial shift( double t ) const; + PPolynomial reflect( double r=0. ) const; + + PPolynomial< Degree-1 > derivative(void) const; + PPolynomial< Degree+1 > integral(void) const; + + void getSolutions( double c , std::vector< double >& roots , double EPS , double min=-DBL_MAX , double max=DBL_MAX ) const; + + void printnl( void ) const; + + PPolynomial< Degree+1 > MovingAverage( double radius ) const; + static PPolynomial BSpline( double radius=0.5 ); + + void write( FILE* fp , int samples , double min , double max ) const; +}; +#include "PPolynomial.inl" +#endif // P_POLYNOMIAL_INCLUDED diff --git a/colmap/include/colmap/lib/PoissonRecon/PPolynomial.inl b/colmap/include/colmap/lib/PoissonRecon/PPolynomial.inl new file mode 100644 index 0000000000000000000000000000000000000000..9670512a2ebb8b5ffdb990294fee40e5c4f05b6a --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/PPolynomial.inl @@ -0,0 +1,470 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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 "Factor.h" + +//////////////////////// +// StartingPolynomial // +//////////////////////// +template +template +StartingPolynomial StartingPolynomial::operator * (const StartingPolynomial& p) const{ + StartingPolynomial sp; + if(start>p.start){sp.start=start;} + else{sp.start=p.start;} + sp.p=this->p*p.p; + return sp; +} +template +StartingPolynomial StartingPolynomial::scale( double s ) const +{ + StartingPolynomial q; + q.start = start*s; + q.p = p.scale(s); + return q; +} +template +StartingPolynomial StartingPolynomial::shift(double s) const{ + StartingPolynomial q; + q.start=start+s; + q.p=p.shift(s); + return q; +} + + +template +int StartingPolynomial::operator < (const StartingPolynomial& sp) const{ + if(start +int StartingPolynomial::Compare(const void* v1,const void* v2){ + double d=((StartingPolynomial*)(v1))->start-((StartingPolynomial*)(v2))->start; + if ( d<0 ) return -1; + else if( d>0 ) return 1; + else return 0; +} + +///////////////// +// PPolynomial // +///////////////// +template< int Degree > +PPolynomial< Degree >::PPolynomial( void ) +{ + polyCount = 0; + polys = NullPointer( StartingPolynomial< Degree > ); +} +template< int Degree > +PPolynomial::PPolynomial( const PPolynomial& p ) +{ + polyCount = 0; + polys = NullPointer( StartingPolynomial< Degree > ); + set( p.polyCount ); + memcpy( polys , p.polys , sizeof( StartingPolynomial ) * p.polyCount ); +} + +template< int Degree > +PPolynomial< Degree >::~PPolynomial( void ) +{ + FreePointer( polys ); + polyCount = 0; +} +template< int Degree > +void PPolynomial< Degree >::set( Pointer( StartingPolynomial< Degree > ) sps , int count ) +{ + int c=0; + set( count ); + qsort( sps , count , sizeof( StartingPolynomial< Degree > ) , StartingPolynomial< Degree >::Compare ); + for( int i=0 ; i int PPolynomial< Degree >::size( void ) const{ return int(sizeof(StartingPolynomial)*polyCount); } + +template< int Degree > +void PPolynomial::set( size_t size ) +{ + FreePointer( polys ); + polyCount = size; + if( size ) + { + polys = AllocPointer< StartingPolynomial< Degree > >( size ); + memset( polys , 0 , sizeof( StartingPolynomial< Degree > )*size ); + } +} +template< int Degree > +void PPolynomial::reset( size_t newSize ) +{ + polyCount = newSize; + polys = ReAllocPointer< StartingPolynomial< Degree > >( polys , newSize ); +} +template< int Degree > +PPolynomial< Degree >& PPolynomial< Degree >::compress( double delta ) +{ + int _polyCount = polyCount; + Pointer( StartingPolynomial< Degree > ) _polys = polys; + + polyCount = 1 , polys = NullPointer( StartingPolynomial< Degree > ); + for( int i=1 ; i<_polyCount ; i++ ) if( _polys[i].start-_polys[i-1].start>delta ) polyCount++; + if( polyCount==_polyCount ) polys = _polys; + else + { + polys = AllocPointer< StartingPolynomial< Degree > >( polyCount ); + polys[0] = _polys[0] , polyCount = 0; + for( int i=1 ; i<_polyCount ; i++ ) + { + if( _polys[i].start-_polys[i-1].start>delta ) polys[ ++polyCount ] = _polys[i]; + else polys[ polyCount ].p += _polys[i].p; + } + polyCount++; + FreePointer( _polys ); + } + return *this; +} + +template< int Degree > +PPolynomial& PPolynomial::operator = (const PPolynomial& p){ + set(p.polyCount); + memcpy(polys,p.polys,sizeof(StartingPolynomial)*p.polyCount); + return *this; +} + +template +template +PPolynomial& PPolynomial::operator = (const PPolynomial& p){ + set(p.polyCount); + for(int i=0;i +double PPolynomial::operator ()( double t ) const +{ + double v=0; + for( int i=0 ; ipolys[i].start ; i++ ) v+=polys[i].p(t); + return v; +} + +template +double PPolynomial::integral( double tMin , double tMax ) const +{ + int m=1; + double start,end,s,v=0; + start=tMin; + end=tMax; + if(tMin>tMax){ + m=-1; + start=tMax; + end=tMin; + } + for(int i=0;i +double PPolynomial::Integral(void) const{return integral(polys[0].start,polys[polyCount-1].start);} +template +PPolynomial PPolynomial::operator + (const PPolynomial& p) const{ + PPolynomial q; + int i,j; + size_t idx=0; + q.set(polyCount+p.polyCount); + i=j=-1; + + while(idx=int(p.polyCount)-1) {q.polys[idx]= polys[++i];} + else if (i>=int( polyCount)-1) {q.polys[idx]=p.polys[++j];} + else if(polys[i+1].start +PPolynomial PPolynomial::operator - (const PPolynomial& p) const{ + PPolynomial q; + int i,j; + size_t idx=0; + q.set(polyCount+p.polyCount); + i=j=-1; + + while(idx=int(p.polyCount)-1) {q.polys[idx]= polys[++i];} + else if (i>=int( polyCount)-1) {q.polys[idx].start=p.polys[++j].start;q.polys[idx].p=p.polys[j].p*(-1.0);} + else if(polys[i+1].start +PPolynomial& PPolynomial::addScaled(const PPolynomial& p,double scale){ + int i,j; + StartingPolynomial* oldPolys=polys; + size_t idx=0,cnt=0,oldPolyCount=polyCount; + polyCount=0; + polys=NULL; + set(oldPolyCount+p.polyCount); + i=j=-1; + while(cnt=int( p.polyCount)-1) {polys[idx]=oldPolys[++i];} + else if (i>=int(oldPolyCount)-1) {polys[idx].start= p.polys[++j].start;polys[idx].p=p.polys[j].p*scale;} + else if (oldPolys[i+1].start +template +PPolynomial PPolynomial::operator * (const PPolynomial& p) const{ + PPolynomial q; + StartingPolynomial *sp; + int i,j,spCount=int(polyCount*p.polyCount); + + sp=(StartingPolynomial*)malloc(sizeof(StartingPolynomial)*spCount); + for(i=0;i +template +PPolynomial PPolynomial::operator * (const Polynomial& p) const{ + PPolynomial q; + q.set(polyCount); + for(int i=0;i +PPolynomial PPolynomial::scale( double s ) const +{ + PPolynomial q; + q.set( polyCount ); + for( size_t i=0 ; i ) , StartingPolynomial< Degree >::Compare ); + return q; +} +template< int Degree > +PPolynomial< Degree > PPolynomial< Degree >::reflect( double r ) const +{ + PPolynomial q; + q.set( polyCount ); + for( size_t i=0 ; i ) , StartingPolynomial< Degree >::Compare ); + return q; +} +template +PPolynomial PPolynomial::shift( double s ) const +{ + PPolynomial q; + q.set(polyCount); + for(size_t i=0;i +PPolynomial PPolynomial::derivative(void) const{ + PPolynomial q; + q.set(polyCount); + for(size_t i=0;i +PPolynomial PPolynomial::integral(void) const{ + int i; + PPolynomial q; + q.set(polyCount); + for(i=0;i +PPolynomial& PPolynomial::operator += ( double s ) {polys[0].p+=s;} +template +PPolynomial& PPolynomial::operator -= ( double s ) {polys[0].p-=s;} +template +PPolynomial& PPolynomial::operator *= ( double s ) +{ + for(int i=0;i +PPolynomial& PPolynomial::operator /= ( double s ) +{ + for(size_t i=0;i +PPolynomial PPolynomial::operator + ( double s ) const +{ + PPolynomial q=*this; + q+=s; + return q; +} +template +PPolynomial PPolynomial::operator - ( double s ) const +{ + PPolynomial q=*this; + q-=s; + return q; +} +template +PPolynomial PPolynomial::operator * ( double s ) const +{ + PPolynomial q=*this; + q*=s; + return q; +} +template +PPolynomial PPolynomial::operator / ( double s ) const +{ + PPolynomial q=*this; + q/=s; + return q; +} + +template +void PPolynomial::printnl(void) const{ + Polynomial p; + + if(!polyCount){ + Polynomial p; + printf("[-Infinity,Infinity]\n"); + } + else{ + for(size_t i=0;i +PPolynomial< 0 > PPolynomial< 0 >::BSpline( double radius ) +{ + PPolynomial q; + q.set(2); + + q.polys[0].start=-radius; + q.polys[1].start= radius; + + q.polys[0].p.coefficients[0]= 1.0; + q.polys[1].p.coefficients[0]=-1.0; + return q; +} +template< int Degree > +PPolynomial< Degree > PPolynomial::BSpline( double radius ) +{ + return PPolynomial< Degree-1 >::BSpline().MovingAverage( radius ); +} +template +PPolynomial PPolynomial::MovingAverage( double radius ) const +{ + PPolynomial A; + Polynomial p; + Pointer( StartingPolynomial< Degree+1 > ) sps; + sps = AllocPointer< StartingPolynomial< Degree+1 > >( polyCount*2 ); + + + for(int i=0;i +void PPolynomial::getSolutions(double c,std::vector& roots,double EPS,double min,double max) const{ + Polynomial p; + std::vector tempRoots; + + p.setZero(); + for(size_t i=0;imax){break;} + if(ipolys[i].start && (i+1==polyCount || tempRoots[j]<=polys[i+1].start)){ + if(tempRoots[j]>min && tempRoots[j] +void PPolynomial::write(FILE* fp,int samples,double min,double max) const{ + fwrite(&samples,sizeof(int),1,fp); + for(int i=0;i +#include +#include +#include + +#define PLY_ASCII 1 /* ascii PLY file */ +#define PLY_BINARY_BE 2 /* binary PLY file, big endian */ +#define PLY_BINARY_LE 3 /* binary PLY file, little endian */ +#define PLY_BINARY_NATIVE 4 /* binary PLY file, same endianness as current architecture */ + +#define PLY_OKAY 0 /* ply routine worked okay */ +#define PLY_ERROR -1 /* error in ply routine */ + + /* scalar data types supported by PLY format */ + +#define PLY_START_TYPE 0 +#define PLY_CHAR 1 +#define PLY_SHORT 2 +#define PLY_INT 3 +#define PLY_UCHAR 4 +#define PLY_USHORT 5 +#define PLY_UINT 6 +#define PLY_FLOAT 7 +#define PLY_DOUBLE 8 +#define PLY_INT_8 9 +#define PLY_UINT_8 10 +#define PLY_INT_16 11 +#define PLY_UINT_16 12 +#define PLY_INT_32 13 +#define PLY_UINT_32 14 +#define PLY_FLOAT_32 15 +#define PLY_FLOAT_64 16 + +#define PLY_END_TYPE 17 + +#define PLY_SCALAR 0 +#define PLY_LIST 1 + +#define PLY_STRIP_COMMENT_HEADER 0 + +typedef struct PlyProperty { /* description of a property */ + + char *name; /* property name */ + int external_type; /* file's data type */ + int internal_type; /* program's data type */ + int offset; /* offset bytes of prop in a struct */ + + int is_list; /* 1 = list, 0 = scalar */ + int count_external; /* file's count type */ + int count_internal; /* program's count type */ + int count_offset; /* offset byte for list count */ + +} PlyProperty; + +typedef struct PlyElement { /* description of an element */ + char *name; /* element name */ + int num; /* number of elements in this object */ + int size; /* size of element (bytes) or -1 if variable */ + int nprops; /* number of properties for this element */ + PlyProperty **props; /* list of properties in the file */ + char *store_prop; /* flags: property wanted by user? */ + int other_offset; /* offset to un-asked-for props, or -1 if none*/ + int other_size; /* size of other_props structure */ +} PlyElement; + +typedef struct PlyOtherProp { /* describes other properties in an element */ + char *name; /* element name */ + int size; /* size of other_props */ + int nprops; /* number of properties in other_props */ + PlyProperty **props; /* list of properties in other_props */ +} PlyOtherProp; + +typedef struct OtherData { /* for storing other_props for an other element */ + void *other_props; +} OtherData; + +typedef struct OtherElem { /* data for one "other" element */ + char *elem_name; /* names of other elements */ + int elem_count; /* count of instances of each element */ + OtherData **other_data; /* actual property data for the elements */ + PlyOtherProp *other_props; /* description of the property data */ +} OtherElem; + +typedef struct PlyOtherElems { /* "other" elements, not interpreted by user */ + int num_elems; /* number of other elements */ + OtherElem *other_list; /* list of data for other elements */ +} PlyOtherElems; + +typedef struct PlyFile { /* description of PLY file */ + FILE *fp; /* file pointer */ + int file_type; /* ascii or binary */ + float version; /* version number of file */ + int nelems; /* number of elements of object */ + PlyElement **elems; /* list of elements */ + int num_comments; /* number of comments */ + char **comments; /* list of comments */ + int num_obj_info; /* number of items of object information */ + char **obj_info; /* list of object info items */ + PlyElement *which_elem; /* which element we're currently writing */ + PlyOtherElems *other_elems; /* "other" elements from a PLY file */ +} PlyFile; + + /* memory allocation */ +extern char *my_alloc(); +#define myalloc(mem_size) my_alloc((mem_size), __LINE__, __FILE__) + +#ifndef ALLOCN +#define REALLOCN(PTR,TYPE,OLD_N,NEW_N) \ +{ \ + if ((OLD_N) == 0) \ +{ ALLOCN((PTR),TYPE,(NEW_N));} \ + else \ +{ \ + (PTR) = (TYPE *)realloc((PTR),(NEW_N)*sizeof(TYPE)); \ + if (((PTR) == NULL) && ((NEW_N) != 0)) \ +{ \ + fprintf(stderr, "Memory reallocation failed on line %d in %s\n", \ + __LINE__, __FILE__); \ + fprintf(stderr, " tried to reallocate %d->%d\n", \ + (OLD_N), (NEW_N)); \ + exit(-1); \ +} \ + if ((NEW_N)>(OLD_N)) \ + memset((char *)(PTR)+(OLD_N)*sizeof(TYPE), 0, \ + ((NEW_N)-(OLD_N))*sizeof(TYPE)); \ +} \ +} + +#define ALLOCN(PTR,TYPE,N) \ +{ (PTR) = (TYPE *) calloc(((unsigned)(N)),sizeof(TYPE));\ + if ((PTR) == NULL) { \ + fprintf(stderr, "Memory allocation failed on line %d in %s\n", \ + __LINE__, __FILE__); \ + exit(-1); \ + } \ +} + + +#define FREE(PTR) { free((PTR)); (PTR) = NULL; } +#endif + + +/*** delcaration of routines ***/ + +extern PlyFile *ply_write(FILE *, int, const char **, int); +extern PlyFile *ply_open_for_writing(char *, int, const char **, int, float *); +extern void ply_describe_element(PlyFile *, char *, int, int, PlyProperty *); +extern void ply_describe_property(PlyFile *, const char *, PlyProperty *); +extern void ply_element_count(PlyFile *, const char *, int); +extern void ply_header_complete(PlyFile *); +extern void ply_put_element_setup(PlyFile *, const char *); +extern void ply_put_element(PlyFile *, void *); +extern void ply_put_comment(PlyFile *, char *); +extern void ply_put_obj_info(PlyFile *, char *); +extern PlyFile *ply_read(FILE *, int *, char ***); +extern PlyFile *ply_open_for_reading( char *, int *, char ***, int *, float *); +extern PlyProperty **ply_get_element_description(PlyFile *, char *, int*, int*); +extern void ply_get_element_setup( PlyFile *, char *, int, PlyProperty *); +extern int ply_get_property(PlyFile *, char *, PlyProperty *); +extern PlyOtherProp *ply_get_other_properties(PlyFile *, char *, int); +extern void ply_get_element(PlyFile *, void *); +extern char **ply_get_comments(PlyFile *, int *); +extern char **ply_get_obj_info(PlyFile *, int *); +extern void ply_close(PlyFile *); +extern void ply_get_info(PlyFile *, float *, int *); +extern PlyOtherElems *ply_get_other_element (PlyFile *, char *, int); +extern void ply_describe_other_elements ( PlyFile *, PlyOtherElems *); +extern void ply_put_other_elements (PlyFile *); +extern void ply_free_other_elements (PlyOtherElems *); +extern void ply_describe_other_properties(PlyFile *, PlyOtherProp *, int); + +extern int equal_strings(const char *, const char *); + +#ifdef __cplusplus +} +#endif +#include "Geometry.h" +#include + +template< class Real > int PLYType( void ); +template<> inline int PLYType< int >( void ){ return PLY_INT ; } +template<> inline int PLYType< char >( void ){ return PLY_CHAR ; } +template<> inline int PLYType< unsigned char >( void ){ return PLY_UCHAR ; } +template<> inline int PLYType< float >( void ){ return PLY_FLOAT ; } +template<> inline int PLYType< double >( void ){ return PLY_DOUBLE; } +template< class Real > inline int PLYType( void ){ fprintf( stderr , "[ERROR] Unrecognized type\n" ) , exit( 0 ); } + +typedef struct PlyFace +{ + unsigned char nr_vertices; + int *vertices; + int segment; +} PlyFace; +static PlyProperty face_props[] = +{ + { _strdup( "vertex_indices" ) , PLY_INT , PLY_INT , offsetof( PlyFace , vertices ) , 1 , PLY_UCHAR, PLY_UCHAR , offsetof(PlyFace,nr_vertices) }, +}; + + +/////////////////// +// PlyVertexType // +/////////////////// + +// The "Wrapper" class indicates the class to cast to/from in order to support linear operations. +template< class Real > +class PlyVertex +{ +public: + typedef PlyVertex Wrapper; + + const static int ReadComponents=3; + const static int WriteComponents=3; + static PlyProperty ReadProperties[]; + static PlyProperty WriteProperties[]; + + Point3D< Real > point; + + PlyVertex( void ) { ; } + PlyVertex( Point3D< Real > p ) { point=p; } + PlyVertex operator + ( PlyVertex p ) const { return PlyVertex( point+p.point ); } + PlyVertex operator - ( PlyVertex p ) const { return PlyVertex( point-p.point ); } + template< class _Real > PlyVertex operator * ( _Real s ) const { return PlyVertex( point*s ); } + template< class _Real > PlyVertex operator / ( _Real s ) const { return PlyVertex( point/s ); } + PlyVertex& operator += ( PlyVertex p ) { point += p.point ; return *this; } + PlyVertex& operator -= ( PlyVertex p ) { point -= p.point ; return *this; } + template< class _Real > PlyVertex& operator *= ( _Real s ) { point *= s ; return *this; } + template< class _Real > PlyVertex& operator /= ( _Real s ) { point /= s ; return *this; } +}; +template< class Real , class _Real > PlyVertex< Real > operator * ( XForm4x4< _Real > xForm , PlyVertex< Real > v ) { return PlyVertex< Real >( xForm * v.point ); } +template< class Real > PlyProperty PlyVertex< Real >::ReadProperties[]= +{ + { _strdup( "x" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyVertex , point.coords[0] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "y" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyVertex , point.coords[1] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "z" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyVertex , point.coords[2] ) ) , 0 , 0 , 0 , 0 } +}; +template< class Real > PlyProperty PlyVertex< Real >::WriteProperties[]= +{ + { _strdup( "x" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyVertex , point.coords[0] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "y" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyVertex , point.coords[1] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "z" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyVertex , point.coords[2] ) ) , 0 , 0 , 0 , 0 } +}; +template< class Real > +class PlyValueVertex +{ +public: + typedef PlyValueVertex Wrapper; + + const static int ReadComponents=4; + const static int WriteComponents=4; + static PlyProperty ReadProperties[]; + static PlyProperty WriteProperties[]; + + Point3D point; + Real value; + + PlyValueVertex( void ) : value( Real(0) ) { ; } + PlyValueVertex( Point3D< Real > p , Real v ) : point(p) , value(v) { ; } + PlyValueVertex operator + ( PlyValueVertex p ) const { return PlyValueVertex( point+p.point , value+p.value ); } + PlyValueVertex operator - ( PlyValueVertex p ) const { return PlyValueVertex( point-p.value , value-p.value ); } + template< class _Real > PlyValueVertex operator * ( _Real s ) const { return PlyValueVertex( point*s , Real(value*s) ); } + template< class _Real > PlyValueVertex operator / ( _Real s ) const { return PlyValueVertex( point/s , Real(value/s) ); } + PlyValueVertex& operator += ( PlyValueVertex p ) { point += p.point , value += p.value ; return *this; } + PlyValueVertex& operator -= ( PlyValueVertex p ) { point -= p.point , value -= p.value ; return *this; } + template< class _Real > PlyValueVertex& operator *= ( _Real s ) { point *= s , value *= Real(s) ; return *this; } + template< class _Real > PlyValueVertex& operator /= ( _Real s ) { point /= s , value /= Real(s) ; return *this; } +}; +template< class Real , class _Real > PlyValueVertex< Real > operator * ( XForm4x4< _Real > xForm , PlyValueVertex< Real > v ) { return PlyValueVertex< Real >( xForm * v.point , v.value ); } +template< class Real > PlyProperty PlyValueVertex< Real >::ReadProperties[]= +{ + { _strdup( "x" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyValueVertex , point.coords[0] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "y" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyValueVertex , point.coords[1] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "z" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyValueVertex , point.coords[2] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "value" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyValueVertex , value ) ) , 0 , 0 , 0 , 0 } +}; +template< class Real > PlyProperty PlyValueVertex< Real >::WriteProperties[]= +{ + { _strdup( "x" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyValueVertex , point.coords[0] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "y" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyValueVertex , point.coords[1] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "z" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyValueVertex , point.coords[2] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "value" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyValueVertex , value ) ) , 0 , 0 , 0 , 0 } +}; +template< class Real > +class PlyOrientedVertex +{ +public: + typedef PlyOrientedVertex Wrapper; + + const static int ReadComponents=6; + const static int WriteComponents=6; + static PlyProperty ReadProperties[]; + static PlyProperty WriteProperties[]; + + Point3D point , normal; + + PlyOrientedVertex( void ) { ; } + PlyOrientedVertex( Point3D< Real > p , Point3D< Real > n ) : point(p) , normal(n) { ; } + PlyOrientedVertex operator + ( PlyOrientedVertex p ) const { return PlyOrientedVertex( point+p.point , normal+p.normal ); } + PlyOrientedVertex operator - ( PlyOrientedVertex p ) const { return PlyOrientedVertex( point-p.value , normal-p.normal ); } + template< class _Real > PlyOrientedVertex operator * ( _Real s ) const { return PlyOrientedVertex( point*s , normal*s ); } + template< class _Real > PlyOrientedVertex operator / ( _Real s ) const { return PlyOrientedVertex( point/s , normal/s ); } + PlyOrientedVertex& operator += ( PlyOrientedVertex p ) { point += p.point , normal += p.normal ; return *this; } + PlyOrientedVertex& operator -= ( PlyOrientedVertex p ) { point -= p.point , normal -= p.normal ; return *this; } + template< class _Real > PlyOrientedVertex& operator *= ( _Real s ) { point *= s , normal *= s ; return *this; } + template< class _Real > PlyOrientedVertex& operator /= ( _Real s ) { point /= s , normal /= s ; return *this; } +}; +template< class Real , class _Real > PlyOrientedVertex< Real > operator * ( XForm4x4< _Real > xForm , PlyOrientedVertex< Real > v ) { return PlyOrientedVertex< Real >( xForm * v.point , xForm.inverse().transpose() * v.normal ); } +template< class Real > PlyProperty PlyOrientedVertex< Real >::ReadProperties[]= +{ + { _strdup( "x" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyOrientedVertex , point.coords[0] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "y" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyOrientedVertex , point.coords[1] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "z" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyOrientedVertex , point.coords[2] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "nx" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyOrientedVertex , normal.coords[0] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "ny" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyOrientedVertex , normal.coords[1] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "nz" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyOrientedVertex , normal.coords[2] ) ) , 0 , 0 , 0 , 0 } +}; +template< class Real > PlyProperty PlyOrientedVertex< Real >::WriteProperties[]= +{ + { _strdup( "x" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyOrientedVertex , point.coords[0] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "y" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyOrientedVertex , point.coords[1] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "z" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyOrientedVertex , point.coords[2] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "nx" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyOrientedVertex , normal.coords[0] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "ny" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyOrientedVertex , normal.coords[1] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "nz" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyOrientedVertex , normal.coords[2] ) ) , 0 , 0 , 0 , 0 } +}; +template< class Real > +class PlyColorVertex +{ +public: + struct _PlyColorVertex + { + Point3D< Real > point , color; + _PlyColorVertex( void ) { ; } + _PlyColorVertex( Point3D< Real > p , Point3D< Real > c ) : point(p) , color(c) { ; } + _PlyColorVertex( PlyColorVertex< Real > p ){ point = p.point ; for( int c=0 ; c<3 ; c++ ) color[c] = (Real) p.color[c]; } + operator PlyColorVertex< Real > () + { + PlyColorVertex< Real > p; + p.point = point; + for( int c=0 ; c<3 ; c++ ) p.color[c] = (unsigned char)std::max< int >( 0 , std::min< int >( 255 , (int)( color[c]+0.5 ) ) ); + return p; + } + + _PlyColorVertex operator + ( _PlyColorVertex p ) const { return _PlyColorVertex( point+p.point , color+p.color ); } + _PlyColorVertex operator - ( _PlyColorVertex p ) const { return _PlyColorVertex( point-p.value , color-p.color ); } + template< class _Real > _PlyColorVertex operator * ( _Real s ) const { return _PlyColorVertex( point*s , color*s ); } + template< class _Real > _PlyColorVertex operator / ( _Real s ) const { return _PlyColorVertex( point/s , color/s ); } + _PlyColorVertex& operator += ( _PlyColorVertex p ) { point += p.point , color += p.color ; return *this; } + _PlyColorVertex& operator -= ( _PlyColorVertex p ) { point -= p.point , color -= p.color ; return *this; } + template< class _Real > _PlyColorVertex& operator *= ( _Real s ) { point *= s , color *= s ; return *this; } + template< class _Real > _PlyColorVertex& operator /= ( _Real s ) { point /= s , color /= s ; return *this; } + }; + + typedef _PlyColorVertex Wrapper; + + const static int ReadComponents=9; + const static int WriteComponents=6; + static PlyProperty ReadProperties[]; + static PlyProperty WriteProperties[]; + + Point3D< Real > point; + unsigned char color[3]; + + operator Point3D< Real >& (){ return point; } + operator const Point3D< Real >& () const { return point; } + PlyColorVertex( void ) { point.coords[0] = point.coords[1] = point.coords[2] = 0 , color[0] = color[1] = color[2] = 0; } + PlyColorVertex( const Point3D& p ) { point=p; } + PlyColorVertex( const Point3D< Real >& p , const unsigned char c[3] ) { point = p , color[0] = c[0] , color[1] = c[1] , color[2] = c[2]; } +}; +template< class Real , class _Real > PlyColorVertex< Real > operator * ( XForm4x4< _Real > xForm , PlyColorVertex< Real > v ) { return PlyColorVertex< Real >( xForm * v.point , v.color ); } + +template< class Real > PlyProperty PlyColorVertex< Real >::ReadProperties[]= +{ + { _strdup( "x" ) , PLYType< Real >() , PLYType< Real >(), int( offsetof( PlyColorVertex , point.coords[0] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "y" ) , PLYType< Real >() , PLYType< Real >(), int( offsetof( PlyColorVertex , point.coords[1] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "z" ) , PLYType< Real >() , PLYType< Real >(), int( offsetof( PlyColorVertex , point.coords[2] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "red" ) , PLYType< unsigned char >() , PLYType< unsigned char >(), int( offsetof( PlyColorVertex , color[0] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "green" ) , PLYType< unsigned char >() , PLYType< unsigned char >(), int( offsetof( PlyColorVertex , color[1] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "blue" ) , PLYType< unsigned char >() , PLYType< unsigned char >(), int( offsetof( PlyColorVertex , color[2] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "r" ) , PLYType< unsigned char >() , PLYType< unsigned char >(), int( offsetof( PlyColorVertex , color[0] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "g" ) , PLYType< unsigned char >() , PLYType< unsigned char >(), int( offsetof( PlyColorVertex , color[1] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "b" ) , PLYType< unsigned char >() , PLYType< unsigned char >(), int( offsetof( PlyColorVertex , color[2] ) ) , 0 , 0 , 0 , 0 } +}; +template< class Real > PlyProperty PlyColorVertex< Real >::WriteProperties[]= +{ + { _strdup( "x" ) , PLYType< Real >() , PLYType< Real >(), int( offsetof( PlyColorVertex , point.coords[0] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "y" ) , PLYType< Real >() , PLYType< Real >(), int( offsetof( PlyColorVertex , point.coords[1] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "z" ) , PLYType< Real >() , PLYType< Real >(), int( offsetof( PlyColorVertex , point.coords[2] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "red" ) , PLYType< unsigned char >() , PLYType< unsigned char >(), int( offsetof( PlyColorVertex , color[0] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "green" ) , PLYType< unsigned char >() , PLYType< unsigned char >(), int( offsetof( PlyColorVertex , color[1] ) ) , 0 , 0 , 0 , 0 }, + { _strdup( "blue" ) , PLYType< unsigned char >() , PLYType< unsigned char >(), int( offsetof( PlyColorVertex , color[2] ) ) , 0 , 0 , 0 , 0 } +}; +template< class Real > +class PlyColorAndValueVertex +{ +public: + struct _PlyColorAndValueVertex + { + Point3D< Real > point , color; + Real value; + _PlyColorAndValueVertex( void ) : value(0) { ; } + _PlyColorAndValueVertex( Point3D< Real > p , Point3D< Real > c , Real v ) : point(p) , color(c) , value(v) { ; } + _PlyColorAndValueVertex( PlyColorAndValueVertex< Real > p ){ point = p.point ; for( int c=0 ; c<3 ; c++ ) color[c] = (Real) p.color[c] ; value = p.value; } + operator PlyColorAndValueVertex< Real > () + { + PlyColorAndValueVertex< Real > p; + p.point = point; + for( int c=0 ; c<3 ; c++ ) p.color[c] = (unsigned char)std::max< int >( 0 , std::min< int >( 255 , (int)( color[c]+0.5 ) ) ); + p.value = value; + return p; + } + + _PlyColorAndValueVertex operator + ( _PlyColorAndValueVertex p ) const { return _PlyColorAndValueVertex( point+p.point , color+p.color , value+p.value ); } + _PlyColorAndValueVertex operator - ( _PlyColorAndValueVertex p ) const { return _PlyColorAndValueVertex( point-p.value , color-p.color , value+p.value ); } + template< class _Real > _PlyColorAndValueVertex operator * ( _Real s ) const { return _PlyColorAndValueVertex( point*s , color*s , value*s ); } + template< class _Real > _PlyColorAndValueVertex operator / ( _Real s ) const { return _PlyColorAndValueVertex( point/s , color/s , value/s ); } + _PlyColorAndValueVertex& operator += ( _PlyColorAndValueVertex p ) { point += p.point , color += p.color , value += p.value ; return *this; } + _PlyColorAndValueVertex& operator -= ( _PlyColorAndValueVertex p ) { point -= p.point , color -= p.color , value -= p.value ; return *this; } + template< class _Real > _PlyColorAndValueVertex& operator *= ( _Real s ) { point *= s , color *= s , value *= (Real)s ; return *this; } + template< class _Real > _PlyColorAndValueVertex& operator /= ( _Real s ) { point /= s , color /= s , value /= (Real)s ; return *this; } + }; + + typedef _PlyColorAndValueVertex Wrapper; + + const static int ReadComponents=10; + const static int WriteComponents=7; + static PlyProperty ReadProperties[]; + static PlyProperty WriteProperties[]; + + Point3D< Real > point; + unsigned char color[3]; + Real value; + + operator Point3D< Real >& (){ return point; } + operator const Point3D< Real >& () const { return point; } + PlyColorAndValueVertex( void ) { point.coords[0] = point.coords[1] = point.coords[2] = (Real)0 , color[0] = color[1] = color[2] = 0 , value = (Real)0; } + PlyColorAndValueVertex( const Point3D< Real >& p ) { point=p; } + PlyColorAndValueVertex( const Point3D< Real >& p , const unsigned char c[3] , Real v) { point = p , color[0] = c[0] , color[1] = c[1] , color[2] = c[2] , value = v; } +}; +template< class Real , class _Real > PlyColorAndValueVertex< Real > operator * ( XForm4x4< _Real > xForm , PlyColorAndValueVertex< Real > v ) { return PlyColorAndValueVertex< Real >( xForm * v.point , v.color , v.value ); } +template< class Real > PlyProperty PlyColorAndValueVertex< Real >::ReadProperties[]= +{ + { _strdup( "x" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyColorAndValueVertex , point.coords[0] ) ) , 0 , 0 , 0 , 0 } , + { _strdup( "y" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyColorAndValueVertex , point.coords[1] ) ) , 0 , 0 , 0 , 0 } , + { _strdup( "z" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyColorAndValueVertex , point.coords[2] ) ) , 0 , 0 , 0 , 0 } , + { _strdup( "value" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyColorAndValueVertex , value ) ) , 0 , 0 , 0 , 0 } , + { _strdup( "red" ) , PLYType< unsigned char >() , PLYType< unsigned char >() , int( offsetof( PlyColorAndValueVertex , color[0] ) ) , 0 , 0 , 0 , 0 } , + { _strdup( "green" ) , PLYType< unsigned char >() , PLYType< unsigned char >() , int( offsetof( PlyColorAndValueVertex , color[1] ) ) , 0 , 0 , 0 , 0 } , + { _strdup( "blue" ) , PLYType< unsigned char >() , PLYType< unsigned char >() , int( offsetof( PlyColorAndValueVertex , color[2] ) ) , 0 , 0 , 0 , 0 } , + { _strdup( "r" ) , PLYType< unsigned char >() , PLYType< unsigned char >() , int( offsetof( PlyColorAndValueVertex , color[0] ) ) , 0 , 0 , 0 , 0 } , + { _strdup( "g" ) , PLYType< unsigned char >() , PLYType< unsigned char >() , int( offsetof( PlyColorAndValueVertex , color[1] ) ) , 0 , 0 , 0 , 0 } , + { _strdup( "b" ) , PLYType< unsigned char >() , PLYType< unsigned char >() , int( offsetof( PlyColorAndValueVertex , color[2] ) ) , 0 , 0 , 0 , 0 } +}; +template< class Real > PlyProperty PlyColorAndValueVertex< Real >::WriteProperties[]= +{ + { _strdup( "x" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyColorAndValueVertex , point.coords[0] ) ) , 0 , 0 , 0 , 0 } , + { _strdup( "y" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyColorAndValueVertex , point.coords[1] ) ) , 0 , 0 , 0 , 0 } , + { _strdup( "z" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyColorAndValueVertex , point.coords[2] ) ) , 0 , 0 , 0 , 0 } , + { _strdup( "value" ) , PLYType< Real >() , PLYType< Real >() , int( offsetof( PlyColorAndValueVertex , value ) ) , 0 , 0 , 0 , 0 } , + { _strdup( "red" ) , PLYType< unsigned char >() , PLYType< unsigned char >() , int( offsetof( PlyColorAndValueVertex , color[0] ) ) , 0 , 0 , 0 , 0 } , + { _strdup( "green" ) , PLYType< unsigned char >() , PLYType< unsigned char >() , int( offsetof( PlyColorAndValueVertex , color[1] ) ) , 0 , 0 , 0 , 0 } , + { _strdup( "blue" ) , PLYType< unsigned char >() , PLYType< unsigned char >() , int( offsetof( PlyColorAndValueVertex , color[2] ) ) , 0 , 0 , 0 , 0 } +}; + +template< class Vertex , class Real > +int PlyWritePolygons( char* fileName , CoredMeshData< Vertex >* mesh , int file_type , const Point3D< float >& translate , float scale , char** comments=NULL , int commentNum=0 , XForm4x4< Real > xForm=XForm4x4< Real >::Identity() ); + +template< class Vertex , class Real > +int PlyWritePolygons( char* fileName , CoredMeshData< Vertex >* mesh , int file_type , char** comments=NULL , int commentNum=0 , XForm4x4< Real > xForm=XForm4x4< Real >::Identity() ); + +inline bool PlyReadHeader( char* fileName , PlyProperty* properties , int propertyNum , bool* readFlags , int& file_type ) +{ + int nr_elems; + char **elist; + float version; + PlyFile* ply; + char* elem_name; + int num_elems; + int nr_props; + PlyProperty** plist; + + ply = ply_open_for_reading( fileName , &nr_elems , &elist , &file_type , &version ); + if( !ply ) return false; + + for( int i=0 ; ielems[i]->name ); + free( ply->elems[i]->store_prop ); + for( int j=0 ; jelems[i]->nprops ; j++ ) + { + free( ply->elems[i]->props[j]->name ); + free( ply->elems[i]->props[j] ); + } + free( ply->elems[i]->props ); + } + for( int i=0 ; ielems[i] ); + free( ply->elems ); + for( int i=0 ; inum_comments ; i++ ) free( ply->comments[i] ); + free( ply->comments ); + for( int i=0 ; inum_obj_info ; i++ ) free( ply->obj_info[i] ); + free( ply->obj_info ); + ply_free_other_elements( ply->other_elems ); + + for( int i=0 ; iname ); + free( plist[j] ); + } + free( plist ); + } // for each type of element + + for( int i=0 ; ielems[i]->name ); + free( ply->elems[i]->store_prop ); + for( int j=0 ; jelems[i]->nprops ; j++ ) + { + free( ply->elems[i]->props[j]->name ); + free( ply->elems[i]->props[j] ); + } + if( ply->elems[i]->props && ply->elems[i]->nprops ) free(ply->elems[i]->props); + } + for( int i=0 ; ielems[i]); + free( ply->elems) ; + for( int i=0 ; inum_comments ; i++ ) free( ply->comments[i] ); + free( ply->comments ); + for( int i=0 ; inum_obj_info ; i++ ) free( ply->obj_info[i] ); + free( ply->obj_info ); + ply_free_other_elements(ply->other_elems); + + + for( int i=0 ; i +int PlyReadPolygons(char* fileName, + std::vector& vertices,std::vector >& polygons, + PlyProperty* properties,int propertyNum, + int& file_type, + char*** comments=NULL,int* commentNum=NULL , bool* readFlags=NULL ); + +template +int PlyWritePolygons(char* fileName, + const std::vector& vertices,const std::vector >& polygons, + PlyProperty* properties,int propertyNum, + int file_type, + char** comments=NULL,const int& commentNum=0); + +template +int PlyWritePolygons(char* fileName, + const std::vector& vertices , const std::vector< std::vector< int > >& polygons, + PlyProperty* properties,int propertyNum, + int file_type, + char** comments,const int& commentNum) +{ + int nr_vertices=int(vertices.size()); + int nr_faces=int(polygons.size()); + float version; + const char *elem_names[] = { "vertex" , "face" }; + PlyFile *ply = ply_open_for_writing( fileName , 2 , elem_names , file_type , &version ); + if (!ply){return 0;} + + // + // describe vertex and face properties + // + ply_element_count(ply, "vertex", nr_vertices); + for(int i=0;imaxFaceVerts) + { + delete[] ply_face.vertices; + maxFaceVerts=int(polygons[i].size()); + ply_face.vertices=new int[maxFaceVerts]; + } + ply_face.nr_vertices=int(polygons[i].size()); + for(int j=0;j +int PlyReadPolygons(char* fileName, + std::vector& vertices , std::vector >& polygons , + PlyProperty* properties , int propertyNum , + int& file_type , + char*** comments , int* commentNum , bool* readFlags ) +{ + int nr_elems; + char **elist; + float version; + int i,j,k; + PlyFile* ply; + char* elem_name; + int num_elems; + int nr_props; + PlyProperty** plist; + PlyFace ply_face; + + ply = ply_open_for_reading(fileName, &nr_elems, &elist, &file_type, &version); + if(!ply) return 0; + + if(comments) + { + (*comments)=new char*[*commentNum+ply->num_comments]; + for(int i=0;inum_comments;i++) + (*comments)[i]=_strdup(ply->comments[i]); + *commentNum=ply->num_comments; + } + + for (i=0; i < nr_elems; i++) { + elem_name = elist[i]; + plist = ply_get_element_description(ply, elem_name, &num_elems, &nr_props); + if(!plist) + { + for(i=0;ielems[i]->name); + free(ply->elems[i]->store_prop); + for(j=0;jelems[i]->nprops;j++){ + free(ply->elems[i]->props[j]->name); + free(ply->elems[i]->props[j]); + } + free(ply->elems[i]->props); + } + for(i=0;ielems[i]);} + free(ply->elems); + for(i=0;inum_comments;i++){free(ply->comments[i]);} + free(ply->comments); + for(i=0;inum_obj_info;i++){free(ply->obj_info[i]);} + free(ply->obj_info); + ply_free_other_elements (ply->other_elems); + + for(i=0;iname); + free(plist[j]); + } + free(plist); + } // for each type of element + + for(i=0;ielems[i]->name); + free(ply->elems[i]->store_prop); + for(j=0;jelems[i]->nprops;j++){ + free(ply->elems[i]->props[j]->name); + free(ply->elems[i]->props[j]); + } + if(ply->elems[i]->props && ply->elems[i]->nprops){free(ply->elems[i]->props);} + } + for(i=0;ielems[i]);} + free(ply->elems); + for(i=0;inum_comments;i++){free(ply->comments[i]);} + free(ply->comments); + for(i=0;inum_obj_info;i++){free(ply->obj_info[i]);} + free(ply->obj_info); + ply_free_other_elements (ply->other_elems); + + + for(i=0;i +int PlyWritePolygons( char* fileName , CoredMeshData< Vertex >* mesh , int file_type , const Point3D& translate , float scale , char** comments , int commentNum , XForm4x4< Real > xForm ) +{ + int i; + int nr_vertices=int(mesh->outOfCorePointCount()+mesh->inCorePoints.size()); + int nr_faces=mesh->polygonCount(); + float version; + const char *elem_names[] = { "vertex" , "face" }; + PlyFile *ply = ply_open_for_writing( fileName , 2 , elem_names , file_type , &version ); + if( !ply ) return 0; + + mesh->resetIterator(); + + // + // describe vertex and face properties + // + ply_element_count( ply , "vertex" , nr_vertices ); + for( int i=0 ; iinCorePoints.size() ) ; i++ ) + { + Vertex vertex = xForm * ( mesh->inCorePoints[i] * scale + translate ); + ply_put_element(ply, (void *) &vertex); + } + for( i=0; ioutOfCorePointCount() ; i++ ) + { + Vertex vertex; + mesh->nextOutOfCorePoint( vertex ); + vertex = xForm * ( vertex * scale +translate ); + ply_put_element(ply, (void *) &vertex); + } // for, write vertices + + // write faces + std::vector< CoredVertexIndex > polygon; + ply_put_element_setup( ply , "face" ); + for( i=0 ; inextPolygon( polygon ); + ply_face.nr_vertices = int( polygon.size() ); + ply_face.vertices = new int[ polygon.size() ]; + for( int i=0 ; iinCorePoints.size() ); + ply_put_element( ply, (void *) &ply_face ); + delete[] ply_face.vertices; + } // for, write faces + + ply_close( ply ); + return 1; +} +template< class Vertex , class Real > +int PlyWritePolygons( char* fileName , CoredMeshData< Vertex >* mesh , int file_type , char** comments , int commentNum , XForm4x4< Real > xForm ) +{ + int i; + int nr_vertices=int(mesh->outOfCorePointCount()+mesh->inCorePoints.size()); + int nr_faces=mesh->polygonCount(); + float version; + const char *elem_names[] = { "vertex" , "face" }; + PlyFile *ply = ply_open_for_writing( fileName , 2 , elem_names , file_type , &version ); + if( !ply ) return 0; + + mesh->resetIterator(); + + // + // describe vertex and face properties + // + ply_element_count( ply , "vertex" , nr_vertices ); + for( int i=0 ; iinCorePoints.size() ) ; i++ ) + { + Vertex vertex = xForm * mesh->inCorePoints[i]; + ply_put_element(ply, (void *) &vertex); + } + for( i=0; ioutOfCorePointCount() ; i++ ) + { + Vertex vertex; + mesh->nextOutOfCorePoint( vertex ); + vertex = xForm * ( vertex ); + ply_put_element(ply, (void *) &vertex); + } // for, write vertices + + // write faces + std::vector< CoredVertexIndex > polygon; + ply_put_element_setup( ply , "face" ); + for( i=0 ; inextPolygon( polygon ); + ply_face.nr_vertices = int( polygon.size() ); + ply_face.vertices = new int[ polygon.size() ]; + for( int i=0 ; iinCorePoints.size() ); + ply_put_element( ply, (void *) &ply_face ); + delete[] ply_face.vertices; + } // for, write faces + + ply_close( ply ); + return 1; +} +inline int PlyDefaultFileType(void){return PLY_ASCII;} + +#endif /* !__PLY_H__ */ diff --git a/colmap/include/colmap/lib/PoissonRecon/PointStream.h b/colmap/include/colmap/lib/PoissonRecon/PointStream.h new file mode 100644 index 0000000000000000000000000000000000000000..4d08d2aedf16f61a476e0a635a5402840710d465 --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/PointStream.h @@ -0,0 +1,175 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior writften 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +#ifndef POINT_STREAM_INCLUDED +#define POINT_STREAM_INCLUDED +#include "Ply.h" + +template< class Real > +struct OrientedPoint3D +{ + Point3D< Real > p , n; + OrientedPoint3D( Point3D< Real > pp=Point3D< Real >() , Point3D< Real > nn=Point3D< Real >() ) : p(pp) , n(nn) { ; } +}; + +template< class Real > +class OrientedPointStream +{ +public: + virtual ~OrientedPointStream( void ){} + virtual void reset( void ) = 0; + virtual bool nextPoint( OrientedPoint3D< Real >& p ) = 0; +}; +template< class Real , class Data > +class OrientedPointStreamWithData : public OrientedPointStream< Real > +{ +public: + virtual ~OrientedPointStreamWithData( void ){} + virtual void reset( void ) = 0; + virtual bool nextPoint( OrientedPoint3D< Real >& p , Data& d ) = 0; + + virtual bool nextPoint( OrientedPoint3D< Real >& p ){ Data d ; return nextPoint( p , d ); } +}; + +template< class Real > +class MemoryOrientedPointStream : public OrientedPointStream< Real > +{ + const OrientedPoint3D< Real >* _points; + size_t _pointCount; + size_t _current; +public: + MemoryOrientedPointStream( size_t pointCount , const OrientedPoint3D< Real >* points ); + ~MemoryOrientedPointStream( void ); + void reset( void ); + bool nextPoint( OrientedPoint3D< Real >& p ); +}; + +template< class Real , class Data > +class MemoryOrientedPointStreamWithData : public OrientedPointStreamWithData< Real , Data > +{ + const std::pair< OrientedPoint3D< Real > , Data >* _points; + size_t _pointCount; + size_t _current; +public: + MemoryOrientedPointStreamWithData( size_t pointCount , const std::pair< OrientedPoint3D< Real > , Data >* points ); + ~MemoryOrientedPointStreamWithData( void ); + void reset( void ); + bool nextPoint( OrientedPoint3D< Real >& p , Data& d ); +}; + +template< class Real > +class ASCIIOrientedPointStream : public OrientedPointStream< Real > +{ + FILE* _fp; +public: + ASCIIOrientedPointStream( const char* fileName ); + ~ASCIIOrientedPointStream( void ); + void reset( void ); + bool nextPoint( OrientedPoint3D< Real >& p ); +}; + +template< class Real , class Data > +class ASCIIOrientedPointStreamWithData : public OrientedPointStreamWithData< Real , Data > +{ + FILE* _fp; + Data (*_readData)( FILE* ); +public: + ASCIIOrientedPointStreamWithData( const char* fileName , Data (*readData)( FILE* ) ); + ~ASCIIOrientedPointStreamWithData( void ); + void reset( void ); + bool nextPoint( OrientedPoint3D< Real >& p , Data& d ); +}; + +template< class Real > +class BinaryOrientedPointStream : public OrientedPointStream< Real > +{ + FILE* _fp; + static const int POINT_BUFFER_SIZE=1024; + OrientedPoint3D< Real > _pointBuffer[ POINT_BUFFER_SIZE ]; + int _pointsInBuffer , _currentPointIndex; +public: + BinaryOrientedPointStream( const char* filename ); + ~BinaryOrientedPointStream( void ); + void reset( void ); + bool nextPoint( OrientedPoint3D< Real >& p ); +}; + +template< class Real , class Data > +class BinaryOrientedPointStreamWithData : public OrientedPointStreamWithData< Real , Data > +{ + FILE* _fp; + static const int POINT_BUFFER_SIZE=1024; + std::pair< OrientedPoint3D< Real > , Data > _pointBuffer[ POINT_BUFFER_SIZE ]; + int _pointsInBuffer , _currentPointIndex; +public: + BinaryOrientedPointStreamWithData( const char* filename ); + ~BinaryOrientedPointStreamWithData( void ); + void reset( void ); + bool nextPoint( OrientedPoint3D< Real >& p , Data& d ); +}; + +template< class Real > +class PLYOrientedPointStream : public OrientedPointStream< Real > +{ + char* _fileName; + PlyFile* _ply; + int _nr_elems; + char **_elist; + + int _pCount , _pIdx; + void _free( void ); +public: + PLYOrientedPointStream( const char* fileName ); + ~PLYOrientedPointStream( void ); + void reset( void ); + bool nextPoint( OrientedPoint3D< Real >& p ); +}; + +template< class Real , class Data > +class PLYOrientedPointStreamWithData : public OrientedPointStreamWithData< Real , Data > +{ + struct _PlyOrientedVertexWithData : public PlyOrientedVertex< Real > { Data data; }; + char* _fileName; + PlyFile* _ply; + int _nr_elems; + char **_elist; + PlyProperty* _dataProperties; + int _dataPropertiesCount; + bool (*_validationFunction)( const bool* ); + + int _pCount , _pIdx; + void _free( void ); +public: + PLYOrientedPointStreamWithData( const char* fileName , const PlyProperty* dataProperties , int dataPropertiesCount , bool (*validationFunction)( const bool* )=NULL ); + ~PLYOrientedPointStreamWithData( void ); + void reset( void ); + bool nextPoint( OrientedPoint3D< Real >& p , Data& d ); +}; + +#include "PointStream.inl" +#endif // POINT_STREAM_INCLUDED diff --git a/colmap/include/colmap/lib/PoissonRecon/PointStream.inl b/colmap/include/colmap/lib/PoissonRecon/PointStream.inl new file mode 100644 index 0000000000000000000000000000000000000000..1808fa46fa6160586afa46f307038cd2aabbf997 --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/PointStream.inl @@ -0,0 +1,408 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + + +/////////////////////////////// +// MemoryOrientedPointStream // +/////////////////////////////// +template< class Real > +MemoryOrientedPointStream< Real >::MemoryOrientedPointStream( size_t pointCount , const OrientedPoint3D< Real >* points ){ _points = points , _pointCount = pointCount , _current = 0; } +template< class Real > +MemoryOrientedPointStream< Real >::~MemoryOrientedPointStream( void ){ ; } +template< class Real > +void MemoryOrientedPointStream< Real >::reset( void ) { _current=0; } +template< class Real > +bool MemoryOrientedPointStream< Real >::nextPoint( OrientedPoint3D< Real >& p ) +{ + if( _current>=_pointCount ) return false; + p = _points[_current]; + _current++; + return true; +} + +////////////////////////////// +// ASCIIOrientedPointStream // +////////////////////////////// +template< class Real > +ASCIIOrientedPointStream< Real >::ASCIIOrientedPointStream( const char* fileName ) +{ + _fp = fopen( fileName , "r" ); + if( !_fp ) fprintf( stderr , "Failed to open file for reading: %s\n" , fileName ) , exit( 0 ); +} +template< class Real > +ASCIIOrientedPointStream< Real >::~ASCIIOrientedPointStream( void ) +{ + fclose( _fp ); + _fp = NULL; +} +template< class Real > +void ASCIIOrientedPointStream< Real >::reset( void ) { fseek( _fp , SEEK_SET , 0 ); } +template< class Real > +bool ASCIIOrientedPointStream< Real >::nextPoint( OrientedPoint3D< Real >& p ) +{ + float c[2*3]; + if( fscanf( _fp , " %f %f %f %f %f %f " , &c[0] , &c[1] , &c[2] , &c[3] , &c[4] , &c[5] )!=2*3 ) return false; + p.p[0] = c[0] , p.p[1] = c[1] , p.p[2] = c[2]; + p.n[0] = c[3] , p.n[1] = c[4] , p.n[2] = c[5]; + return true; +} + +/////////////////////////////// +// BinaryOrientedPointStream // +/////////////////////////////// +template< class Real > +BinaryOrientedPointStream< Real >::BinaryOrientedPointStream( const char* fileName ) +{ + _pointsInBuffer = _currentPointIndex = 0; + _fp = fopen( fileName , "rb" ); + if( !_fp ) fprintf( stderr , "Failed to open file for reading: %s\n" , fileName ) , exit( 0 ); +} +template< class Real > +BinaryOrientedPointStream< Real >::~BinaryOrientedPointStream( void ) +{ + fclose( _fp ); + _fp = NULL; +} +template< class Real > +void BinaryOrientedPointStream< Real >::reset( void ) +{ + fseek( _fp , SEEK_SET , 0 ); + _pointsInBuffer = _currentPointIndex = 0; +} +template< class Real > +bool BinaryOrientedPointStream< Real >::nextPoint( OrientedPoint3D< Real >& p ) +{ + if( _currentPointIndex<_pointsInBuffer ) + { + p = _pointBuffer[ _currentPointIndex ]; + _currentPointIndex++; + return true; + } + else + { + _currentPointIndex = 0; + _pointsInBuffer = int( fread( _pointBuffer , sizeof( OrientedPoint3D< Real > ) , POINT_BUFFER_SIZE , _fp ) ); + if( !_pointsInBuffer ) return false; + else return nextPoint( p ); + } +} + +//////////////////////////// +// PLYOrientedPointStream // +//////////////////////////// +template< class Real > +PLYOrientedPointStream< Real >::PLYOrientedPointStream( const char* fileName ) +{ + _fileName = new char[ strlen( fileName )+1 ]; + strcpy( _fileName , fileName ); + _ply = NULL; + reset(); +} +template< class Real > +void PLYOrientedPointStream< Real >::reset( void ) +{ + int fileType; + float version; + PlyProperty** plist; + if( _ply ) _free(); + _ply = ply_open_for_reading( _fileName, &_nr_elems, &_elist, &fileType, &version ); + if( !_ply ) + { + fprintf( stderr, "[ERROR] Failed to open ply file for reading: %s\n" , _fileName ); + exit( 0 ); + } + bool foundVertices = false; + for( int i=0 ; i<_nr_elems ; i++ ) + { + int num_elems; + int nr_props; + char* elem_name = _elist[i]; + plist = ply_get_element_description( _ply , elem_name , &num_elems , &nr_props ); + if( !plist ) + { + fprintf( stderr , "[ERROR] Failed to get element description: %s\n" , elem_name ); + exit( 0 ); + } + + if( equal_strings( "vertex" , elem_name ) ) + { + foundVertices = true; + _pCount = num_elems , _pIdx = 0; + for( int i=0 ; i::ReadComponents ; i++ ) + if( !ply_get_property( _ply , elem_name , &(PlyOrientedVertex< Real >::ReadProperties[i]) ) ) + { + fprintf( stderr , "[ERROR] Failed to find property in ply file: %s\n" , PlyOrientedVertex< Real >::ReadProperties[i].name ); + exit( 0 ); + } + } + for( int j=0 ; jname ); + free( plist[j] ); + } + free( plist ); + if( foundVertices ) break; + } + if( !foundVertices ) + { + fprintf( stderr , "[ERROR] Could not find vertices in ply file\n" ); + exit( 0 ); + } +} +template< class Real > +void PLYOrientedPointStream< Real >::_free( void ) +{ + if( _ply ) ply_close( _ply ) , _ply = NULL; + if( _elist ) + { + for( int i=0 ; i<_nr_elems ; i++ ) free( _elist[i] ); + free( _elist ); + } +} +template< class Real > +PLYOrientedPointStream< Real >::~PLYOrientedPointStream( void ) +{ + _free(); + if( _fileName ) delete[] _fileName , _fileName = NULL; +} +template< class Real > +bool PLYOrientedPointStream< Real >::nextPoint( OrientedPoint3D< Real >& p ) +{ + if( _pIdx<_pCount ) + { + PlyOrientedVertex< Real > op; + ply_get_element( _ply, (void *)&op ); + p.p = op.point; + p.n = op.normal; + _pIdx++; + return true; + } + else return false; +} + +/////////////////////////////////////// +// MemoryOrientedPointStreamWithData // +/////////////////////////////////////// +template< class Real , class Data > +MemoryOrientedPointStreamWithData< Real , Data >::MemoryOrientedPointStreamWithData( size_t pointCount , const std::pair< OrientedPoint3D< Real > , Data >* points ){ _points = points , _pointCount = pointCount , _current = 0; } +template< class Real , class Data > +MemoryOrientedPointStreamWithData< Real , Data >::~MemoryOrientedPointStreamWithData( void ){ ; } +template< class Real , class Data > +void MemoryOrientedPointStreamWithData< Real , Data >::reset( void ) { _current=0; } +template< class Real , class Data > +bool MemoryOrientedPointStreamWithData< Real , Data >::nextPoint( OrientedPoint3D< Real >& p , Data& d ) +{ + if( _current>=_pointCount ) return false; + p = _points[_current].first; + d = _points[_current].second; + _current++; + return true; +} + +////////////////////////////////////// +// ASCIIOrientedPointStreamWithData // +////////////////////////////////////// +template< class Real , class Data > +ASCIIOrientedPointStreamWithData< Real , Data >::ASCIIOrientedPointStreamWithData( const char* fileName , Data (*readData)( FILE* ) ) : _readData( readData ) +{ + _fp = fopen( fileName , "r" ); + if( !_fp ) fprintf( stderr , "Failed to open file for reading: %s\n" , fileName ) , exit( 0 ); +} +template< class Real , class Data > +ASCIIOrientedPointStreamWithData< Real , Data >::~ASCIIOrientedPointStreamWithData( void ) +{ + fclose( _fp ); + _fp = NULL; +} +template< class Real , class Data > +void ASCIIOrientedPointStreamWithData< Real , Data >::reset( void ) { fseek( _fp , SEEK_SET , 0 ); } +template< class Real , class Data > +bool ASCIIOrientedPointStreamWithData< Real , Data >::nextPoint( OrientedPoint3D< Real >& p , Data& d ) +{ + float c[2*3]; + if( fscanf( _fp , " %f %f %f %f %f %f " , &c[0] , &c[1] , &c[2] , &c[3] , &c[4] , &c[5] )!=2*3 ) return false; + p.p[0] = c[0] , p.p[1] = c[1] , p.p[2] = c[2]; + p.n[0] = c[3] , p.n[1] = c[4] , p.n[2] = c[5]; + d = _readData( _fp ); + return true; +} + +/////////////////////////////////////// +// BinaryOrientedPointStreamWithData // +/////////////////////////////////////// +template< class Real , class Data > +BinaryOrientedPointStreamWithData< Real , Data >::BinaryOrientedPointStreamWithData( const char* fileName ) +{ + _pointsInBuffer = _currentPointIndex = 0; + _fp = fopen( fileName , "rb" ); + if( !_fp ) fprintf( stderr , "Failed to open file for reading: %s\n" , fileName ) , exit( 0 ); +} +template< class Real , class Data > +BinaryOrientedPointStreamWithData< Real , Data >::~BinaryOrientedPointStreamWithData( void ) +{ + fclose( _fp ); + _fp = NULL; +} +template< class Real , class Data > +void BinaryOrientedPointStreamWithData< Real , Data >::reset( void ) +{ + fseek( _fp , SEEK_SET , 0 ); + _pointsInBuffer = _currentPointIndex = 0; +} +template< class Real , class Data > +bool BinaryOrientedPointStreamWithData< Real , Data >::nextPoint( OrientedPoint3D< Real >& p , Data& d ) +{ + if( _currentPointIndex<_pointsInBuffer ) + { + p = _pointBuffer[ _currentPointIndex ].first; + d = _pointBuffer[ _currentPointIndex ].second; + _currentPointIndex++; + return true; + } + else + { + _currentPointIndex = 0; + _pointsInBuffer = int( fread( _pointBuffer , sizeof( std::pair< OrientedPoint3D< Real > , Data > ) , POINT_BUFFER_SIZE , _fp ) ); + if( !_pointsInBuffer ) return false; + else return nextPoint( p , d ); + } +} + +//////////////////////////////////// +// PLYOrientedPointStreamWithData // +//////////////////////////////////// +template< class Real , class Data > +PLYOrientedPointStreamWithData< Real , Data >::PLYOrientedPointStreamWithData( const char* fileName , const PlyProperty* dataProperties , int dataPropertiesCount , bool (*validationFunction)( const bool* ) ) : _dataPropertiesCount( dataPropertiesCount ) , _validationFunction( validationFunction ) +{ + _dataProperties = new PlyProperty[ _dataPropertiesCount ]; + memcpy( _dataProperties , dataProperties , sizeof(PlyProperty) * _dataPropertiesCount ); + for( int i=0 ; i<_dataPropertiesCount ; i++ ) _dataProperties[i].offset += sizeof( PlyOrientedVertex< Real > ); + _fileName = new char[ strlen( fileName )+1 ]; + strcpy( _fileName , fileName ); + _ply = NULL; + reset(); +} +template< class Real , class Data > +void PLYOrientedPointStreamWithData< Real , Data >::reset( void ) +{ + int fileType; + float version; + PlyProperty** plist; + if( _ply ) _free(); + _ply = ply_open_for_reading( _fileName, &_nr_elems, &_elist, &fileType, &version ); + if( !_ply ) + { + fprintf( stderr, "[ERROR] Failed to open ply file for reading: %s\n" , _fileName ); + exit( 0 ); + } + bool foundVertices = false; + for( int i=0 ; i<_nr_elems ; i++ ) + { + int num_elems; + int nr_props; + char* elem_name = _elist[i]; + plist = ply_get_element_description( _ply , elem_name , &num_elems , &nr_props ); + if( !plist ) + { + fprintf( stderr , "[ERROR] Failed to get element description: %s\n" , elem_name ); + exit( 0 ); + } + + if( equal_strings( "vertex" , elem_name ) ) + { + foundVertices = true; + _pCount = num_elems , _pIdx = 0; + for( int i=0 ; i::ReadComponents ; i++ ) + if( !ply_get_property( _ply , elem_name , &(PlyOrientedVertex< Real >::ReadProperties[i]) ) ) + { + fprintf( stderr , "[ERROR] Failed to find property in ply file: %s\n" , PlyOrientedVertex< Real >::ReadProperties[i].name ); + exit( 0 ); + } + if( _validationFunction ) + { + bool* properties = new bool[_dataPropertiesCount]; + for( int i=0 ; i<_dataPropertiesCount ; i++ ) + if( !ply_get_property( _ply , elem_name , &(_dataProperties[i]) ) ) properties[i] = false; + else properties[i] = true; + bool valid = _validationFunction( properties ); + delete[] properties; + if( !valid ) fprintf( stderr , "[ERROR] Failed to validate properties in file\n" ) , exit( 0 ); + } + else + { + for( int i=0 ; i<_dataPropertiesCount ; i++ ) + if( !ply_get_property( _ply , elem_name , &(_dataProperties[i]) ) ) + fprintf( stderr , "[WARNING] Failed to find property in ply file: %s\n" , _dataProperties[i].name ); + } + } + for( int j=0 ; jname ); + free( plist[j] ); + } + free( plist ); + if( foundVertices ) break; + } + if( !foundVertices ) + { + fprintf( stderr , "[ERROR] Could not find vertices in ply file\n" ); + exit( 0 ); + } +} +template< class Real , class Data > +void PLYOrientedPointStreamWithData< Real , Data >::_free( void ) +{ + if( _ply ) ply_close( _ply ) , _ply = NULL; + if( _elist ) + { + for( int i=0 ; i<_nr_elems ; i++ ) free( _elist[i] ); + free( _elist ); + } +} +template< class Real , class Data > +PLYOrientedPointStreamWithData< Real , Data >::~PLYOrientedPointStreamWithData( void ) +{ + _free(); + if( _fileName ) delete[] _fileName , _fileName = NULL; + if( _dataProperties ) delete[] _dataProperties , _dataProperties = NULL; +} +template< class Real , class Data > +bool PLYOrientedPointStreamWithData< Real , Data >::nextPoint( OrientedPoint3D< Real >& p , Data& d ) +{ + if( _pIdx<_pCount ) + { + _PlyOrientedVertexWithData op; + ply_get_element( _ply, (void *)&op ); + p.p = op.point; + p.n = op.normal; + d = op.data; + _pIdx++; + return true; + } + else return false; +} diff --git a/colmap/include/colmap/lib/PoissonRecon/PoissonRecon.h b/colmap/include/colmap/lib/PoissonRecon/PoissonRecon.h new file mode 100644 index 0000000000000000000000000000000000000000..d26379986ccb835c8fc2d4674e89f0c60c6cfb1c --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/PoissonRecon.h @@ -0,0 +1 @@ +int PoissonRecon(int argc, char* argv[]); diff --git a/colmap/include/colmap/lib/PoissonRecon/Polynomial.h b/colmap/include/colmap/lib/PoissonRecon/Polynomial.h new file mode 100644 index 0000000000000000000000000000000000000000..397b7bdbb9237e6fb75a62eaac5fecfdad0cc811 --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/Polynomial.h @@ -0,0 +1,100 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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. +*/ + +#ifndef POLYNOMIAL_INCLUDED +#define POLYNOMIAL_INCLUDED + +#define NEW_POLYNOMIAL_CODE 1 + +#include + +template< int Degree > +class Polynomial +{ +public: + double coefficients[Degree+1]; + + Polynomial(void); + template + Polynomial(const Polynomial& P); + double operator()( double t ) const; + double integral( double tMin , double tMax ) const; + + int operator == (const Polynomial& p) const; + int operator != (const Polynomial& p) const; + int isZero(void) const; + void setZero(void); + + template + Polynomial& operator = (const Polynomial &p); + Polynomial& operator += (const Polynomial& p); + Polynomial& operator -= (const Polynomial& p); + Polynomial operator - (void) const; + Polynomial operator + (const Polynomial& p) const; + Polynomial operator - (const Polynomial& p) const; + template + Polynomial operator * (const Polynomial& p) const; + + Polynomial& operator += ( double s ); + Polynomial& operator -= ( double s ); + Polynomial& operator *= ( double s ); + Polynomial& operator /= ( double s ); + Polynomial operator + ( double s ) const; + Polynomial operator - ( double s ) const; + Polynomial operator * ( double s ) const; + Polynomial operator / ( double s ) const; + + Polynomial scale( double s ) const; + Polynomial shift( double t ) const; + + Polynomial derivative(void) const; + Polynomial integral(void) const; + + void printnl(void) const; + + Polynomial& addScaled(const Polynomial& p,double scale); + + static void Negate(const Polynomial& in,Polynomial& out); + static void Subtract(const Polynomial& p1,const Polynomial& p2,Polynomial& q); + static void Scale(const Polynomial& p,double w,Polynomial& q); + static void AddScaled(const Polynomial& p1,double w1,const Polynomial& p2,double w2,Polynomial& q); + static void AddScaled(const Polynomial& p1,const Polynomial& p2,double w2,Polynomial& q); + static void AddScaled(const Polynomial& p1,double w1,const Polynomial& p2,Polynomial& q); + + void getSolutions(double c,std::vector& roots,double EPS) const; + int getSolutions( double c , double* roots , double EPS ) const; + + // [NOTE] Both of these methods define the indexing according to DeBoor's algorithm, so that + // Polynomial< Degree >BSplineComponent( 0 )( 1.0 )=0 for all Degree>0. + static Polynomial BSplineComponent( int i ); + static void BSplineComponentValues( double x , double* values ); + static void BinomialCoefficients( int bCoefficients[Degree+1] ); +}; + +#include "Polynomial.inl" +#endif // POLYNOMIAL_INCLUDED diff --git a/colmap/include/colmap/lib/PoissonRecon/Polynomial.inl b/colmap/include/colmap/lib/PoissonRecon/Polynomial.inl new file mode 100644 index 0000000000000000000000000000000000000000..ea7ae5044303ac6e1d942772e5c92304f6f278d8 --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/Polynomial.inl @@ -0,0 +1,369 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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 +#include +#include +#include "Factor.h" + +//////////////// +// Polynomial // +//////////////// +template +Polynomial::Polynomial(void){memset(coefficients,0,sizeof(double)*(Degree+1));} +template +template +Polynomial::Polynomial(const Polynomial& P){ + memset(coefficients,0,sizeof(double)*(Degree+1)); + for(int i=0;i<=Degree && i<=Degree2;i++){coefficients[i]=P.coefficients[i];} +} + + +template +template +Polynomial& Polynomial::operator = (const Polynomial &p){ + int d=Degree +Polynomial Polynomial::derivative(void) const{ + Polynomial p; + for(int i=0;i +Polynomial Polynomial::integral(void) const{ + Polynomial p; + p.coefficients[0]=0; + for(int i=0;i<=Degree;i++){p.coefficients[i+1]=coefficients[i]/(i+1);} + return p; +} +template<> double Polynomial< 0 >::operator() ( double t ) const { return coefficients[0]; } +template<> double Polynomial< 1 >::operator() ( double t ) const { return coefficients[0]+coefficients[1]*t; } +template<> double Polynomial< 2 >::operator() ( double t ) const { return coefficients[0]+(coefficients[1]+coefficients[2]*t)*t; } +template +double Polynomial::operator() ( double t ) const{ + double v=coefficients[Degree]; + for( int d=Degree-1 ; d>=0 ; d-- ) v = v*t + coefficients[d]; + return v; +} +template +double Polynomial::integral( double tMin , double tMax ) const +{ + double v=0; + double t1,t2; + t1=tMin; + t2=tMax; + for(int i=0;i<=Degree;i++){ + v+=coefficients[i]*(t2-t1)/(i+1); + if(t1!=-DBL_MAX && t1!=DBL_MAX){t1*=tMin;} + if(t2!=-DBL_MAX && t2!=DBL_MAX){t2*=tMax;} + } + return v; +} +template +int Polynomial::operator == (const Polynomial& p) const{ + for(int i=0;i<=Degree;i++){if(coefficients[i]!=p.coefficients[i]){return 0;}} + return 1; +} +template +int Polynomial::operator != (const Polynomial& p) const{ + for(int i=0;i<=Degree;i++){if(coefficients[i]==p.coefficients[i]){return 0;}} + return 1; +} +template +int Polynomial::isZero(void) const{ + for(int i=0;i<=Degree;i++){if(coefficients[i]!=0){return 0;}} + return 1; +} +template +void Polynomial::setZero(void){memset(coefficients,0,sizeof(double)*(Degree+1));} + +template +Polynomial& Polynomial::addScaled(const Polynomial& p,double s){ + for(int i=0;i<=Degree;i++){coefficients[i]+=p.coefficients[i]*s;} + return *this; +} +template +Polynomial& Polynomial::operator += (const Polynomial& p){ + for(int i=0;i<=Degree;i++){coefficients[i]+=p.coefficients[i];} + return *this; +} +template +Polynomial& Polynomial::operator -= (const Polynomial& p){ + for(int i=0;i<=Degree;i++){coefficients[i]-=p.coefficients[i];} + return *this; +} +template +Polynomial Polynomial::operator + (const Polynomial& p) const{ + Polynomial q; + for(int i=0;i<=Degree;i++){q.coefficients[i]=(coefficients[i]+p.coefficients[i]);} + return q; +} +template +Polynomial Polynomial::operator - (const Polynomial& p) const{ + Polynomial q; + for(int i=0;i<=Degree;i++) {q.coefficients[i]=coefficients[i]-p.coefficients[i];} + return q; +} +template +void Polynomial::Scale(const Polynomial& p,double w,Polynomial& q){ + for(int i=0;i<=Degree;i++){q.coefficients[i]=p.coefficients[i]*w;} +} +template +void Polynomial::AddScaled(const Polynomial& p1,double w1,const Polynomial& p2,double w2,Polynomial& q){ + for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]*w1+p2.coefficients[i]*w2;} +} +template +void Polynomial::AddScaled(const Polynomial& p1,double w1,const Polynomial& p2,Polynomial& q){ + for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]*w1+p2.coefficients[i];} +} +template +void Polynomial::AddScaled(const Polynomial& p1,const Polynomial& p2,double w2,Polynomial& q){ + for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]+p2.coefficients[i]*w2;} +} + +template +void Polynomial::Subtract(const Polynomial &p1,const Polynomial& p2,Polynomial& q){ + for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]-p2.coefficients[i];} +} +template +void Polynomial::Negate(const Polynomial& in,Polynomial& out){ + out=in; + for(int i=0;i<=Degree;i++){out.coefficients[i]=-out.coefficients[i];} +} + +template +Polynomial Polynomial::operator - (void) const{ + Polynomial q=*this; + for(int i=0;i<=Degree;i++){q.coefficients[i]=-q.coefficients[i];} + return q; +} +template +template +Polynomial Polynomial::operator * (const Polynomial& p) const{ + Polynomial q; + for(int i=0;i<=Degree;i++){for(int j=0;j<=Degree2;j++){q.coefficients[i+j]+=coefficients[i]*p.coefficients[j];}} + return q; +} + +template +Polynomial& Polynomial::operator += ( double s ) +{ + coefficients[0]+=s; + return *this; +} +template +Polynomial& Polynomial::operator -= ( double s ) +{ + coefficients[0]-=s; + return *this; +} +template +Polynomial& Polynomial::operator *= ( double s ) +{ + for(int i=0;i<=Degree;i++){coefficients[i]*=s;} + return *this; +} +template +Polynomial& Polynomial::operator /= ( double s ) +{ + for(int i=0;i<=Degree;i++){coefficients[i]/=s;} + return *this; +} +template +Polynomial Polynomial::operator + ( double s ) const +{ + Polynomial q=*this; + q.coefficients[0]+=s; + return q; +} +template +Polynomial Polynomial::operator - ( double s ) const +{ + Polynomial q=*this; + q.coefficients[0]-=s; + return q; +} +template +Polynomial Polynomial::operator * ( double s ) const +{ + Polynomial q; + for(int i=0;i<=Degree;i++){q.coefficients[i]=coefficients[i]*s;} + return q; +} +template +Polynomial Polynomial::operator / ( double s ) const +{ + Polynomial q; + for( int i=0 ; i<=Degree ; i++ ) q.coefficients[i] = coefficients[i]/s; + return q; +} +template +Polynomial Polynomial::scale( double s ) const +{ + Polynomial q=*this; + double s2=1.0; + for(int i=0;i<=Degree;i++){ + q.coefficients[i]*=s2; + s2/=s; + } + return q; +} +template +Polynomial Polynomial::shift( double t ) const +{ + Polynomial q; + for(int i=0;i<=Degree;i++){ + double temp=1; + for(int j=i;j>=0;j--){ + q.coefficients[j]+=coefficients[i]*temp; + temp*=-t*j; + temp/=(i-j+1); + } + } + return q; +} +template +void Polynomial::printnl(void) const{ + for(int j=0;j<=Degree;j++){ + printf("%6.4f x^%d ",coefficients[j],j); + if(j=0){printf("+");} + } + printf("\n"); +} +template +void Polynomial::getSolutions(double c,std::vector& roots,double EPS) const +{ + double r[4][2]; + int rCount=0; + roots.clear(); + switch(Degree){ + case 1: + rCount=Factor(coefficients[1],coefficients[0]-c,r,EPS); + break; + case 2: + rCount=Factor(coefficients[2],coefficients[1],coefficients[0]-c,r,EPS); + break; + case 3: + rCount=Factor(coefficients[3],coefficients[2],coefficients[1],coefficients[0]-c,r,EPS); + break; +// case 4: +// rCount=Factor(coefficients[4],coefficients[3],coefficients[2],coefficients[1],coefficients[0]-c,r,EPS); +// break; + default: + printf("Can't solve polynomial of degree: %d\n",Degree); + } + for(int i=0;i +int Polynomial::getSolutions( double c , double* roots , double EPS ) const +{ + double _roots[4][2]; + int _rCount=0; + switch( Degree ) + { + case 1: _rCount = Factor( coefficients[1] , coefficients[0]-c , _roots , EPS ) ; break; + case 2: _rCount = Factor( coefficients[2] , coefficients[1] , coefficients[0]-c , _roots , EPS ) ; break; + case 3: _rCount = Factor( coefficients[3] , coefficients[2] , coefficients[1] , coefficients[0]-c , _roots , EPS ) ; break; +// case 4: _rCount = Factor( coefficients[4] , coefficients[3] , coefficients[2] , coefficients[1] , coefficients[0]-c , _roots , EPS ) ; break; + default: printf( "Can't solve polynomial of degree: %d\n" , Degree ); + } + int rCount = 0; + for( int i=0 ; i<_rCount ; i++ ) if( fabs(_roots[i][1])<=EPS ) roots[rCount++] = _roots[i][0]; + return rCount; +} +// The 0-th order B-spline +template< > +Polynomial< 0 > Polynomial< 0 >::BSplineComponent( int i ) +{ + Polynomial p; + p.coefficients[0] = 1.; + return p; +} + +// The Degree-th order B-spline +template< int Degree > +Polynomial< Degree > Polynomial< Degree >::BSplineComponent( int i ) +{ + // B_d^i(x) = \int_x^1 B_{d-1}^{i}(y) dy + \int_0^x B_{d-1}^{i-1} y dy + // = \int_0^1 B_{d-1}^{i}(y) dy - \int_0^x B_{d-1}^{i}(y) dy + \int_0^x B_{d-1}^{i-1} y dy + Polynomial p; + if( i _p = Polynomial< Degree-1 >::BSplineComponent( i ).integral(); + p -= _p; + p.coefficients[0] += _p(1); + } + if( i>0 ) + { + Polynomial< Degree > _p = Polynomial< Degree-1 >::BSplineComponent( i-1 ).integral(); + p += _p; + } + return p; +} + + +// The 0-th order B-spline values +template< > void Polynomial< 0 >::BSplineComponentValues( double x , double* values ){ values[0] = 1.; } +// The Degree-th order B-spline +template< int Degree > void Polynomial< Degree >::BSplineComponentValues( double x , double* values ) +{ + const double Scale = 1./Degree; + Polynomial< Degree-1 >::BSplineComponentValues( x , values+1 ); + values[0] = values[1] * (1.-x) * Scale; + for( int i=1 ; i void Polynomial< 0 >::BinomialCoefficients( int bCoefficients[1] ){ bCoefficients[0] = 1; } +template< int Degree > void Polynomial< Degree >::BinomialCoefficients( int bCoefficients[Degree+1] ) +{ + Polynomial< Degree-1 >::BinomialCoefficients( bCoefficients ); + int leftValue = 0; + for( int i=0 ; i +struct MatrixEntry +{ + MatrixEntry( void ) { N =-1; Value = 0; } + MatrixEntry( int i ) { N = i; Value = 0; } + MatrixEntry( int i , T v ) { N = i; Value = v; } + int N; + T Value; +}; + +template class SparseMatrix +{ +private: + bool _contiguous; + int _maxEntriesPerRow; + void _init( void ); +public: + int rows; + Pointer( int ) rowSizes; + Pointer( Pointer( MatrixEntry< T > ) ) m_ppElements; + Pointer( MatrixEntry< T > ) operator[] ( int idx ) { return m_ppElements[idx]; } + ConstPointer( MatrixEntry< T > ) operator[] ( int idx ) const { return m_ppElements[idx]; } + + SparseMatrix( void ); + SparseMatrix( int rows ); + SparseMatrix( int rows , int maxEntriesPerRow ); + void Resize( int rows ); + void Resize( int rows , int maxEntriesPerRow ); + void SetRowSize( int row , int count ); + int Entries( void ) const; + + SparseMatrix( const SparseMatrix& M ); + ~SparseMatrix(); + + void SetZero(); + + SparseMatrix& operator = (const SparseMatrix& M); + + SparseMatrix operator * (const T& V) const; + SparseMatrix& operator *= (const T& V); + + template< class T2 > void Multiply( ConstPointer( T2 ) in , Pointer( T2 ) out , int threads=1 ) const; + template< class T2 > void MultiplyAndAddAverage( ConstPointer( T2 ) in , Pointer( T2 ) out , int threads=1 ) const; + + bool write( FILE* fp ) const; + bool write( const char* fileName ) const; + bool read( FILE* fp ); + bool read( const char* fileName ); + + template< class T2 > void getDiagonal( Pointer( T2 ) diagonal , int threads=1 ) const; + template< class T2 > static int SolveJacobi( const SparseMatrix& M , ConstPointer( T2 ) b , Pointer( T2 ) x , Pointer( T2 ) Mx , T2 sor , int threads=1 ); + template< class T2 > static int SolveJacobi( const SparseMatrix& M , ConstPointer( T2 ) diagonal , ConstPointer( T2 ) b , Pointer( T2 ) x , Pointer( T2 ) Mx , T2 sor , int threads=1 ); + template< class T2 > static int SolveGS( const SparseMatrix& M , ConstPointer( T2 ) b , Pointer( T2 ) x , bool forward ); + template< class T2 > static int SolveGS( const SparseMatrix& M , ConstPointer( T2 ) diagonal , ConstPointer( T2 ) b , Pointer( T2 ) x , bool forward ); + template< class T2 > static int SolveGS( const std::vector< std::vector< int > >& mcIndices , const SparseMatrix& M , ConstPointer( T2 ) diagonal , ConstPointer( T2 ) b , Pointer( T2 ) x , bool forward , int threads=1 ); + template< class T2 > static int SolveGS( const std::vector< std::vector< int > >& mcIndices , const SparseMatrix& M , ConstPointer( T2 ) b , Pointer( T2 ) x , bool forward , int threads=1 ); + template< class T2 > static int SolveCG( const SparseMatrix& M , ConstPointer( T2 ) b , int iters , Pointer( T2 ) x , T2 eps=1e-8 , int reset=1 , bool addDCTerm=false , bool solveNormal=false , int threads=1 ); +}; + + +#if !NEW_SPARSE_MATRIX +template< class T2 > +struct MapReduceVector +{ +private: + int _dim; +public: + std::vector< T2* > out; + MapReduceVector( void ) { _dim = 0; } + ~MapReduceVector( void ) + { + if( _dim ) for( int t=0 ; t +class SparseSymmetricMatrix : public SparseMatrix< T > +{ +public: + + template< class T2 > + Vector< T2 > operator * ( const Vector& V ) const; + + template< class T2 > + Vector< T2 > Multiply( const Vector& V ) const; + + template< class T2 > + void Multiply( const Vector& In, Vector& Out , bool addDCTerm=false ) const; + + template< class T2 > + void Multiply( const Vector& In, Vector& Out , MapReduceVector< T2 >& OutScratch , bool addDCTerm=false ) const; + + template< class T2 > + void Multiply( const Vector& In, Vector& Out , std::vector< T2* >& OutScratch , const std::vector< int >& bounds ) const; + + template< class T2 > + static int SolveCG( const SparseSymmetricMatrix& M , const Vector& b , int iters , Vector& x , T2 eps=1e-8 , int reset=1 , int threads=0 , bool addDCTerm=false , bool solveNormal=false ); + + template< class T2 > + static int SolveCG( const SparseSymmetricMatrix& M , const Vector& b , int iters , Vector& x , MapReduceVector& scratch , T2 eps=1e-8 , int reset=1 , bool addDCTerm=false , bool solveNormal=false ); +#ifdef WIN32 + template< class T2 > + static int SolveCGAtomic( const SparseSymmetricMatrix& M , const Vector& b , int iters , Vector& x , T2 eps=1e-8 , int reset=1 , int threads=0 , bool solveNormal=false ); +#endif // WIN32 + template< class T2 > + static int SolveJacobi( const SparseSymmetricMatrix& M , const Vector& diagonal , const Vector& b , Vector& x , MapReduceVector& scratch , Vector& Mx , T2 sor , int reset ); + template< class T2 > + static int SolveJacobi( const SparseSymmetricMatrix& M , const Vector& b , int iters , Vector& x , MapReduceVector& scratch , T2 sor=T2(1.) , int reset=1 ); + template< class T2 > + static int SolveJacobi( const SparseSymmetricMatrix& M , const Vector& diagonal , const Vector& b , Vector& x , Vector& Mx , T2 sor , int reset ); + template< class T2 > + static int SolveJacobi( const SparseSymmetricMatrix& M , const Vector& b , int iters , Vector& x , T2 sor=T2(1.) , int reset=1 ); + + enum + { + ORDERING_UPPER_TRIANGULAR , + ORDERING_LOWER_TRIANGULAR , + ORDERING_NONE + }; + template< class T2 > + static int SolveGS( const std::vector< std::vector< int > >& mcIndices , const SparseSymmetricMatrix& M , const Vector& diagonal , const Vector& b , Vector& x , MapReduceVector& scratch , Vector& Mx , Vector& dx , bool forward , int reset ); + template< class T2 > + static int SolveGS( const std::vector< std::vector< int > >& mcIndices , const SparseSymmetricMatrix& M , const Vector& b , int iters , Vector& x , MapReduceVector& scratch , bool forward , int reset=1 ); + + template< class T2 > + static int SolveGS( const SparseSymmetricMatrix& M , const Vector& diagonal , const Vector& b , Vector& x , MapReduceVector& scratch , Vector& Mx , Vector& dx , bool forward , int reset , int ordering ); + template< class T2 > + static int SolveGS( const SparseSymmetricMatrix& M , const Vector& b , int iters , Vector& x , MapReduceVector& scratch , bool forward , int reset=1 , int ordering=ORDERING_NONE ); + template< class T2 > + static int SolveGS( const SparseSymmetricMatrix& M , const Vector& diagonal , const Vector& b , Vector& x , Vector& Mx , Vector& dx , bool forward , int reset , int ordering ); + template< class T2 > + static int SolveGS( const SparseSymmetricMatrix& M , const Vector& b , int iters , Vector& x , bool forward , int reset=1 , int ordering=ORDERING_NONE ); + + template< class T2 > + void getDiagonal( Vector< T2 >& diagonal , int threads=1 ) const; +}; +#endif // !NEW_SPARSE_MATRIX + +#include "SparseMatrix.inl" + +#endif + diff --git a/colmap/include/colmap/lib/PoissonRecon/SparseMatrix.inl b/colmap/include/colmap/lib/PoissonRecon/SparseMatrix.inl new file mode 100644 index 0000000000000000000000000000000000000000..c181d8278a4ba750820c3eeba46747aaef5086fb --- /dev/null +++ b/colmap/include/colmap/lib/PoissonRecon/SparseMatrix.inl @@ -0,0 +1,493 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +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 Johns Hopkins University 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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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 +#include + + +/////////////////// +// SparseMatrix // +/////////////////// +/////////////////////////////////////// +// SparseMatrix Methods and Memebers // +/////////////////////////////////////// + +template< class T > +void SparseMatrix< T >::_init( void ) +{ + _contiguous = false; + _maxEntriesPerRow = 0; + rows = 0; + rowSizes = NullPointer( int ); + m_ppElements = NullPointer( Pointer( MatrixEntry< T > ) ); +} + +template< class T > SparseMatrix< T >::SparseMatrix( void ){ _init(); } + +template< class T > SparseMatrix< T >::SparseMatrix( int rows ){ _init() , Resize( rows ); } +template< class T > SparseMatrix< T >::SparseMatrix( int rows , int maxEntriesPerRow ){ _init() , Resize( rows , maxEntriesPerRow ); } + +template< class T > +SparseMatrix< T >::SparseMatrix( const SparseMatrix& M ) +{ + _init(); + if( M._contiguous ) Resize( M.rows , M._maxEntriesPerRow ); + else Resize( M.rows ); + for( int i=0 ; i ) * rowSizes[i] ); + } +} +template +int SparseMatrix::Entries( void ) const +{ + int e = 0; + for( int i=0 ; i +SparseMatrix& SparseMatrix::operator = (const SparseMatrix& M) +{ + if( M._contiguous ) Resize( M.rows , M._maxEntriesPerRow ); + else Resize( M.rows ); + for( int i=0 ; i ) * rowSizes[i] ); + } + return *this; +} + +template +SparseMatrix::~SparseMatrix( void ){ Resize( 0 ); } + +template< class T > +bool SparseMatrix< T >::write( const char* fileName ) const +{ + FILE* fp = fopen( fileName , "wb" ); + if( !fp ) return false; + bool ret = write( fp ); + fclose( fp ); + return ret; +} +template< class T > +bool SparseMatrix< T >::read( const char* fileName ) +{ + FILE* fp = fopen( fileName , "rb" ); + if( !fp ) return false; + bool ret = read( fp ); + fclose( fp ); + return ret; +} +template< class T > +bool SparseMatrix< T >::write( FILE* fp ) const +{ + if( fwrite( &rows , sizeof( int ) , 1 , fp )!=1 ) return false; + if( fwrite( rowSizes , sizeof( int ) , rows , fp )!=rows ) return false; + for( int i=0 ; i ) , rowSizes[i] , fp )!=rowSizes[i] ) return false; + return true; +} +template< class T > +bool SparseMatrix< T >::read( FILE* fp ) +{ + int r; + if( fread( &r , sizeof( int ) , 1 , fp )!=1 ) return false; + Resize( r ); + if( fread( rowSizes , sizeof( int ) , rows , fp )!=rows ) return false; + for( int i=0 ; i ) , rowSizes[i] , fp )!=rowSizes[i] ) return false; + } + return true; +} + + +template< class T > +void SparseMatrix< T >::Resize( int r ) +{ + if( rows>0 ) + { + if( _contiguous ){ if( _maxEntriesPerRow ) FreePointer( m_ppElements[0] ); } + else for( int i=0 ; i( r ); + m_ppElements = AllocPointer< Pointer( MatrixEntry< T > ) >( r ); + memset( rowSizes , 0 , sizeof( int ) * r ); + } + _contiguous = false; + _maxEntriesPerRow = 0; +} +template< class T > +void SparseMatrix< T >::Resize( int r , int e ) +{ + if( rows>0 ) + { + if( _contiguous ){ if( _maxEntriesPerRow ) FreePointer( m_ppElements[0] ); } + else for( int i=0 ; i( r ); + m_ppElements = AllocPointer< Pointer( MatrixEntry< T > ) >( r ); + m_ppElements[0] = AllocPointer< MatrixEntry< T > >( r * e ); + memset( rowSizes , 0 , sizeof( int ) * r ); + for( int i=1 ; i +void SparseMatrix< T >::SetRowSize( int row , int count ) +{ + if( _contiguous ) + { + if( count>_maxEntriesPerRow ) fprintf( stderr , "[ERROR] Cannot set row size on contiguous matrix: %d<=%d\n" , count , _maxEntriesPerRow ) , exit( 0 ); + rowSizes[row] = count; + } + else if( row>=0 && row0 ) m_ppElements[row] = AllocPointer< MatrixEntry< T > >( count ); + // [WARNING] Why wasn't this line here before??? + rowSizes[row] = count; + } +} + + +template +void SparseMatrix::SetZero() +{ + Resize(this->m_N, this->m_M); +} + +template +SparseMatrix SparseMatrix::operator * (const T& V) const +{ + SparseMatrix M(*this); + M *= V; + return M; +} + +template +SparseMatrix& SparseMatrix::operator *= (const T& V) +{ + for( int i=0 ; i +template< class T2 > +void SparseMatrix< T >::Multiply( ConstPointer( T2 ) in , Pointer( T2 ) out , int threads ) const +{ +#pragma omp parallel for num_threads( threads ) + for( int i=0 ; i ) start = m_ppElements[i]; + ConstPointer( MatrixEntry< T > ) end = start + rowSizes[i]; + ConstPointer( MatrixEntry< T > ) e; + for( e=start ; e!=end ; e++ ) _out += in[ e->N ] * e->Value; + out[i] = _out; + } +} +template< class T > +template< class T2 > +void SparseMatrix< T >::MultiplyAndAddAverage( ConstPointer( T2 ) in , Pointer( T2 ) out , int threads ) const +{ + T2 average = 0; + for( int i=0 ; i +template< class T2 > +int SparseMatrix::SolveJacobi( const SparseMatrix& M , ConstPointer( T2 ) diagonal , ConstPointer( T2 ) b , Pointer( T2 ) x , Pointer( T2 ) Mx , T2 sor , int threads ) +{ + M.Multiply( x , Mx , threads ); +#if ZERO_TESTING_JACOBI + for( int j=0 ; j +template< class T2 > +int SparseMatrix::SolveJacobi( const SparseMatrix& M , ConstPointer( T2 ) b , Pointer( T2 ) x , Pointer( T2 ) Mx , T2 sor , int threads ) +{ + M.Multiply( x , Mx , threads ); +#if ZERO_TESTING_JACOBI + for( int j=0 ; j +template +int SparseMatrix::SolveGS( const SparseMatrix& M , ConstPointer( T2 ) diagonal , ConstPointer( T2 ) b , Pointer( T2 ) x , bool forward ) +{ +#define ITERATE \ + { \ + ConstPointer( MatrixEntry< T > ) start = M[j]; \ + ConstPointer( MatrixEntry< T > ) end = start + M.rowSizes[j]; \ + ConstPointer( MatrixEntry< T > ) e; \ + T2 _b = b[j]; \ + for( e=start ; e!=end ; e++ ) _b -= x[ e->N ] * e->Value; \ + x[j] += _b / diagonal[j]; \ + } + +#if ZERO_TESTING_JACOBI + if( forward ) for( int j=0 ; j=0 ; j-- ){ if( diagonal[j] ){ ITERATE; } } +#else // !ZERO_TESTING_JACOBI + if( forward ) for( int j=0 ; j=0 ; j-- ){ ITERATE; } +#endif // ZERO_TESTING_JACOBI +#undef ITERATE + return M.rows; +} +template +template +int SparseMatrix::SolveGS( const std::vector< std::vector< int > >& mcIndices , const SparseMatrix& M , ConstPointer( T2 ) diagonal , ConstPointer( T2 ) b , Pointer( T2 ) x , bool forward , int threads ) +{ + int sum=0; +#ifdef _WIN32 +#define SetOMPParallel __pragma( omp parallel for num_threads( threads ) ) +#else // !_WIN32 +#define SetOMPParallel _Pragma( "omp parallel for num_threads( threads )" ) +#endif // _WIN32 +#if ZERO_TESTING_JACOBI +#define ITERATE( indices ) \ + { \ +SetOMPParallel \ + for( int k=0 ; k ) start = M[jj]; \ + ConstPointer( MatrixEntry< T > ) end = start + M.rowSizes[jj]; \ + ConstPointer( MatrixEntry< T > ) e; \ + T2 _b = b[jj]; \ + for( e=start ; e!=end ; e++ ) _b -= x[ e->N ] * e->Value; \ + x[jj] += _b / diagonal[jj]; \ + } \ + } +#else // !ZERO_TESTING_JACOBI +#define ITERATE( indices ) \ + { \ +SetOMPParallel \ + for( int k=0 ; k ) start = M[jj]; \ + ConstPointer( MatrixEntry< T > ) end = start + M.rowSizes[jj]; \ + ConstPointer( MatrixEntry< T > ) e; \ + T2 _b = b[jj]; \ + for( e=start ; e!=end ; e++ ) _b -= x[ e->N ] * e->Value; \ + x[jj] += _b / diagonal[jj]; \ + } \ + } +#endif // ZERO_TESTING_JACOBI + if( forward ) for( int j=0 ; j=0 ; j-- ){ sum += int( mcIndices[j].size() ) ; ITERATE( mcIndices[j] ); } +#undef ITERATE +#undef SetOMPParallel + return sum; +} +template +template +int SparseMatrix::SolveGS( const SparseMatrix& M , ConstPointer( T2 ) b , Pointer( T2 ) x , bool forward ) +{ + int start = forward ? 0 : M.rows-1 , end = forward ? M.rows : -1 , dir = forward ? 1 : -1; + for( int j=start ; j!=end ; j+=dir ) + { + T diagonal = M[j][0].Value; +#if ZERO_TESTING_JACOBI + if( diagonal ) +#endif // ZERO_TESTING_JACOBI + { + ConstPointer( MatrixEntry< T > ) start = M[j]; + ConstPointer( MatrixEntry< T > ) end = start + M.rowSizes[j]; + ConstPointer( MatrixEntry< T > ) e; + start++; + T2 _b = b[j]; + for( e=start ; e!=end ; e++ ) _b -= x[ e->N ] * e->Value; + x[j] = _b / diagonal; + } + } + return M.rows; +} +template +template +int SparseMatrix::SolveGS( const std::vector< std::vector< int > >& mcIndices , const SparseMatrix& M , ConstPointer( T2 ) b , Pointer( T2 ) x , bool forward , int threads ) +{ + int sum=0 , start = forward ? 0 : int( mcIndices.size() )-1 , end = forward ? int( mcIndices.size() ) : -1 , dir = forward ? 1 : -1; + for( int j=start ; j!=end ; j+=dir ) + { + const std::vector< int >& _mcIndices = mcIndices[j]; + sum += int( _mcIndices.size() ); + { +#pragma omp parallel for num_threads( threads ) + for( int k=0 ; k ) start = M[jj]; + ConstPointer( MatrixEntry< T > ) end = start + M.rowSizes[jj]; + ConstPointer( MatrixEntry< T > ) e; + start++; + T2 _b = b[jj]; + for( e=start ; e!=end ; e++ ) _b -= x[ e->N ] * e->Value; + x[jj] = _b / diagonal; + } + } + } + } + return sum; +} + +template< class T > +template< class T2 > +void SparseMatrix< T >::getDiagonal( Pointer( T2 ) diagonal , int threads ) const +{ +#pragma omp parallel for num_threads( threads ) + for( int i=0 ; i ) start = m_ppElements[i]; + ConstPointer( MatrixEntry< T > ) end = start + rowSizes[i]; + ConstPointer( MatrixEntry< T > ) e; + for( e=start ; e!=end ; e++ ) if( e->N==i ) d += e->Value; + diagonal[i] = d; + } +} +template< class T > +template< class T2 > +int SparseMatrix< T >::SolveCG( const SparseMatrix& A , ConstPointer( T2 ) b , int iters , Pointer( T2 ) x , T2 eps , int reset , bool addDCTerm , bool solveNormal , int threads ) +{ + eps *= eps; + int dim = A.rows; + Pointer( T2 ) r = AllocPointer< T2 >( dim ); + Pointer( T2 ) d = AllocPointer< T2 >( dim ); + Pointer( T2 ) q = AllocPointer< T2 >( dim ); + Pointer( T2 ) temp = NullPointer( T2 ); + if( reset ) memset( x , 0 , sizeof(T2)* dim ); + if( solveNormal ) temp = AllocPointer< T2 >( dim ); + + double delta_new = 0 , delta_0; + if( solveNormal ) + { + if( addDCTerm ) A.MultiplyAndAddAverage( ( ConstPointer( T2 ) )x , temp , threads ) , A.MultiplyAndAddAverage( ( ConstPointer( T2 ) )temp , r , threads ) , A.MultiplyAndAddAverage( ( ConstPointer( T2 ) )b , temp , threads ); + else A.Multiply( ( ConstPointer( T2 ) )x , temp , threads ) , A.Multiply( ( ConstPointer( T2 ) )temp , r , threads ) , A.Multiply( ( ConstPointer( T2 ) )b , temp , threads ); +#pragma omp parallel for num_threads( threads ) reduction( + : delta_new ) + for( int i=0 ; ieps*delta_0 ; ii++ ) + { + if( solveNormal ) + if( addDCTerm ) A.MultiplyAndAddAverage( ( ConstPointer( T2 ) )d , temp , threads ) , A.MultiplyAndAddAverage( ( ConstPointer( T2 ) )temp , q , threads ); + else A.Multiply( ( ConstPointer( T2 ) )d , temp , threads ) , A.Multiply( ( ConstPointer( T2 ) )temp , q , threads ); + else + if( addDCTerm ) A.MultiplyAndAddAverage( ( ConstPointer( T2 ) )d , q , threads ); + else A.Multiply( ( ConstPointer( T2 ) )d , q , threads ); + double dDotQ = 0; +#pragma omp parallel for num_threads( threads ) reduction( + : dDotQ ) + for( int i=0 ; i 1200 + template + static int DownSamplePixelDataI(unsigned int gl_format, int width, int height, + int ds, const Uint * pin, Uint * pout); + template + static int DownSamplePixelDataI2F(unsigned int gl_format, int width, int height, + int ds, const Uint * pin, float * pout, int skip = 0); +#endif + static int DownSamplePixelDataF(unsigned int gl_format, int width, int height, + int ds, const float * pin, float * pout, int skip = 0); + static int TruncateWidthCU(int w) {return w & 0xfffffffc; } +public: + GLTexInput() : _down_sampled(0), _rgb_converted(0), _data_modified(0), + _converted_data(0), _pixel_data(0){} + int SetImageData(int width, int height, const void * data, + unsigned int gl_format, unsigned int gl_type); + int LoadImageFile(char * imagepath, int & w, int &h); + void VerifyTexture(); + virtual ~GLTexInput(); +}; + +//GLTexPacked doesn't have any data +//so that we can use the GLTexImage* pointer to index a GLTexPacked Vector + +class GLTexPacked:public GLTexImage +{ +public: + virtual void DrawImage(); + virtual void DrawQuadUS(int scale); + virtual void DrawQuadDS(int scale); + virtual void FillMargin(int marginx, int marginy); + virtual void InitTexture(int width, int height, int clamp_to_edge =1); + virtual void TexConvertRGB(); + virtual void SetImageSize(int width, int height); + virtual void ZeroHistoMargin(); + //virtual void GetHistWH(int& w, int& h){return w = (3 + sz)>>1;} +public: + void DrawMargin(int right, int bottom, int mx, int my); + GLTexPacked():GLTexImage(){} +}; + + +#endif // !defined(GL_TEX_IMAGE_H) + diff --git a/colmap/include/colmap/lib/SiftGPU/GlobalUtil.h b/colmap/include/colmap/lib/SiftGPU/GlobalUtil.h new file mode 100644 index 0000000000000000000000000000000000000000..4a0f6e634ee23861aa510ec71ab90c7805682758 --- /dev/null +++ b/colmap/include/colmap/lib/SiftGPU/GlobalUtil.h @@ -0,0 +1,157 @@ +//////////////////////////////////////////////////////////////////////////// +// File: GlobalUtil.h +// Author: Changchang Wu +// Description : +// GlobalParam: Global parameters +// ClockTimer: Timer +// GlobalUtil: Global Function wrapper +// +// Copyright (c) 2007 University of North Carolina at Chapel Hill +// All Rights Reserved +// +// Permission to use, copy, modify and distribute this software and its +// documentation for educational, research and non-profit purposes, without +// fee, and without a written agreement is hereby granted, provided that the +// above copyright notice and the following paragraph appear in all copies. +// +// The University of North Carolina at Chapel Hill make no representations +// about the suitability of this software for any purpose. It is provided +// 'as is' without express or implied warranty. +// +// Please send BUG REPORTS to ccwu@cs.unc.edu +// +//////////////////////////////////////////////////////////////////////////// + + +#ifndef _GLOBAL_UTILITY_H +#define _GLOBAL_UTILITY_H + + +//wrapper for some shader function +//class ProgramGPU; +class LiteWindow; + +class GlobalParam +{ +public: + static GLuint _texTarget; + static GLuint _iTexFormat; + static int _texMaxDim; + static int _texMaxDimGL; + static int _texMinDim; + static int _MemCapGPU; + static int _FitMemoryCap; + static int _verbose; + static int _timingS; + static int _timingO; + static int _timingL; + static int _usePackedTex; + static int _IsNvidia; + static int _KeepShaderLoop; + static int _UseCUDA; + static int _UseOpenCL; + static int _UseDynamicIndexing; + static int _debug; + static int _MaxFilterWidth; + static float _FilterWidthFactor; + static float _OrientationWindowFactor; + static float _DescriptorWindowFactor; + static int _MaxOrientation; + static int _OrientationPack2; + static int _ListGenGPU; + static int _ListGenSkipGPU; + static int _SupportNVFloat; + static int _SupportTextureRG; + static int _FullSupported; + static float _MaxFeaturePercent; + static int _MaxLevelFeatureNum; + static int _DescriptorPPR; + static int _DescriptorPPT; //pixel per texture for one descriptor + static int _FeatureTexBlock; + static int _NarrowFeatureTex; //implemented but no performance improvement + static int _SubpixelLocalization; + static int _ProcessOBO; //not implemented yet + static int _TruncateMethod; + static int _PreciseBorder; //implemented + static int _UseSiftGPUEX; + static int _ForceTightPyramid; + static int _octave_min_default; + static int _octave_num_default; + static int _InitPyramidWidth; + static int _InitPyramidHeight; + static int _PreProcessOnCPU; + static int _GoodOpenGL; + static int _FixedOrientation; + static int _LoweOrigin; + static int _ExitAfterSIFT; + static int _NormalizedSIFT; + static int _BinarySIFT; + static int _KeepExtremumSign; + static int _FeatureCountThreshold; + static int _KeyPointListForceLevel0; + static int _DarknessAdaption; + + //for compatability with old version: + static float _OrientationExtraFactor; + static float _OrientationGaussianFactor; + static float _MulitiOrientationThreshold; + + //////////////////////////////////////// + static int _WindowInitX; + static int _WindowInitY; + static const char* _WindowDisplay; + static int _DeviceIndex; +}; + + +class ClockTimer +{ +private: + char _current_event[256]; + int _time_start; + int _time_stop; +public: + static int ClockMS(); + static double CLOCK(); + static void InitHighResolution(); + void StopTimer(int verb = 1); + void StartTimer(const char * event, int verb=0); + float GetElapsedTime(); +}; + +class GlobalUtil:public GlobalParam +{ + static ClockTimer _globalTimer; +public: + inline static double CLOCK() { return ClockTimer::CLOCK(); } + inline static void StopTimer() { _globalTimer.StopTimer(_timingS); } + inline static void StartTimer(const char * event) { _globalTimer.StartTimer(event, _timingO); } + inline static float GetElapsedTime() { return _globalTimer.GetElapsedTime(); } + + static void FitViewPort(int width, int height); + static void SetTextureParameter(); + static void SetTextureParameterUS(); +#ifdef _DEBUG + static void CheckErrorsGL(const char* location = NULL); +#else + static void inline CheckErrorsGL(const char* location = NULL){}; +#endif + static bool CheckFramebufferStatus(); + //initialize Opengl parameters + static void SelectDisplay(); + static void InitGLParam(int NotTargetGL = 0); + static void SetGLParam(); + static int CreateWindowEZ(); + static void CleanupOpenGL(); + static void SetDeviceParam(int argc, char** argv); + static int CreateWindowEZ(LiteWindow* window); +}; + + +#if defined(_MSC_VER) && _MSC_VER == 1200 +#define max(a,b) (((a) > (b)) ? (a) : (b)) +#define min(a,b) (((a) < (b)) ? (a) : (b)) +#endif + +#endif + diff --git a/colmap/include/colmap/lib/SiftGPU/LiteWindow.h b/colmap/include/colmap/lib/SiftGPU/LiteWindow.h new file mode 100644 index 0000000000000000000000000000000000000000..4d54222c1742dad57f099a5fac67daf98038a387 --- /dev/null +++ b/colmap/include/colmap/lib/SiftGPU/LiteWindow.h @@ -0,0 +1,13 @@ +#ifndef LITE_WINDOW_H +#define LITE_WINDOW_H + +class LiteWindow { + public: + LiteWindow() {} + virtual ~LiteWindow() {} + int IsValid() { return 0; } + void MakeCurrent() {} + void Create(int x = -1, int y = -1, const char* display = NULL) {} +}; + +#endif diff --git a/colmap/include/colmap/lib/SiftGPU/ProgramCG.h b/colmap/include/colmap/lib/SiftGPU/ProgramCG.h new file mode 100644 index 0000000000000000000000000000000000000000..6ce072cd56c4a75d12185489b12f9d86c19fa68f --- /dev/null +++ b/colmap/include/colmap/lib/SiftGPU/ProgramCG.h @@ -0,0 +1,161 @@ +//////////////////////////////////////////////////////////////////////////// +// File: ProgramCG.h +// Author: Changchang Wu +// Description : interface for the ProgramCG classes. +// ProgramCG: Cg programs +// ShaderBagCG: All Cg shaders for Sift in a bag +// FilterGLCG: Cg Gaussian Filters +// +// Copyright (c) 2007 University of North Carolina at Chapel Hill +// All Rights Reserved +// +// Permission to use, copy, modify and distribute this software and its +// documentation for educational, research and non-profit purposes, without +// fee, and without a written agreement is hereby granted, provided that the +// above copyright notice and the following paragraph appear in all copies. +// +// The University of North Carolina at Chapel Hill make no representations +// about the suitability of this software for any purpose. It is provided +// 'as is' without express or implied warranty. +// +// Please send BUG REPORTS to ccwu@cs.unc.edu +// +//////////////////////////////////////////////////////////////////////////// + + +#if defined(CG_SIFTGPU_ENABLED) + +#ifndef _PROGRAM_CG_H +#define _PROGRAM_CG_H + +#include "ProgramGPU.h" +class FragmentProgram; +#include "Cg/cgGL.h" + +class ProgramCG:public ProgramGPU +{ + CGprogram _programID; + CGprofile _profile; + int _valid; +public: + static CGcontext _Context; + static CGprofile _FProfile; +public: + operator CGprogram (){return _programID;} + CGprogram GetProgramID(){return _programID;} + int UseProgram(); + int IsValidProgram(){return _programID && _valid;} + static void ErrorCallback(); + static void InitContext(); + static void DestroyContext(); + ProgramCG(const char * code, const char** cg_compile_args= NULL, CGprofile profile = ProgramCG::_FProfile); + ProgramCG(); + virtual ~ProgramCG(); + +}; + +class ShaderBagCG:public ShaderBag +{ + CGparameter _param_dog_texu; + CGparameter _param_dog_texd; + CGparameter _param_genlist_start_tex0; + CGparameter _param_ftex_width; + CGparameter _param_genlist_step_tex; + CGparameter _param_genlist_step_tex0; + CGparameter _param_genvbo_size; + CGparameter _param_orientation_gtex; + CGparameter _param_orientation_stex; + CGparameter _param_orientation_size; + CGparameter _param_descriptor_gtex; + CGparameter _param_descriptor_size; + CGparameter _param_descriptor_dsize; + CGparameter _param_margin_copy_truncate; + CGparameter _param_genlist_init_bbox; +public: + virtual void LoadDescriptorShader(); + void LoadDescriptorShaderF2(); + static void WriteOrientationCodeToStream(ostream& out); + virtual void SetGenListInitParam(int w, int h); + virtual void SetMarginCopyParam(int xmax, int ymax); + virtual void SetFeatureOrientationParam(int gtex, int width, int height, float sigma, int stex = 0, float step = 1.0f); + virtual void SetFeatureDescirptorParam(int gtex, int otex, float dwidth, float fwidth, float width, float height, float sigma); + virtual void SetSimpleOrientationInput(int oTex, float sigma, float sigma_step); + void LoadOrientationShader(); + virtual void SetGenListStartParam(float width, int tex0); + static ProgramCG* LoadGenListStepShader(int start, int step); + static ProgramCG* LoadGenListStepShaderV2(int start, int step); + void LoadGenListShader(int ndoglev, int nlev); + virtual void UnloadProgram(); + virtual void SetDogTexParam(int texU, int texD); + virtual void SetGenListStepParam(int tex, int tex0); + virtual void SetGenVBOParam( float width, float fwidth, float size); + virtual void LoadFixedShaders(); + virtual void LoadDisplayShaders(); + virtual void LoadKeypointShader(float threshold, float edgeThreshold); + virtual int LoadKeypointShaderMR(float threshold, float edgeThreshold); + ShaderBagCG(); + virtual ~ShaderBagCG(){} +}; + + +class FilterGLCG : public FilterProgram +{ +private: + ProgramGPU* CreateFilterH(float kernel[], float offset[], int width); + ProgramGPU* CreateFilterV(float kernel[], float offset[], int height); + //packed version + ProgramGPU* CreateFilterHPK(float kernel[], float offset[], int width); + ProgramGPU* CreateFilterVPK(float kernel[], float offset[], int height); +}; + +class ShaderBagPKCG:public ShaderBag +{ +private: + CGparameter _param_dog_texu; + CGparameter _param_dog_texd; + CGparameter _param_margin_copy_truncate; + CGparameter _param_grad_pass_texp; + CGparameter _param_genlist_init_bbox; + CGparameter _param_genlist_start_tex0; + CGparameter _param_ftex_width; + CGparameter _param_genlist_step_tex; + CGparameter _param_genlist_step_tex0; + CGparameter _param_genlist_end_ktex; + CGparameter _param_genvbo_size; + CGparameter _param_orientation_gtex; + CGparameter _param_orientation_otex; + CGparameter _param_orientation_size; + CGparameter _param_descriptor_gtex; + CGparameter _param_descriptor_otex; + CGparameter _param_descriptor_size; + CGparameter _param_descriptor_dsize; + +public: + ShaderBagPKCG(); + virtual ~ShaderBagPKCG(){} + virtual void LoadDescriptorShader(); + virtual void LoadDescriptorShaderF2(); + virtual void LoadOrientationShader(); + virtual void LoadGenListShader(int ndoglev, int nlev); + virtual void LoadGenListShaderV2(int ndoglev, int nlev); + virtual void UnloadProgram() ; + virtual void LoadKeypointShader(float threshold, float edgeTrheshold); + virtual void LoadFixedShaders(); + virtual void LoadDisplayShaders(); + virtual void SetGradPassParam(int texP); + virtual void SetGenListEndParam(int ktex); +public: + //parameters + virtual void SetGenListStartParam(float width, int tex0); + virtual void SetGenListInitParam(int w, int h); + virtual void SetMarginCopyParam(int xmax, int ymax); + virtual void SetDogTexParam(int texU, int texD); + virtual void SetGenListStepParam(int tex, int tex0); + virtual void SetGenVBOParam( float width, float fwidth, float size); + virtual void SetFeatureDescirptorParam(int gtex, int otex, float dwidth, float fwidth, float width, float height, float sigma); + virtual void SetFeatureOrientationParam(int gtex, int width, int height, float sigma, int stex, float step); + virtual void SetSimpleOrientationInput(int oTex, float sigma, float sigma_step); +}; +#endif +#endif + diff --git a/colmap/include/colmap/lib/SiftGPU/ProgramCL.h b/colmap/include/colmap/lib/SiftGPU/ProgramCL.h new file mode 100644 index 0000000000000000000000000000000000000000..e134992b2ff44312c16bc4a4f3ef5bbc6ee209bf --- /dev/null +++ b/colmap/include/colmap/lib/SiftGPU/ProgramCL.h @@ -0,0 +1,164 @@ +//////////////////////////////////////////////////////////////////////////// +// File: ProgramCL.h +// Author: Changchang Wu +// Description : interface for the ProgramCL classes. +// ProgramCL: Cg programs +// ShaderBagCG: All Cg shaders for Sift in a bag +// FilterCL: Cg Gaussian Filters +// +// Copyright (c) 2007 University of North Carolina at Chapel Hill +// All Rights Reserved +// +// Permission to use, copy, modify and distribute this software and its +// documentation for educational, research and non-profit purposes, without +// fee, and without a written agreement is hereby granted, provided that the +// above copyright notice and the following paragraph appear in all copies. +// +// The University of North Carolina at Chapel Hill make no representations +// about the suitability of this software for any purpose. It is provided +// 'as is' without express or implied warranty. +// +// Please send BUG REPORTS to ccwu@cs.unc.edu +// +//////////////////////////////////////////////////////////////////////////// + + +#if defined(CL_SIFTGPU_ENABLED) + +#ifndef _PROGRAM_CL_H +#define _PROGRAM_CL_H + +#include "ProgramGPU.h" + +class ProgramCL: public ProgramGPU +{ + cl_program _program; + cl_kernel _kernel; + int _valid; +public: + int IsValidProgram(){return _program && _valid;} + ProgramCL(const char* name, const char * code, cl_context contex, cl_device_id device); + ProgramCL(); + void PrintBuildLog(cl_device_id device, int all); + virtual ~ProgramCL(); + virtual int UseProgram(){return 1;} + virtual void * GetProgramID() {return _kernel;} + friend class ProgramBagCL; + friend class ProgramBagCLN; +}; + +class CLTexImage; +class FilterCL +{ +public: + ProgramCL* s_shader_h; + ProgramCL* s_shader_v; + int _size; + int _id; + CLTexImage * _weight; +public: + FilterCL() : s_shader_h(NULL), s_shader_v(NULL), _size(0), _id(0), _weight(NULL) {} + ~FilterCL() {if(s_shader_h) delete s_shader_h; if(s_shader_v) delete s_shader_v; if(_weight) delete _weight; } +}; + +class SiftParam; + +class ProgramBagCL +{ +protected: + cl_platform_id _platform; + cl_device_id _device; + cl_context _context; + cl_command_queue _queue; +protected: + ProgramCL * s_gray; + ProgramCL * s_sampling; + ProgramCL * s_sampling_k; + ProgramCL * s_sampling_u; + ProgramCL * s_zero_pass; + ProgramCL * s_packup; + ProgramCL * s_unpack; + ProgramCL * s_unpack_dog; + ProgramCL * s_unpack_grd; + ProgramCL * s_unpack_key; + ProgramCL * s_dog_pass; + ProgramCL * s_grad_pass; + ProgramCL * s_grad_pass2; + ProgramCL * s_gray_pack; + ProgramCL * s_keypoint; +public: + FilterCL * f_gaussian_skip0; + vector f_gaussian_skip0_v; + FilterCL * f_gaussian_skip1; + FilterCL ** f_gaussian_step; + int _gaussian_step_num; +public: + ProgramBagCL(); + bool InitializeContext(); + virtual ~ProgramBagCL(); + void FinishCL(); + cl_context GetContextCL() {return _context;} + cl_command_queue GetCommandQueue() {return _queue;} + static const char* GetErrorString(cl_int error); + static bool CheckErrorCL(cl_int error, const char* location = NULL); +public: + FilterCL * CreateGaussianFilter(float sigma); + void CreateGaussianFilters(SiftParam¶m); + void SelectInitialSmoothingFilter(int octave_min, SiftParam¶m); + void FilterInitialImage(CLTexImage* tex, CLTexImage* buf); + void FilterSampledImage(CLTexImage* tex, CLTexImage* buf); + void UnpackImage(CLTexImage*src, CLTexImage* dst); + void UnpackImageDOG(CLTexImage*src, CLTexImage* dst); + void UnpackImageGRD(CLTexImage*src, CLTexImage* dst); + void UnpackImageKEY(CLTexImage*src, CLTexImage* dog, CLTexImage* dst); + void ComputeDOG(CLTexImage*tex, CLTexImage* texp, CLTexImage* dog, CLTexImage* grad, CLTexImage* rot); + void ComputeKEY(CLTexImage*dog, CLTexImage* key, float Tdog, float Tedge); +public: + virtual void SampleImageU(CLTexImage *dst, CLTexImage *src, int log_scale); + virtual void SampleImageD(CLTexImage *dst, CLTexImage *src, int log_scale = 1); + virtual void FilterImage(FilterCL* filter, CLTexImage *dst, CLTexImage *src, CLTexImage*tmp); + virtual ProgramCL* CreateFilterH(float kernel[], int width); + virtual ProgramCL* CreateFilterV(float kernel[], int width); + virtual FilterCL* CreateFilter(float kernel[], int width); +public: + virtual void InitProgramBag(SiftParam¶m); + virtual void LoadDescriptorShader(); + virtual void LoadDescriptorShaderF2(); + virtual void LoadOrientationShader(); + virtual void LoadGenListShader(int ndoglev, int nlev); + virtual void UnloadProgram() ; + virtual void LoadKeypointShader(); + virtual void LoadFixedShaders(); + virtual void LoadDisplayShaders(); + virtual void LoadDynamicShaders(SiftParam& param); +public: + //parameters + virtual void SetGradPassParam(int texP); + virtual void SetGenListEndParam(int ktex); + virtual void SetGenListStartParam(float width, int tex0); + virtual void SetGenListInitParam(int w, int h); + virtual void SetMarginCopyParam(int xmax, int ymax); + virtual void SetDogTexParam(int texU, int texD); + virtual void SetGenListStepParam(int tex, int tex0); + virtual void SetGenVBOParam( float width, float fwidth, float size); + virtual void SetFeatureDescirptorParam(int gtex, int otex, float dwidth, float fwidth, float width, float height, float sigma); + virtual void SetFeatureOrientationParam(int gtex, int width, int height, float sigma, int stex, float step); + virtual void SetSimpleOrientationInput(int oTex, float sigma, float sigma_step); + +}; + +class CLTexImage ; +class ProgramBagCLN: public ProgramBagCL +{ +public: + virtual void SampleImageD(CLTexImage *dst, CLTexImage *src, int log_scale = 1); + virtual FilterCL* CreateFilter(float kernel[], int width); + virtual ProgramCL* CreateFilterH(float kernel[], int width); + virtual ProgramCL* CreateFilterV(float kernel[], int width); + virtual void FilterImage(FilterCL* filter, CLTexImage *dst, CLTexImage *src, CLTexImage*tmp); + virtual void LoadFixedShaders(); + virtual void LoadDisplayShaders(); +}; +#endif +#endif + diff --git a/colmap/include/colmap/lib/SiftGPU/ProgramCU.h b/colmap/include/colmap/lib/SiftGPU/ProgramCU.h new file mode 100644 index 0000000000000000000000000000000000000000..36e2ccad4413fc6457f43f05a8db238d1758c359 --- /dev/null +++ b/colmap/include/colmap/lib/SiftGPU/ProgramCU.h @@ -0,0 +1,74 @@ +//////////////////////////////////////////////////////////////////////////// +// File: ProgramCU.h +// Author: Changchang Wu +// Description : interface for the ProgramCU classes. +// It is basically a wrapper around all the CUDA kernels +// +// Copyright (c) 2007 University of North Carolina at Chapel Hill +// All Rights Reserved +// +// Permission to use, copy, modify and distribute this software and its +// documentation for educational, research and non-profit purposes, without +// fee, and without a written agreement is hereby granted, provided that the +// above copyright notice and the following paragraph appear in all copies. +// +// The University of North Carolina at Chapel Hill make no representations +// about the suitability of this software for any purpose. It is provided +// 'as is' without express or implied warranty. +// +// Please send BUG REPORTS to ccwu@cs.unc.edu +// +//////////////////////////////////////////////////////////////////////////// + +#ifndef _PROGRAM_CU_H +#define _PROGRAM_CU_H +#if defined(CUDA_SIFTGPU_ENABLED) + +class CuTexImage; + +class ProgramCU +{ +public: + //GPU FUNCTIONS + static void FinishCUDA(); + static int CheckErrorCUDA(const char* location); + static int CheckCudaDevice(int device); +public: + ////SIFTGPU FUNCTIONS + static void CreateFilterKernel(float sigma, float* kernel, int& width); + template static void FilterImage(CuTexImage *dst, CuTexImage *src, CuTexImage* buf); + static void FilterImage(CuTexImage *dst, CuTexImage *src, CuTexImage* buf, float sigma); + static void ComputeDOG(CuTexImage* gus, CuTexImage* dog, CuTexImage* got); + static void ComputeKEY(CuTexImage* dog, CuTexImage* key, float Tdog, float Tedge); + static void InitHistogram(CuTexImage* key, CuTexImage* hist); + static void ReduceHistogram(CuTexImage*hist1, CuTexImage* hist2); + static void GenerateList(CuTexImage* list, CuTexImage* hist); + static void ComputeOrientation(CuTexImage*list, CuTexImage* got, CuTexImage*key, + float sigma, float sigma_step, int existing_keypoint); + static void ComputeDescriptor(CuTexImage*list, CuTexImage* got, CuTexImage* dtex, int rect = 0, int stream = 0); + + //data conversion + static void SampleImageU(CuTexImage *dst, CuTexImage *src, int log_scale); + static void SampleImageD(CuTexImage *dst, CuTexImage *src, int log_scale = 1); + static void ReduceToSingleChannel(CuTexImage* dst, CuTexImage* src, int convert_rgb); + static void ConvertByteToFloat(CuTexImage*src, CuTexImage* dst); + + //visualization + static void DisplayConvertDOG(CuTexImage* dog, CuTexImage* out); + static void DisplayConvertGRD(CuTexImage* got, CuTexImage* out); + static void DisplayConvertKEY(CuTexImage* key, CuTexImage* dog, CuTexImage* out); + static void DisplayKeyPoint(CuTexImage* ftex, CuTexImage* out); + static void DisplayKeyBox(CuTexImage* ftex, CuTexImage* out); + + //SIFTMATCH FUNCTIONS + static void MultiplyDescriptor(CuTexImage* tex1, CuTexImage* tex2, CuTexImage* texDot, CuTexImage* texCRT); + static void MultiplyDescriptorG(CuTexImage* texDes1, CuTexImage* texDes2, + CuTexImage* texLoc1, CuTexImage* texLoc2, CuTexImage* texDot, CuTexImage* texCRT, + float* H, float hdistmax, float* F, float fdistmax); + static void GetRowMatch(CuTexImage* texDot, CuTexImage* texMatch, float distmax, float ratiomax); + static void GetColMatch(CuTexImage* texCRT, CuTexImage* texMatch, float distmax, float ratiomax); +}; + +#endif +#endif + diff --git a/colmap/include/colmap/lib/SiftGPU/ProgramGLSL.h b/colmap/include/colmap/lib/SiftGPU/ProgramGLSL.h new file mode 100644 index 0000000000000000000000000000000000000000..d83dc26aef0d029b5bdb4dd39a1760e8c6284e03 --- /dev/null +++ b/colmap/include/colmap/lib/SiftGPU/ProgramGLSL.h @@ -0,0 +1,267 @@ +//////////////////////////////////////////////////////////////////////////// +// File: ProgramGLSL.h +// Author: Changchang Wu +// Description : Interface for ProgramGLSL classes +// ProgramGLSL: Glsl Program +// FilterGLSL: Glsl Gaussian Filters +// ShaderBag: base class of ShaderBagPKSL and ShaderBagGLSL +// +// Copyright (c) 2007 University of North Carolina at Chapel Hill +// All Rights Reserved +// +// Permission to use, copy, modify and distribute this software and its +// documentation for educational, research and non-profit purposes, without +// fee, and without a written agreement is hereby granted, provided that the +// above copyright notice and the following paragraph appear in all copies. +// +// The University of North Carolina at Chapel Hill make no representations +// about the suitability of this software for any purpose. It is provided +// 'as is' without express or implied warranty. +// +// Please send BUG REPORTS to ccwu@cs.unc.edu +// +//////////////////////////////////////////////////////////////////////////// + + +#ifndef _PROGRAM_GLSL_H +#define _PROGRAM_GLSL_H + + +#include "ProgramGPU.h" + +class ProgramGLSL:public ProgramGPU +{ + class ShaderObject + { + GLuint _shaderID; + int _type; + int _compiled; + static int ReadShaderFile(const char * source, char *& code); + void CheckCompileLog(); + public: + void PrintCompileLog(ostream & os ); + int inline IsValidShaderObject(){ return _shaderID && _compiled;} + int IsValidVertexShader(); + int IsValidFragmentShader(); + GLuint GetShaderID(){return _shaderID;} + ~ShaderObject(); + ShaderObject(int shadertype, const char * source, int filesource =0); + }; + +protected: + int _linked; + GLint _TextureParam0; + GLuint _programID; +private: + void AttachShaderObject(ShaderObject& shader); + void DetachShaderObject(ShaderObject& shader); + +public: + void ReLink(); + int IsNative(); + int UseProgram(); + void PrintLinkLog(std::ostream&os); + int ValidateProgram(); + void CheckLinkLog(); + int LinkProgram(); + operator GLuint (){return _programID;} + virtual void * GetProgramID() { return (void*) _programID; } +public: + ProgramGLSL(); + ~ProgramGLSL(); + ProgramGLSL(const char* frag_source); +}; + + +class GLTexImage; +class FilterGLSL : public FilterProgram +{ +private: + ProgramGPU* CreateFilterH(float kernel[], int width); + ProgramGPU* CreateFilterV(float kernel[], int height); + ProgramGPU* CreateFilterHPK(float kernel[], int width); + ProgramGPU* CreateFilterVPK(float kernel[], int height); +public: + void MakeFilterProgram(float kernel[], int width); +public: + FilterGLSL(float sigma) ; +}; + +class SiftParam; + +///////////////////////////////////////////////////////////////////////////////// +//class ShaderBag +//desciption: pure virtual class +// provides storage and usage interface of all the shaders for SIFT +// two implementations are ShaderBagPKSL and ShaderBagGLSL +///////////////////////////////////////////////////////////////////////////////// +class ShaderBag +{ +public: + //shader: rgb to gray + ProgramGPU * s_gray; + //shader: copy keypoint to PBO + ProgramGPU * s_copy_key; + //shader: debug view + ProgramGPU * s_debug; + //shader: orientation + //shader: assign simple orientation to keypoints if hardware is low + ProgramGPU * s_orientation; + //shader: display gaussian levels + ProgramGPU * s_display_gaussian; + //shader: display difference of gassian + ProgramGPU * s_display_dog; + //shader: display gradient + ProgramGPU * s_display_grad; + //shader: display keypoints as red(maximum) and blue (minimum) + ProgramGPU * s_display_keys; + //shader: up/down-sample + ProgramGPU * s_sampling; + //shader: compute gradient/dog + ProgramGPU * s_grad_pass; + ProgramGPU * s_dog_pass; + //shader: keypoint detection in one pass + ProgramGPU * s_keypoint; + ProgramGPU * s_seperate_sp; + //shader: feature list generations.. + ProgramGPU * s_genlist_init_tight; + ProgramGPU * s_genlist_init_ex; + ProgramGPU * s_genlist_histo; + ProgramGPU * s_genlist_start; + ProgramGPU * s_genlist_step; + ProgramGPU * s_genlist_end; + ProgramGPU * s_zero_pass; + //shader: generate vertex to display SIFT as a square + ProgramGPU * s_vertex_list; + //shader: descriptor + ProgramGPU * s_descriptor_fp; + //shader: copy pixels to margin + ProgramGPU * s_margin_copy; +public: + FilterProgram * f_gaussian_skip0; + vector f_gaussian_skip0_v; + FilterProgram * f_gaussian_skip1; + FilterProgram ** f_gaussian_step; + int _gaussian_step_num; +public: + virtual void SetGenListInitParam(int w, int h){}; + virtual void SetGenListEndParam(int ktex){}; + virtual void SetMarginCopyParam(int xmax, int ymax){}; + virtual void LoadDescriptorShader(){}; + virtual void SetFeatureDescirptorParam(int gtex, int otex, float dwidth, float fwidth, float width, float height, float sigma){}; + virtual void SetFeatureOrientationParam(int gtex, int width, int height, float sigma, int stex, float step){}; + virtual void SetSimpleOrientationInput(int oTex, float sigma, float sigma_step){}; + virtual void LoadOrientationShader() =0; + virtual void SetGenListStartParam(float width, int tex0) =0; + virtual void LoadGenListShader(int ndoglev, int nlev)=0; + virtual void UnloadProgram()=0; + virtual void LoadKeypointShader(float threshold, float edgeTrheshold) = 0; + virtual void LoadFixedShaders()=0; + virtual void LoadDisplayShaders() = 0; + virtual void SetDogTexParam(int texU, int texD)=0; + virtual void SetGradPassParam(int texP=0){} + virtual void SetGenListStepParam(int tex, int tex0) = 0; + virtual void SetGenVBOParam( float width, float fwidth, float size)=0; +public: + void CreateGaussianFilters(SiftParam¶m); + void SelectInitialSmoothingFilter(int octave_min, SiftParam¶m); + void LoadDynamicShaders(SiftParam& param); + ShaderBag(); + virtual ~ShaderBag(); +}; + + +class ShaderBagGLSL:public ShaderBag +{ + GLint _param_dog_texu; + GLint _param_dog_texd; + GLint _param_ftex_width; + GLint _param_genlist_start_tex0; + GLint _param_genlist_step_tex0; + GLint _param_genvbo_size; + GLint _param_orientation_gtex; + GLint _param_orientation_size; + GLint _param_orientation_stex; + GLint _param_margin_copy_truncate; + GLint _param_genlist_init_bbox; + GLint _param_descriptor_gtex; + GLint _param_descriptor_size; + GLint _param_descriptor_dsize; +public: + virtual void SetMarginCopyParam(int xmax, int ymax); + void SetSimpleOrientationInput(int oTex, float sigma, float sigma_step); + void LoadOrientationShader(); + void LoadDescriptorShaderF2(); + virtual void LoadDescriptorShader(); + virtual void SetFeatureOrientationParam(int gtex, int width, int height, float sigma, int stex = 0, float step = 1.0f); + virtual void SetFeatureDescirptorParam(int gtex, int otex, float dwidth, float fwidth, float width, float height, float sigma); + static void WriteOrientationCodeToStream(ostream& out); + static ProgramGLSL* LoadGenListStepShader(int start, int step); + virtual void SetGenListInitParam(int w, int h); + virtual void SetGenListStartParam(float width, int tex0); + virtual void LoadGenListShader(int ndoglev, int nlev); + virtual void UnloadProgram(); + virtual void LoadKeypointShader(float threshold, float edgeTrheshold); + virtual void LoadFixedShaders(); + virtual void LoadDisplayShaders(); + virtual void SetDogTexParam(int texU, int texD); + virtual void SetGenListStepParam(int tex, int tex0); + virtual void SetGenVBOParam( float width, float fwidth, float size); + virtual ~ShaderBagGLSL(){} +}; + + +class ShaderBagPKSL:public ShaderBag +{ +private: + GLint _param_dog_texu; + GLint _param_dog_texd; + GLint _param_dog_texi; + GLint _param_margin_copy_truncate; + GLint _param_grad_pass_texp; + GLint _param_genlist_init_bbox; + GLint _param_genlist_start_tex0; + GLint _param_ftex_width; + GLint _param_genlist_step_tex0; + GLint _param_genlist_end_ktex; + GLint _param_genvbo_size; + GLint _param_orientation_gtex; + GLint _param_orientation_otex; + GLint _param_orientation_size; + GLint _param_descriptor_gtex; + GLint _param_descriptor_otex; + GLint _param_descriptor_size; + GLint _param_descriptor_dsize; + + // + ProgramGLSL* s_rect_description; +public: + ShaderBagPKSL () {s_rect_description = NULL; } + virtual ~ShaderBagPKSL() {if(s_rect_description) delete s_rect_description; } + virtual void LoadFixedShaders(); + virtual void LoadDisplayShaders(); + virtual void LoadOrientationShader() ; + virtual void SetGenListStartParam(float width, int tex0) ; + virtual void LoadGenListShader(int ndoglev, int nlev); + virtual void UnloadProgram(); + virtual void LoadKeypointShader(float threshold, float edgeTrheshold) ; + virtual void LoadDescriptorShader(); + virtual void LoadDescriptorShaderF2(); + static ProgramGLSL* LoadDescriptorProgramRECT(); + static ProgramGLSL* LoadDescriptorProgramPKSL(); +///////////////// + virtual void SetDogTexParam(int texU, int texD); + virtual void SetGradPassParam(int texP); + virtual void SetGenListStepParam(int tex, int tex0); + virtual void SetGenVBOParam( float width, float fwidth, float size); + virtual void SetFeatureDescirptorParam(int gtex, int otex, float dwidth, float fwidth, float width, float height, float sigma); + virtual void SetFeatureOrientationParam(int gtex, int width, int height, float sigma, int stex, float step); + virtual void SetSimpleOrientationInput(int oTex, float sigma, float sigma_step); + virtual void SetGenListEndParam(int ktex); + virtual void SetGenListInitParam(int w, int h); + virtual void SetMarginCopyParam(int xmax, int ymax); +}; + + +#endif + diff --git a/colmap/include/colmap/lib/SiftGPU/ProgramGPU.h b/colmap/include/colmap/lib/SiftGPU/ProgramGPU.h new file mode 100644 index 0000000000000000000000000000000000000000..203e52db4325e4b2b9effa02f3fa7e36770611f7 --- /dev/null +++ b/colmap/include/colmap/lib/SiftGPU/ProgramGPU.h @@ -0,0 +1,59 @@ +//////////////////////////////////////////////////////////////////////////// +// File: ProgramGPU.h +// Author: Changchang Wu +// Description : Based class for GPU programs +// ProgramGPU: base class of ProgramGLSL +// FilterProgram: base class of FilterGLSL, FilterPKSL +// +// Copyright (c) 2007 University of North Carolina at Chapel Hill +// All Rights Reserved +// +// Permission to use, copy, modify and distribute this software and its +// documentation for educational, research and non-profit purposes, without +// fee, and without a written agreement is hereby granted, provided that the +// above copyright notice and the following paragraph appear in all copies. +// +// The University of North Carolina at Chapel Hill make no representations +// about the suitability of this software for any purpose. It is provided +// 'as is' without express or implied warranty. +// +// Please send BUG REPORTS to ccwu@cs.unc.edu +// +//////////////////////////////////////////////////////////////////////////// + + +#ifndef _PROGRAM_GPU_H +#define _PROGRAM_GPU_H + +//////////////////////////////////////////////////////////////////////////// +//class ProgramGPU +//description: pure virtual class +// provides a common interface for shader programs +/////////////////////////////////////////////////////////////////////////// +class ProgramGPU +{ +public: + //use a gpu program + virtual int UseProgram() = 0; + virtual void* GetProgramID() = 0; + //not used + virtual ~ProgramGPU(){}; +}; + +/////////////////////////////////////////////////////////////////////////// +//class FilterProgram +/////////////////////////////////////////////////////////////////////////// +class FilterProgram +{ +public: + ProgramGPU* s_shader_h; + ProgramGPU* s_shader_v; + int _size; + int _id; +public: + FilterProgram() { s_shader_h = s_shader_v = NULL; _size = _id = 0; } + virtual ~FilterProgram() { if(s_shader_h) delete s_shader_h; if(s_shader_v) delete s_shader_v;} +}; + +#endif + diff --git a/colmap/include/colmap/lib/SiftGPU/PyramidCL.h b/colmap/include/colmap/lib/SiftGPU/PyramidCL.h new file mode 100644 index 0000000000000000000000000000000000000000..1ff6b181cdfb3eec29a236bccf6e22106256a2aa --- /dev/null +++ b/colmap/include/colmap/lib/SiftGPU/PyramidCL.h @@ -0,0 +1,83 @@ +//////////////////////////////////////////////////////////////////////////// +// File: PyramidCL.h +// Author: Changchang Wu +// Description : interface for the PyramdCL +// +// Copyright (c) 2007 University of North Carolina at Chapel Hill +// All Rights Reserved +// +// Permission to use, copy, modify and distribute this software and its +// documentation for educational, research and non-profit purposes, without +// fee, and without a written agreement is hereby granted, provided that the +// above copyright notice and the following paragraph appear in all copies. +// +// The University of North Carolina at Chapel Hill make no representations +// about the suitability of this software for any purpose. It is provided +// 'as is' without express or implied warranty. +// +// Please send BUG REPORTS to ccwu@cs.unc.edu +// +//////////////////////////////////////////////////////////////////////////// + + + +#ifndef _PYRAMID_CL_H +#define _PYRAMID_CL_H +#if defined(CL_SIFTGPU_ENABLED) + +class CLTexImage; +class SiftPyramid; +class ProgramBagCL; +class PyramidCL: public SiftPyramid +{ + CLTexImage* _inputTex; + CLTexImage* _allPyramid; + CLTexImage* _histoPyramidTex; + CLTexImage* _featureTex; + CLTexImage* _descriptorTex; + CLTexImage* _orientationTex; + ProgramBagCL* _OpenCL; + GLTexImage* _bufferTEX; +public: + virtual void GetFeatureDescriptors(); + virtual void GenerateFeatureListTex(); + virtual void ReshapeFeatureListCPU(); + virtual void GenerateFeatureDisplayVBO(); + virtual void DestroySharedData(); + virtual void DestroyPerLevelData(); + virtual void DestroyPyramidData(); + virtual void DownloadKeypoints(); + virtual void GenerateFeatureListCPU(); + virtual void GenerateFeatureList(); + virtual GLTexImage* GetLevelTexture(int octave, int level); + virtual GLTexImage* GetLevelTexture(int octave, int level, int dataName); + virtual void BuildPyramid(GLTexInput * input); + virtual void DetectKeypointsEX(); + virtual void ComputeGradient(); + virtual void GetFeatureOrientations(); + virtual void GetSimplifiedOrientation(); + virtual void InitPyramid(int w, int h, int ds = 0); + virtual void ResizePyramid(int w, int h); + + ////////// + void CopyGradientTex(); + void FitPyramid(int w, int h); + + void InitializeContext(); + int ResizeFeatureStorage(); + int FitHistogramPyramid(CLTexImage* tex); + void SetLevelFeatureNum(int idx, int fcount); + void ConvertInputToCL(GLTexInput* input, CLTexImage* output); + GLTexImage* ConvertTexCL2GL(CLTexImage* tex, int dataName); + CLTexImage* GetBaseLevel(int octave, int dataName = DATA_GAUSSIAN); +private: + void GenerateFeatureList(int i, int j, int reduction_count, vector& hbuffer); +public: + PyramidCL(SiftParam& sp); + virtual ~PyramidCL(); +}; + + +#endif +#endif + diff --git a/colmap/include/colmap/lib/SiftGPU/PyramidCU.h b/colmap/include/colmap/lib/SiftGPU/PyramidCU.h new file mode 100644 index 0000000000000000000000000000000000000000..feb7adebf327927a5fed99035dc3fd8382c209eb --- /dev/null +++ b/colmap/include/colmap/lib/SiftGPU/PyramidCU.h @@ -0,0 +1,86 @@ +//////////////////////////////////////////////////////////////////////////// +// File: PyramidCU.h +// Author: Changchang Wu +// Description : interface for the PyramdCU +// +// Copyright (c) 2007 University of North Carolina at Chapel Hill +// All Rights Reserved +// +// Permission to use, copy, modify and distribute this software and its +// documentation for educational, research and non-profit purposes, without +// fee, and without a written agreement is hereby granted, provided that the +// above copyright notice and the following paragraph appear in all copies. +// +// The University of North Carolina at Chapel Hill make no representations +// about the suitability of this software for any purpose. It is provided +// 'as is' without express or implied warranty. +// +// Please send BUG REPORTS to ccwu@cs.unc.edu +// +//////////////////////////////////////////////////////////////////////////// + + + +#ifndef _PYRAMID_CU_H +#define _PYRAMID_CU_H +#if defined(CUDA_SIFTGPU_ENABLED) + +class GLTexImage; +class CuTexImage; +class SiftPyramid; +class PyramidCU:public SiftPyramid +{ + CuTexImage* _inputTex; + CuTexImage* _allPyramid; + CuTexImage* _histoPyramidTex; + CuTexImage* _featureTex; + CuTexImage* _descriptorTex; + CuTexImage* _orientationTex; + GLuint _bufferPBO; + GLTexImage* _bufferTEX; +public: + virtual void GetFeatureDescriptors(); + virtual void GenerateFeatureListTex(); + virtual void ReshapeFeatureListCPU(); + virtual void GenerateFeatureDisplayVBO(); + virtual void DestroySharedData(); + virtual void DestroyPerLevelData(); + virtual void DestroyPyramidData(); + virtual void DownloadKeypoints(); + virtual void GenerateFeatureListCPU(); + virtual void GenerateFeatureList(); + virtual GLTexImage* GetLevelTexture(int octave, int level); + virtual GLTexImage* GetLevelTexture(int octave, int level, int dataName); + virtual void BuildPyramid(GLTexInput * input); + virtual void DetectKeypointsEX(); + virtual void ComputeGradient(); + virtual void GetFeatureOrientations(); + virtual void GetSimplifiedOrientation(); + virtual void InitPyramid(int w, int h, int ds = 0); + virtual void ResizePyramid(int w, int h); + virtual int IsUsingRectDescription(){return _existing_keypoints & SIFT_RECT_DESCRIPTION; } + ////////// + void CopyGradientTex(); + void FitPyramid(int w, int h); + + void InitializeContext(); + int ResizeFeatureStorage(); + int FitHistogramPyramid(CuTexImage* tex); + void SetLevelFeatureNum(int idx, int fcount); + void ConvertInputToCU(GLTexInput* input); + GLTexImage* ConvertTexCU2GL(CuTexImage* tex, int dataName); + CuTexImage* GetBaseLevel(int octave, int dataName = DATA_GAUSSIAN); + void TruncateWidth(int& w) { w = GLTexInput::TruncateWidthCU(w); } + ////////////////////////// + static int CheckCudaDevice(int device); +private: + void GenerateFeatureList(int i, int j, int reduction_count, vector& hbuffer); +public: + PyramidCU(SiftParam& sp); + virtual ~PyramidCU(); +}; + + + +#endif +#endif diff --git a/colmap/include/colmap/lib/SiftGPU/PyramidGL.h b/colmap/include/colmap/lib/SiftGPU/PyramidGL.h new file mode 100644 index 0000000000000000000000000000000000000000..a5baafc232ef4818ffa0dc40b7befb693d64e042 --- /dev/null +++ b/colmap/include/colmap/lib/SiftGPU/PyramidGL.h @@ -0,0 +1,119 @@ +//////////////////////////////////////////////////////////////////////////// +// File: PyramidGL.h +// Author: Changchang Wu +// Description : interface for the PyramdGL +// class PyramidNaive and PyramidPacked are derived from PyramidGL +// +// Copyright (c) 2007 University of North Carolina at Chapel Hill +// All Rights Reserved +// +// Permission to use, copy, modify and distribute this software and its +// documentation for educational, research and non-profit purposes, without +// fee, and without a written agreement is hereby granted, provided that the +// above copyright notice and the following paragraph appear in all copies. +// +// The University of North Carolina at Chapel Hill make no representations +// about the suitability of this software for any purpose. It is provided +// 'as is' without express or implied warranty. +// +// Please send BUG REPORTS to ccwu@cs.unc.edu +// +//////////////////////////////////////////////////////////////////////////// + + + +#ifndef _PYRAMID_GL_H +#define _PYRAMID_GL_H + +class GLTexImage; +class SiftParam; +class ProgramGPU; +class ShaderMan; +class GlobalUtil; +class SiftPyramid; + +class PyramidGL:public SiftPyramid +{ +protected: + GLTexImage* _histoPyramidTex; + GLTexImage* _featureTex; + GLTexImage* _descriptorTex; + GLTexImage* _orientationTex; +public: + void InitializeContext(); + void SetLevelFeatureNum(int idx, int num); + void GetTextureStorageSize(int num, int &fw, int& fh); + void GetAlignedStorageSize(int num, int align, int &fw, int &fh); + static void InterlaceDescriptorF2(int w, int h, float* buf, float* pd, int step); + static void NormalizeDescriptor(int num, float*pd); + virtual void DownloadKeypoints(); + virtual int ResizeFeatureStorage(); + //////////////////////////// + virtual void DestroyPerLevelData(); + virtual void DestroySharedData(); + virtual void GetFeatureDescriptors(); + virtual void GenerateFeatureListTex(); + virtual void ReshapeFeatureListCPU(); + virtual void GenerateFeatureDisplayVBO(); + virtual void CleanUpAfterSIFT(); + virtual GLTexImage* GetBaseLevel(int octave, int dataName = DATA_GAUSSIAN)=0; +public: + PyramidGL(SiftParam&sp); + virtual ~PyramidGL(); +}; + +class PyramidNaive:public PyramidGL, public ShaderMan +{ +protected: + GLTexImage * _texPyramid; + GLTexImage * _auxPyramid; +public: + void DestroyPyramidData(); + void GetSimplifiedOrientation(); + void GenerateFeatureListCPU(); + virtual void GetFeatureOrientations(); + virtual void GenerateFeatureList(); + void DetectKeypointsEX(); + void ComputeGradient(); + GLTexImage* GetLevelTexture(int octave, int level); + GLTexImage* GetBaseLevel(int octave, int dataName = DATA_GAUSSIAN); + GLTexImage* GetLevelTexture(int octave, int level, int dataName); + void BuildPyramid(GLTexInput * input); + void InitPyramid(int w, int h, int ds); + void FitPyramid(int w, int h); + void ResizePyramid(int w, int h); + void FitHistogramPyramid(); + PyramidNaive(SiftParam & sp); + ~PyramidNaive(); +private: + void GenerateFeatureList(int i, int j); +}; + + +class PyramidPacked:public PyramidGL, public ShaderMan +{ + GLTexPacked * _allPyramid; +public: + PyramidPacked(SiftParam& sp); + ~PyramidPacked(); + void DestroyPyramidData(); + void DetectKeypointsEX(); + void ComputeGradient(); + void BuildPyramid(GLTexInput * input); + void InitPyramid(int w, int h, int ds); + void FitPyramid(int w, int h); + void ResizePyramid(int w, int h); + void FitHistogramPyramid(); + void GenerateFeatureListCPU(); + void GenerateFeatureList(); + void GetSimplifiedOrientation(); + void GetFeatureOrientations(); + GLTexImage* GetBaseLevel(int octave, int dataName = DATA_GAUSSIAN); + GLTexImage* GetLevelTexture(int octave, int level); + GLTexImage* GetLevelTexture(int octave, int level, int dataName); + virtual int IsUsingRectDescription(){return _existing_keypoints & SIFT_RECT_DESCRIPTION; } +private: + void GenerateFeatureList(int i, int j); +}; + +#endif diff --git a/colmap/include/colmap/lib/SiftGPU/ShaderMan.h b/colmap/include/colmap/lib/SiftGPU/ShaderMan.h new file mode 100644 index 0000000000000000000000000000000000000000..b5e49a9ae6a7ba4ebf2b767259fb195e6be0e260 --- /dev/null +++ b/colmap/include/colmap/lib/SiftGPU/ShaderMan.h @@ -0,0 +1,80 @@ +//////////////////////////////////////////////////////////////////////////// +// File: ShaderMan.h +// Author: Changchang Wu +// Description : interface for the ShaderMan class. +// This is a class that manages all the shaders for SIFT +// +// +// Copyright (c) 2007 University of North Carolina at Chapel Hill +// All Rights Reserved +// +// Permission to use, copy, modify and distribute this software and its +// documentation for educational, research and non-profit purposes, without +// fee, and without a written agreement is hereby granted, provided that the +// above copyright notice and the following paragraph appear in all copies. +// +// The University of North Carolina at Chapel Hill make no representations +// about the suitability of this software for any purpose. It is provided +// 'as is' without express or implied warranty. +// +// Please send BUG REPORTS to ccwu@cs.unc.edu +// +//////////////////////////////////////////////////////////////////////////// + + + +#ifndef _SIFT_SHADER_MAN_H +#define _SIFT_SHADER_MAN_H + + +#include "ProgramGPU.h" +#include "ProgramGLSL.h" +/////////////////////////////////////////////////////////////////// +//class ShaderMan +//description: pure static class +// wrapper of shaders from different GPU languages +/////////////////////////////////////////////////////////////////// +class SiftParam; +class FilterGLSL; + +class ShaderMan +{ +public: + static ShaderBag* s_bag; +public: + static void SelectInitialSmoothingFilter(int octave_min, SiftParam¶m); + static void UseShaderMarginCopy(int xmax, int ymax); + static void UseShaderOrientation(int gtex, int width, int height, float sigma, int auxtex, float step, int keypoint_list); + static void UseShaderDescriptor(int gtex, int otex, int dwidth, int fwidth, int width, int height, float sigma); + static void UseShaderSimpleOrientation(int oTex, float sigma, float sigma_step); + static void UseShaderCopyKeypoint(); + static void UseShaderGenVBO( float width, float fwidth, float size); + static void UseShaderDebug(); + static void UseShaderZeroPass(); + static void UseShaderGenListStart(float fw, int tex0); + static void UseShaderGenListStep(int tex, int tex0); + static void UseShaderGenListEnd(int ktex); + static void UseShaderGenListHisto(); + static void UseShaderGenListInit(int w, int h, int tight = 1); + static void UseShaderKeypoint(int texU, int texD); + static void UseShaderGradientPass(int texP = 0); + static void UseShaderDisplayKeypoints(); + static void UseShaderDisplayGrad(); + static void UseShaderRGB2Gray(); + static void UseShaderDisplayDOG(); + static void UseShaderDisplayGaussian(); + /////////////////////////////////////////// + static void FilterInitialImage(GLTexImage* tex, GLTexImage* buf); + static void FilterSampledImage(GLTexImage* tex, GLTexImage* buf); + static void FilterImage(FilterProgram* filter, GLTexImage *dst, GLTexImage *src, GLTexImage*tmp); + static void TextureCopy(GLTexImage*dst, GLTexImage*src); + static void TextureDownSample(GLTexImage* dst, GLTexImage*src, int scale = 2); + static void TextureUpSample(GLTexImage* dst, GLTexImage*src, int scale); + /////////////////////////////////////////////// + static void InitShaderMan(SiftParam¶m); + static void DestroyShaders(); + static int HaveShaderMan(){return s_bag != NULL;} + static void UnloadProgram(); +}; + +#endif diff --git a/colmap/include/colmap/lib/SiftGPU/SiftGPU.h b/colmap/include/colmap/lib/SiftGPU/SiftGPU.h new file mode 100644 index 0000000000000000000000000000000000000000..47b292de51db0fb308c110d4a72b77693529491c --- /dev/null +++ b/colmap/include/colmap/lib/SiftGPU/SiftGPU.h @@ -0,0 +1,392 @@ +//////////////////////////////////////////////////////////////////////////// +// File: SiftGPU.h +// Author: Changchang Wu +// Description : interface for the SIFTGPU class. +// SiftGPU: The SiftGPU Tool. +// SiftGPUEX: SiftGPU + viewer +// SiftParam: Sift Parameters +// SiftMatchGPU: GPU SIFT Matcher; +// +// +// Copyright (c) 2007 University of North Carolina at Chapel Hill +// All Rights Reserved +// +// Permission to use, copy, modify and distribute this software and its +// documentation for educational, research and non-profit purposes, without +// fee, and without a written agreement is hereby granted, provided that the +// above copyright notice and the following paragraph appear in all copies. +// +// The University of North Carolina at Chapel Hill make no representations +// about the suitability of this software for any purpose. It is provided +// 'as is' without express or implied warranty. +// +// Please send BUG REPORTS to ccwu@cs.unc.edu +// +//////////////////////////////////////////////////////////////////////////// + + +#ifndef GPU_SIFT_H +#define GPU_SIFT_H + +#if defined(_WIN32) + #ifdef SIFTGPU_DLL + #ifdef DLL_EXPORT + #define SIFTGPU_EXPORT __declspec(dllexport) + #else + #define SIFTGPU_EXPORT __declspec(dllimport) + #endif + #else + #define SIFTGPU_EXPORT + #endif + #define SIFTGPU_EXPORT_EXTERN SIFTGPU_EXPORT + #if _MSC_VER > 1000 + #pragma once + #endif +#else + #define SIFTGPU_EXPORT + #define SIFTGPU_EXPORT_EXTERN extern "C" +#endif + +#ifdef _MSC_VER +#if _MSC_VER >= 1600 +#include +#else +typedef __int8 int8_t; +typedef __int16 int16_t; +typedef __int32 int32_t; +typedef __int64 int64_t; +typedef unsigned __int8 uint8_t; +typedef unsigned __int16 uint16_t; +typedef unsigned __int32 uint32_t; +typedef unsigned __int64 uint64_t; +#endif +#elif __GNUC__ >= 3 +#include +#endif + +/////////////////////////////////////////////////////////////////// +//clss SiftParam +//description: SIFT parameters +//////////////////////////////////////////////////////////////////// +class GlobalUtil; +class SiftParam +{ +public: + float* _sigma; + float _sigma_skip0; // + float _sigma_skip1; // + + //sigma of the first level + float _sigma0; + float _sigman; + int _sigma_num; + + //how many dog_level in an octave + int _dog_level_num; + int _level_num; + + //starting level in an octave + int _level_min; + int _level_max; + int _level_ds; + //dog threshold + float _dog_threshold; + //edge elimination + float _edge_threshold; + void ParseSiftParam(); +public: + float GetLevelSigma(int lev); + float GetInitialSmoothSigma(int octave_min); + SIFTGPU_EXPORT SiftParam(); +}; + +class LiteWindow; +class GLTexInput; +class ShaderMan; +class SiftPyramid; +class ImageList; +//////////////////////////////////////////////////////////////// +//class SIftGPU +//description: Interface of SiftGPU lib +//////////////////////////////////////////////////////////////// +class SiftGPU:public SiftParam +{ +public: + enum + { + SIFTGPU_NOT_SUPPORTED = 0, + SIFTGPU_PARTIAL_SUPPORTED = 1, // detction works, but not orientation/descriptor + SIFTGPU_FULL_SUPPORTED = 2 + }; + + int gpu_index = 0; + + typedef struct SiftKeypoint + { + float x, y, s, o; //x, y, scale, orientation. + }SiftKeypoint; +protected: + //when more than one images are specified + //_current indicates the active one + int _current; + //_initialized indicates if the shaders and OpenGL/SIFT parameters are initialized + //they are initialized only once for one SiftGPU inistance + //that is, SIFT parameters will not be changed + int _initialized; + //_image_loaded indicates if the current images are loaded + int _image_loaded; + //the name of current input image + char* _imgpath; + //_outpath containes the name of the output file + char* _outpath; + //the list of image filenames + ImageList * _list; + //the texture that holds loaded input image + GLTexInput * _texImage; + //the SiftPyramid + SiftPyramid * _pyramid; + //print out the command line options + static void PrintUsage(); + //Initialize OpenGL and SIFT paremeters, and create the shaders accordingly + void InitSiftGPU(); + //load the image list from a file + void LoadImageList(const char *imlist); +public: + //timing results for 10 steps + float _timing[10]; + inline const char* GetCurrentImagePath() {return _imgpath; } +public: + //set the image list for processing + SIFTGPU_EXPORT virtual void SetImageList(int nimage, const char** filelist); + //get the number of SIFT features in current image + SIFTGPU_EXPORT virtual int GetFeatureNum(); + //save the SIFT result as a ANSCII/BINARY file + SIFTGPU_EXPORT virtual void SaveSIFT(const char * szFileName); + //Copy the SIFT result to two vectors + SIFTGPU_EXPORT virtual void GetFeatureVector(SiftKeypoint * keys, float * descriptors); + //Set keypoint list before running sift to get descriptors + SIFTGPU_EXPORT virtual void SetKeypointList(int num, const SiftKeypoint * keys, int keys_have_orientation = 1); + //Enable downloading results to CPU. + //create a new OpenGL context for processing + //call VerifyContextGL instead if you want to crate openGL context yourself, or your are + //mixing mixing siftgpu with other openGL code + SIFTGPU_EXPORT virtual int CreateContextGL(); + //verify the current opengl context.. + //(for example, you call wglmakecurrent yourself and verify the current context) + SIFTGPU_EXPORT virtual int VerifyContextGL(); + //check if all siftgpu functions are supported + SIFTGPU_EXPORT virtual int IsFullSupported(); + //set verbose mode + SIFTGPU_EXPORT virtual void SetVerbose(int verbose = 4); + //set SiftGPU to brief display mode, which is faster + inline void SetVerboseBrief(){SetVerbose(2);}; + //parse SiftGPU parameters + SIFTGPU_EXPORT virtual void ParseParam(const int argc, const char **argv); + //run SIFT on a new image given filename + SIFTGPU_EXPORT virtual int RunSIFT(const char * imgpath); + //run SIFT on an image in the image list given the file index + SIFTGPU_EXPORT virtual int RunSIFT(int index); + //run SIFT on a new image given the pixel data and format/type; + //gl_format (e.g. GL_LUMINANCE, GL_RGB) is the format of the pixel data + //gl_type (e.g. GL_UNSIGNED_BYTE, GL_FLOAT) is the data type of the pixel data; + //Check glTexImage2D(...format, type,...) for the accepted values + //Using image data of GL_LUMINANCE + GL_UNSIGNED_BYTE can minimize transfer time + SIFTGPU_EXPORT virtual int RunSIFT(int width, int height, const void * data, + unsigned int gl_format, unsigned int gl_type); + //run SIFT on current image (specified by arguments), or processing the current image again + SIFTGPU_EXPORT virtual int RunSIFT(); + //run SIFT with keypoints on current image again. + SIFTGPU_EXPORT virtual int RunSIFT(int num, const SiftKeypoint * keys, int keys_have_orientation = 1); + //constructor, the parameter np is ignored.. + SIFTGPU_EXPORT SiftGPU(int np = 1); + //destructor + SIFTGPU_EXPORT virtual ~SiftGPU(); + //set the active pyramid...dropped function + SIFTGPU_EXPORT virtual void SetActivePyramid(int) {}; + //retrieve the number of images in the image list + SIFTGPU_EXPORT virtual int GetImageCount(); + //set parameter GlobalUtil::_ForceTightPyramid + SIFTGPU_EXPORT virtual void SetTightPyramid(int tight = 1); + //allocate pyramid for a given size of image + SIFTGPU_EXPORT virtual int AllocatePyramid(int width, int height); + //none of the texture in processing can be larger + //automatic down-sample is used if necessary. + SIFTGPU_EXPORT virtual void SetMaxDimension(int sz); + SIFTGPU_EXPORT int GetFeatureCountThreshold(); + SIFTGPU_EXPORT int GetMaxOrientation(); + SIFTGPU_EXPORT int GetMaxDimension(); +}; + + + +//////////////////////////////////////////////////////////////// +//class SIftGPUEX +//description: adds some visualization functions to the interface of SiftGPU +//////////////////////////////////////////////////////////////// + +class SiftGPUEX:public SiftGPU +{ + //view mode + int _view; + //sub view mode + int _sub_view; + //whether display a debug view + int _view_debug; + //colors for SIFT feature display + enum{COLOR_NUM = 36}; + float _colors[COLOR_NUM*3]; + //display functions + void DisplayInput(); //display gray level image of input image + void DisplayDebug(); //display debug view + void DisplayFeatureBox(int i); //display SIFT features + void DisplayLevel(void (*UseDisplayShader)(), int i); //display one level image + void DisplayOctave(void (*UseDisplayShader)(), int i); //display all images in one octave + //display different content of Pyramid by specifying different data and display shader + //the first nskip1 levels and the last nskip2 levels are skiped in display + void DisplayPyramid( void (*UseDisplayShader)(), int dataName, int nskip1 = 0, int nskip2 = 0); + //use HSVtoRGB to generate random colors + static void HSVtoRGB(float hsv[3],float rgb[3]); + +public: + SIFTGPU_EXPORT SiftGPUEX(); + //change view mode + SIFTGPU_EXPORT void SetView(int view, int sub_view, char * title); + //display current view + SIFTGPU_EXPORT void DisplaySIFT(); + //toggle debug mode on/off + SIFTGPU_EXPORT void ToggleDisplayDebug(); + //randomize the display colors + SIFTGPU_EXPORT void RandomizeColor(); + //retrieve the size of current input image + SIFTGPU_EXPORT void GetImageDimension(int &w, int&h); + //get the location of the window specified by user + SIFTGPU_EXPORT void GetInitWindowPotition(int& x, int& y); +}; + +///matcher export +//This is a gpu-based sift match implementation. +class SiftMatchGPU +{ +public: + enum SIFTMATCH_LANGUAGE { + SIFTMATCH_SAME_AS_SIFTGPU = 0, //when siftgpu already initialized. + SIFTMATCH_GLSL = 2, + SIFTMATCH_CUDA = 3, + SIFTMATCH_CUDA_DEVICE0 = 3 //to use device i, use SIFTMATCH_CUDA_DEVICE0 + i + }; + + int gpu_index = 0; + +private: + int __language; + SiftMatchGPU * __matcher; + virtual void InitSiftMatch(){} +protected: + int __max_sift; + //move the two functions here for derived class + SIFTGPU_EXPORT virtual int _CreateContextGL(); + SIFTGPU_EXPORT virtual int _VerifyContextGL(); +public: + //OpenGL Context creation/verification, initialization is done automatically inside + inline int CreateContextGL() {return _CreateContextGL();} + inline int VerifyContextGL() {return _VerifyContextGL();} + + //Consructor, the argument specifies the maximum number of features to match + SIFTGPU_EXPORT SiftMatchGPU(int max_sift = 4096); + + //change gpu_language, check the enumerants in SIFTMATCH_LANGUAGE. + SIFTGPU_EXPORT virtual void SetLanguage(int gpu_language); + + //after calling SetLanguage, you can call SetDeviceParam to select GPU + //-winpos, -display, -cuda [device_id] + //This is only used when you call CreateContextGL.. + //This function doesn't change the language. + SIFTGPU_EXPORT virtual void SetDeviceParam(int argc, char**argv); + + // Allocate all matrices the matrices and return true if successful. + virtual bool Allocate(int max_sift, int mbm); + + //change the maximum of features to match whenever you want + SIFTGPU_EXPORT virtual void SetMaxSift(int max_sift); + SIFTGPU_EXPORT virtual int GetMaxSift() const { return __max_sift; }; + //desctructor + SIFTGPU_EXPORT virtual ~SiftMatchGPU(); + + //Specifiy descriptors to match, index = [0/1] for two features sets respectively + //Option1, use float descriptors, and they be already normalized to 1.0 + SIFTGPU_EXPORT virtual void SetDescriptors(int index, int num, const float* descriptors, int id = -1); + //Option 2 unsigned char descriptors. They must be already normalized to 512 + SIFTGPU_EXPORT virtual void SetDescriptors(int index, int num, const unsigned char * descriptors, int id = -1); + + //match two sets of features, the function RETURNS the number of matches. + //Given two normalized descriptor d1,d2, the distance here is acos(d1 *d2); + SIFTGPU_EXPORT virtual int GetSiftMatch( + int max_match, // the length of the match_buffer. + uint32_t match_buffer[][2], //buffer to receive the matched feature indices + float distmax = 0.7, //maximum distance of sift descriptor + float ratiomax = 0.8, //maximum distance ratio + int mutual_best_match = 1); //mutual best match or one way + + //two functions for guded matching, two constraints can be used + //one homography and one fundamental matrix, the use is as follows + //1. for each image, first call SetDescriptor then call SetFeatureLocation + //2. Call GetGuidedSiftMatch + //input feature location is a vector of [float x, float y, float skip[gap]] + SIFTGPU_EXPORT virtual void SetFeautreLocation(int index, const float* locations, int gap = 0); + inline void SetFeatureLocation(int index, const SiftGPU::SiftKeypoint * keys) + { + SetFeautreLocation(index, (const float*) keys, 2); + } + + //use a guiding Homography H and a guiding Fundamental Matrix F to compute feature matches + //the function returns the number of matches. + SIFTGPU_EXPORT virtual int GetGuidedSiftMatch( + int max_match, uint32_t match_buffer[][2], //buffer to recieve + float* H, //homography matrix, (Set NULL to skip) + float* F, //fundamental matrix, (Set NULL to skip) + float distmax = 0.7, //maximum distance of sift descriptor + float ratiomax = 0.8, //maximum distance ratio + float hdistmax = 32, //threshold for |H * x1 - x2|_2 + float fdistmax = 16, //threshold for sampson error of x2'FX1 + int mutual_best_match = 1); //mutual best or one wayx +}; + +typedef SiftGPU::SiftKeypoint SiftKeypoint; + +//Two exported global functions used to create SiftGPU and SiftMatchGPU +SIFTGPU_EXPORT_EXTERN SiftGPU * CreateNewSiftGPU(int np =1); +SIFTGPU_EXPORT_EXTERN SiftMatchGPU* CreateNewSiftMatchGPU(int max_sift = 4096); + + +//////////////////////////////////////////////////////////////////////////// +class ComboSiftGPU: public SiftGPU, public SiftMatchGPU +{ +}; +SIFTGPU_EXPORT_EXTERN ComboSiftGPU* CreateComboSiftGPU(); + +///////////////////////////////////////////////////////////////////////////////////////////// +//Multi-process mode and remote mode +SIFTGPU_EXPORT_EXTERN ComboSiftGPU* CreateRemoteSiftGPU(int port = 7777, char* remote_server = NULL); +//Run SiftGPU computation on a remote computer/process/thread +//if( remote_server == NULL) +// a local server is created in a different process and connected +// multiple-GPU can be used by creating multiple instances +// GPU selection done through SiftGPU::ParseParam function +//otherwise, +// Assumes the existenc of a remote server and connects to it +// GPU selection skipped if already done on the server-end +// RUN server: server_siftgpu -server port [siftgpu_param] +//example: +// ComboSiftGPU * combo = CreateRemoteSiftGPU(7777, "my.gpuserver.com"); +// SiftGPU* siftgpu = combo, SiftMatchGPU * matcher = combo; +// siftgpu->ParseParam... siftgpu->CreateContextGL.. +// matcher->SetLanguage...matcher->VerifyContextGL... +// // GPU-selection is done throught siftgpu->ParseParam, +// // it doesn't really initialize SiftGPU untill you call CreateContextGL/VerifyContextGL +// delete combo; + +//////////////////////////////////////////////////////////////////////// +//two internally used function. +SIFTGPU_EXPORT int CreateLiteWindow(LiteWindow* window); +SIFTGPU_EXPORT void RunServerLoop(int port, int argc, char** argv); +#endif diff --git a/colmap/include/colmap/lib/SiftGPU/SiftMatch.h b/colmap/include/colmap/lib/SiftGPU/SiftMatch.h new file mode 100644 index 0000000000000000000000000000000000000000..dc4026d7624a812f082c206fc408068c138ee146 --- /dev/null +++ b/colmap/include/colmap/lib/SiftGPU/SiftMatch.h @@ -0,0 +1,93 @@ +//////////////////////////////////////////////////////////////////////////// +// File: SiftMatch.h +// Author: Changchang Wu +// Description : interface for the SiftMatchGL +//// +// Copyright (c) 2007 University of North Carolina at Chapel Hill +// All Rights Reserved +// +// Permission to use, copy, modify and distribute this software and its +// documentation for educational, research and non-profit purposes, without +// fee, and without a written agreement is hereby granted, provided that the +// above copyright notice and the following paragraph appear in all copies. +// +// The University of North Carolina at Chapel Hill make no representations +// about the suitability of this software for any purpose. It is provided +// 'as is' without express or implied warranty. +// +// Please send BUG REPORTS to ccwu@cs.unc.edu +// +//////////////////////////////////////////////////////////////////////////// + + +#ifndef GPU_SIFT_MATCH_H +#define GPU_SIFT_MATCH_H +class GLTexImage; +class ProgramGPU; + +class SiftMatchGL:public SiftMatchGPU +{ + typedef GLint ParameterGL; +private: + //tex storage + GLTexImage _texLoc[2]; + GLTexImage _texDes[2]; + GLTexImage _texDot; + GLTexImage _texMatch[2]; + + //programs + ProgramGPU * s_multiply; + ProgramGPU * s_guided_mult; + ProgramGPU * s_col_max; + ProgramGPU * s_row_max; + + //matching parameters + ParameterGL _param_multiply_tex1; + ParameterGL _param_multiply_tex2; + ParameterGL _param_multiply_size; + ParameterGL _param_rowmax_param; + ParameterGL _param_colmax_param; + + ///guided matching + ParameterGL _param_guided_mult_tex1; + ParameterGL _param_guided_mult_tex2; + ParameterGL _param_guided_mult_texl1; + ParameterGL _param_guided_mult_texl2; + ParameterGL _param_guided_mult_h; + ParameterGL _param_guided_mult_f; + ParameterGL _param_guided_mult_param; + // + int _num_sift[2]; + int _id_sift[2]; + int _have_loc[2]; + + //gpu parameter + int _sift_per_stripe; + int _sift_num_stripe; + int _sift_per_row; + int _pixel_per_sift; + int _initialized; + // + vector sift_buffer; +private: + void AllocateSiftMatch(); + void LoadSiftMatchShadersGLSL(); + int GetBestMatch(int max_match, uint32_t match_buffer[][2], float distmax, float ratiomax, int mbm); +public: + SiftMatchGL(int max_sift, int use_glsl); + virtual ~SiftMatchGL(); +public: + bool Allocate(int max_sift, int mbm) override; + void InitSiftMatch(); + void SetMaxSift(int max_sift) override; + void SetDescriptors(int index, int num, const unsigned char * descriptor, int id = -1); + void SetDescriptors(int index, int num, const float * descriptor, int id = -1); + void SetFeautreLocation(int index, const float* locatoins, int gap); + int GetSiftMatch(int max_match, uint32_t match_buffer[][2], float distmax, float ratiomax, int mbm); + int GetGuidedSiftMatch(int max_match, uint32_t match_buffer[][2], float* H, float* F, + float distmax, float ratiomax, float hdistmax,float fdistmax, int mbm); +}; + + +#endif + diff --git a/colmap/include/colmap/lib/SiftGPU/SiftMatchCU.h b/colmap/include/colmap/lib/SiftGPU/SiftMatchCU.h new file mode 100644 index 0000000000000000000000000000000000000000..8a684485bfd026967bb76c0d618d76a0cc2abe4b --- /dev/null +++ b/colmap/include/colmap/lib/SiftGPU/SiftMatchCU.h @@ -0,0 +1,68 @@ +//////////////////////////////////////////////////////////////////////////// +// File: SiftMatchCU.h +// Author: Changchang Wu +// Description : interface for the SiftMatchCU +//// +// Copyright (c) 2007 University of North Carolina at Chapel Hill +// All Rights Reserved +// +// Permission to use, copy, modify and distribute this software and its +// documentation for educational, research and non-profit purposes, without +// fee, and without a written agreement is hereby granted, provided that the +// above copyright notice and the following paragraph appear in all copies. +// +// The University of North Carolina at Chapel Hill make no representations +// about the suitability of this software for any purpose. It is provided +// 'as is' without express or implied warranty. +// +// Please send BUG REPORTS to ccwu@cs.unc.edu +// +//////////////////////////////////////////////////////////////////////////// + + + +#ifndef CU_SIFT_MATCH_H +#define CU_SIFT_MATCH_H +#if defined(CUDA_SIFTGPU_ENABLED) + +class CuTexImage; +class SiftMatchCU:public SiftMatchGPU +{ +private: + //tex storage + CuTexImage _texLoc[2]; + CuTexImage _texDes[2]; + CuTexImage _texDot; + CuTexImage _texMatch[2]; + CuTexImage _texCRT; + + //programs + // + int _num_sift[2]; + int _id_sift[2]; + int _have_loc[2]; + + //gpu parameter + int _initialized; + vector sift_buffer; +private: + int GetBestMatch(int max_match, uint32_t match_buffer[][2], float distmax, float ratiomax, int mbm); +public: + SiftMatchCU(int max_sift); + virtual ~SiftMatchCU(){}; + void InitSiftMatch(); + bool Allocate(int max_sift, int mbm) override; + void SetMaxSift(int max_sift) override; + void SetDescriptors(int index, int num, const unsigned char * descriptor, int id = -1); + void SetDescriptors(int index, int num, const float * descriptor, int id = -1); + void SetFeautreLocation(int index, const float* locatoins, int gap); + int GetSiftMatch(int max_match, uint32_t match_buffer[][2], float distmax, float ratiomax, int mbm); + int GetGuidedSiftMatch(int max_match, uint32_t match_buffer[][2], float* H, float* F, + float distmax, float ratiomax, float hdistmax, float fdistmax, int mbm); + ////////////////////////////// + static int CheckCudaDevice(int device); +}; + +#endif +#endif + diff --git a/colmap/include/colmap/lib/SiftGPU/SiftPyramid.h b/colmap/include/colmap/lib/SiftGPU/SiftPyramid.h new file mode 100644 index 0000000000000000000000000000000000000000..0845e630c4f29294d655aafffdab56d2b837e7d4 --- /dev/null +++ b/colmap/include/colmap/lib/SiftGPU/SiftPyramid.h @@ -0,0 +1,190 @@ +//////////////////////////////////////////////////////////////////////////// +// File: SiftPyramid.h +// Author: Changchang Wu +// Description : interface for the SiftPyramid class. +// SiftPyramid: data storage for SIFT +// |---PyramidGL: OpenGL based implementation +// | |--PyramidNaive: Unpacked version +// | |--PyramidPacked: packed version +// |--PyramidCU: CUDA-based implementation +// +// Copyright (c) 2007 University of North Carolina at Chapel Hill +// All Rights Reserved +// +// Permission to use, copy, modify and distribute this software and its +// documentation for educational, research and non-profit purposes, without +// fee, and without a written agreement is hereby granted, provided that the +// above copyright notice and the following paragraph appear in all copies. +// +// The University of North Carolina at Chapel Hill make no representations +// about the suitability of this software for any purpose. It is provided +// 'as is' without express or implied warranty. +// +// Please send BUG REPORTS to ccwu@cs.unc.edu +// +//////////////////////////////////////////////////////////////////////////// + + + +#ifndef _SIFT_PYRAMID_H +#define _SIFT_PYRAMID_H + + +class GLTexImage; +class GLTexInput; +class SiftParam; +class GlobalUtil; + +///////////////////////////////////////////////////////////////////////////// +//class SiftPyramid +//description: virutal class of SIFT data pyramid +// provides functions for SiftPU to run steps of GPU SIFT +// class PyramidNaive is the first implementation +// class PyramidPacked is a better OpenGL implementation +// class PyramidCU is a CUDA based implementation +///////////////////////////////////////////////////////////////////////////// + +#define NO_DUPLICATE_DOWNLOAD + +class SiftPyramid : public GlobalUtil +{ +public: + enum{ + DATA_GAUSSIAN = 0, + DATA_DOG = 1, + DATA_KEYPOINT = 2, + DATA_GRAD = 3, + DATA_ROT = 4, + DATA_NUM = 5 + }; + enum{ + SIFT_SKIP_FILTERING = 0x01, + SIFT_SKIP_DETECTION = 0x02, + SIFT_SKIP_ORIENTATION = 0x04, + SIFT_RECT_DESCRIPTION = 0x08 + }; +protected: + SiftParam& param; + int _hpLevelNum; + int* _levelFeatureNum; + int _featureNum; + float* _histo_buffer; + //keypoint list + int _existing_keypoints; + vector _keypoint_index; + //display vbo + GLuint* _featureDisplayVBO; + GLuint* _featurePointVBO; +public: + // + float _timing[8]; + //image size related + //first octave + int _octave_min; + //how many octaves + int _octave_num; + //pyramid storage + int _pyramid_octave_num; + int _pyramid_octave_first; + int _pyramid_width; + int _pyramid_height; + int _down_sample_factor; + int _allocated; + int _alignment; + int _siftgpu_failed; +public: + vector _keypoint_buffer; + vector _descriptor_buffer; +private: + inline void PrepareBuffer(); + inline void LimitFeatureCount(int have_keylist = 0); +public: + //shared by all implementations + virtual void RunSIFT(GLTexInput*input); + virtual void SaveSIFT(const char * szFileName); + virtual void CopyFeatureVector(float*keys, float *descriptors); + virtual void SetKeypointList(int num, const float * keys, int run_on_current, int skip_orientation); + //implementation-dependent functions + virtual void GetFeatureDescriptors() = 0; + virtual void GenerateFeatureListTex() =0; + virtual void ReshapeFeatureListCPU() =0; + virtual void GenerateFeatureDisplayVBO() =0; + virtual void DownloadKeypoints() = 0; + virtual void GenerateFeatureListCPU()=0; + virtual void GenerateFeatureList()=0; + virtual GLTexImage* GetLevelTexture(int octave, int level)=0; + virtual GLTexImage* GetLevelTexture(int octave, int level, int dataName) = 0; + virtual void BuildPyramid(GLTexInput * input)=0; + virtual void ResizePyramid(int w, int h) = 0; + virtual void InitPyramid(int w, int h, int ds = 0)=0; + virtual void DetectKeypointsEX() = 0; + virtual void ComputeGradient() = 0; + virtual void GetFeatureOrientations() = 0; + virtual void GetSimplifiedOrientation() = 0; + + //////////////////////////////// + virtual void CleanUpAfterSIFT() {} + virtual int IsUsingRectDescription() {return 0; } + static int GetRequiredOctaveNum(int inputsz); + + ///inline functions, shared by all implementations + inline void SetFailStatus() {_siftgpu_failed = 1; } + inline int GetSucessStatus() {return _siftgpu_failed == 0; } + inline int GetFeatureNum(){return _featureNum;} + inline int GetHistLevelNum(){return _hpLevelNum;} + inline const GLuint * GetFeatureDipslayVBO(){return _featureDisplayVBO;} + inline const GLuint * GetPointDisplayVBO(){return _featurePointVBO;} + inline const int * GetLevelFeatureNum(){return _levelFeatureNum;} + inline void GetPyramidTiming(float * timing){ for(int i = 0; i < 8; i++) timing[i] = _timing[i]; } + inline void CleanupBeforeSIFT() + { + _siftgpu_failed = 0; + for(int i = 0; i < 8; ++i) _timing[i] = 0; + } + SiftPyramid(SiftParam&sp):param(sp) + { + _featureNum = 0; + _featureDisplayVBO = 0; + _featurePointVBO = 0; + _levelFeatureNum = NULL; + _histo_buffer = NULL; + _hpLevelNum = 0; + + //image size + _octave_num = 0; + _octave_min = 0; + _alignment = 1; + _pyramid_octave_num = _pyramid_octave_first = 0; + _pyramid_width = _pyramid_height = 0; + _allocated = 0; + _down_sample_factor = 0; + + ///// + _existing_keypoints = 0; + } + virtual ~SiftPyramid() {}; + +#ifdef DEBUG_SIFTGPU +private: + void StopDEBUG(); + void BeginDEBUG(const char* imagepath); + void WriteTextureForDEBUG(GLTexImage * tex, const char * namet, ...); +#endif +}; + +#define SIFTGPU_ENABLE_REVERSE_ORDER +#ifdef SIFTGPU_ENABLE_REVERSE_ORDER +#define FIRST_OCTAVE(R) (R? _octave_num - 1 : 0) +#define NOT_LAST_OCTAVE(i, R) (R? (i >= 0) : (i < _octave_num)) +#define GOTO_NEXT_OCTAVE(i, R) (R? (--i) : (++i)) +#define FIRST_LEVEL(R) (R? param._dog_level_num - 1 : 0) +#define GOTO_NEXT_LEVEL(j, R) (R? (--j) : (++j)) +#define NOT_LAST_LEVEL(j, R) (R? (j >= 0) : (j < param._dog_level_num)) +#define FOR_EACH_OCTAVE(i, R) for(int i = FIRST_OCTAVE(R); NOT_LAST_OCTAVE(i, R); GOTO_NEXT_OCTAVE(i, R)) +#define FOR_EACH_LEVEL(j, R) for(int j = FIRST_LEVEL(R); NOT_LAST_LEVEL(j, R); GOTO_NEXT_LEVEL(j, R)) +#else +#define FOR_EACH_OCTAVE(i, R) for(int i = 0; i < _octave_num; ++i) +#define FOR_EACH_LEVEL(j, R) for(int j = 0; j < param._dog_level_num; ++j) +#endif + +#endif diff --git a/colmap/include/colmap/lib/VLFeat/aib.h b/colmap/include/colmap/lib/VLFeat/aib.h new file mode 100644 index 0000000000000000000000000000000000000000..ab7d4be2171b72f7877eda84d7ca679c68abe0e6 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/aib.h @@ -0,0 +1,134 @@ +/** @file aib.h + ** @brief AIB (@ref aib) + ** @author Brian Fulkerson + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_AIB_H +#define VL_AIB_H + +#include "generic.h" +#include "mathop.h" + +/** ------------------------------------------------------------------ + ** @internal + ** @brief AIB algorithm data + ** + ** The implementation is quite straightforward, but the way feature + ** values are handled in order to support efficient joins, + ** deletions and re-arrangement needs to be explained. This is + ** achieved by adding a layer of indirection: + ** - Call each feature value (either original or obtained by a join + ** operation) a node. Nodes are identified by numbers. + ** - Call each element of the various arrays (such as VlAIB::Px) + ** an entry. + ** - Entries are dynamically associated to nodes as specified by + ** VlAIB::nodes. For example, @c Px[i] refers to the node @c + ** nodes[i]. + **/ + +typedef struct _VlAIB +{ + vl_uint *nodes ; /**< Entires to nodes */ + vl_uint nentries ; /**< Total number of entries (= # active nodes) */ + double *beta ; /**< Minimum distance to an entry */ + vl_uint *bidx ; /**< Closest entry */ + + + vl_uint *which ; /**< List of entries to update */ + vl_uint nwhich ; /**< Number of entries to update */ + + double *Pcx; /**< Joint probability table */ + double *Px; /**< Marginal. */ + double *Pc; /**< Marginal. */ + vl_uint nvalues; /**< Number of feature values */ + vl_uint nlabels; /**< Number of labels */ + + vl_uint *parents; /**< Array of parents */ + double *costs; /**< Cost of each merge */ + + vl_uint verbosity ; /** Verbosity level */ +} VlAIB; + +/** @name Create and destroy + ** @{ + **/ +VL_EXPORT +VlAIB * vl_aib_new(double * Pcx, vl_uint nvalues, vl_uint nlabels); + +VL_EXPORT +void vl_aib_delete (VlAIB * aib); +/** @} */ + +/** @name Process data + ** @{ + **/ +VL_EXPORT +void vl_aib_process(VlAIB * aib); +/** @} */ + +/** @name Retrieve results + ** @{ + **/ +VL_INLINE vl_uint * vl_aib_get_parents(VlAIB const * aib); +VL_INLINE double * vl_aib_get_costs(VlAIB const * aib); +/** @} */ + + +/* ------------------------------------------------------------------- + * Inline functions implementation + * ---------------------------------------------------------------- */ + +/** ------------------------------------------------------------------ + ** @brief Get resulting list of parents + ** @param aib AIB filter. + ** @return An array of parents + **/ +VL_INLINE +vl_uint * vl_aib_get_parents(VlAIB const * aib) +{ + return aib->parents; +} + +/** ------------------------------------------------------------------ + ** @brief Get a list of merge costs + ** @param aib AIB filter. + ** @return An array of costs + **/ +VL_INLINE +double * vl_aib_get_costs(VlAIB const * aib) +{ + return aib->costs; +} + +/* ----------------------------------------------------------------- */ +/** @brief Set the verbosity + ** @param self AIB object. + ** @param verbosity a non-negative integer. + **/ +VL_INLINE void +vl_aib_set_verbosity (VlAIB * self, int verbosity) +{ + self->verbosity = verbosity ; +} + +/** @brief Get the verbosity + ** @param self AIB object. + ** @return the verbosity level. + **/ +VL_INLINE int +vl_aib_get_verbosity (VlAIB const * self) +{ + return self->verbosity ; +} + +/* VL_AIB_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/array.h b/colmap/include/colmap/lib/VLFeat/array.h new file mode 100644 index 0000000000000000000000000000000000000000..d29dc483ba40c7172cba77b5b8b5a31287ad1088 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/array.h @@ -0,0 +1,105 @@ +/** @file array.h + ** @brief Array - Definition + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_ARRAY_H +#define VL_ARRAY_H + +#include "generic.h" + +/** @brief Maximum number of array dimensions */ +#define VL_ARRAY_MAX_NUM_DIMENSIONS 16 + +/** @brief Numeric array */ +typedef struct _VlArray +{ + vl_type type ; + vl_bool isEnvelope ; + vl_bool isSparse ; + vl_size numDimensions ; + vl_size dimensions [VL_ARRAY_MAX_NUM_DIMENSIONS] ; + void * data ; + void * rowPointers ; + void * columnPointers ; +} VlArray ; + + +/** @name Get data and parameters + ** @{ */ + +/** @brief Get number of dimensions + ** @param self array. + ** @return number of dimensions. + **/ + +VL_INLINE vl_size +vl_array_get_num_dimensions (VlArray const * self) +{ + return self->numDimensions ; +} + +/** @brief Get dimensions + ** @param self array. + ** @return dimensions. + **/ + +VL_INLINE vl_size const * +vl_array_get_dimensions (VlArray const * self) +{ + return self->dimensions ; +} + +/** @brief Get data + ** @param self array. + ** @return data. + **/ + +VL_INLINE void * +vl_array_get_data (VlArray const * self) +{ + return self->data; +} + +/** @brief Get type + ** @param self array. + ** @return type. + **/ + +VL_INLINE vl_type +vl_array_get_data_type (VlArray const * self) +{ + return self->type ; +} + +VL_EXPORT vl_size vl_array_get_num_elements (VlArray const * self) ; + +/** @{ */ + +/** @name Constructing and destroying + ** @{ */ + +VL_EXPORT VlArray * vl_array_init (VlArray * self, vl_type type, vl_size numDimension, vl_size const * dimensions) ; +VL_EXPORT VlArray * vl_array_init_envelope (VlArray *self, void * data, vl_type type, vl_size numDimension, vl_size const * dimensions) ; +VL_EXPORT VlArray * vl_array_init_matrix (VlArray * self, vl_type type, vl_size numRows, vl_size numColumns) ; +VL_EXPORT VlArray * vl_array_init_matrix_envelope (VlArray * self, void * data, vl_type type, vl_size numRows, vl_size numColumns) ; + +VL_EXPORT VlArray * vl_array_new (vl_type type, vl_size numDimension, vl_size const * dimensions) ; +VL_EXPORT VlArray * vl_array_new_envelope (void * data, vl_type type, vl_size numDimension, vl_size const * dimensions) ; +VL_EXPORT VlArray * vl_array_new_matrix (vl_type type, vl_size numRows, vl_size numColumns) ; +VL_EXPORT VlArray * vl_array_new_matrix_envelope (void * data, vl_type type, vl_size numRows, vl_size numColumns) ; + +VL_EXPORT void vl_array_dealloc (VlArray * self) ; +VL_EXPORT void vl_array_delete (VlArray * self) ; +/** @} */ + +/* VL_ARRAY_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/covdet.h b/colmap/include/colmap/lib/VLFeat/covdet.h new file mode 100644 index 0000000000000000000000000000000000000000..c0fdabb72101a1b413300718df866fb92809d541 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/covdet.h @@ -0,0 +1,263 @@ +/** @file covdet.h + ** @brief Covariant feature detectors (@ref covdet) + ** @author Karel Lenc + ** @author Andrea Vedaldi + ** @author Michal Perdoch + **/ + +/* +Copyright (C) 2013-14 Andrea Vedaldi. +Copyright (C) 2012 Karel Lenc, Andrea Vedaldi and Michal Perdoch. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_COVDET_H +#define VL_COVDET_H + +#include "generic.h" +#include "stringop.h" +#include "scalespace.h" + +#include + +/* ---------------------------------------------------------------- */ +/* Feature Frames */ +/* ---------------------------------------------------------------- */ + +/** @name Feature frames + ** @{ */ + +/** @brief Types of feature frames */ +typedef enum _VlFrameType { + VL_FRAMETYPE_DISC = 1, /**< A disc. */ + VL_FRAMETYPE_ORIENTED_DISC, /**< An oriented disc. */ + VL_FRAMETYPE_ELLIPSE, /**< An ellipse. */ + VL_FRAMETYPE_ORIENTED_ELLIPSE, /**< An oriented ellipse. */ + VL_FRAMETYPE_NUM +} VlFrameType ; + +/** @brief Names of the frame types */ +VL_EXPORT const char* vlFrameNames [VL_FRAMETYPE_NUM] ; + +/** @brief Mapping between string values and VlFrameType values */ +VL_EXPORT VlEnumerator vlFrameTypes [VL_FRAMETYPE_NUM] ; + +/** @brief Disc feature frame */ +typedef struct _VlFrameDisc +{ + float x ; /**< center x-coordinate */ + float y ; /**< center y-coordinate */ + float sigma ; /**< radius or scale */ +} VlFrameDisc ; + +/** @brief Oriented disc feature frame + ** An upright frame has @c angle equal to zero. + **/ +typedef struct _VlFrameOrientedDisc { + float x ; /**< center x-coordinate */ + float y ; /**< center y-coordinate */ + float sigma ; /**< radius or scale */ + float angle ; /**< rotation angle (rad) */ +} VlFrameOrientedDisc ; + +/** @brief Ellipse feature frame */ +typedef struct _VlFrameEllipse { + float x ; /**< center x-coordinate */ + float y ; /**< center y-coordinate */ + float e11 ; /**< */ + float e12 ; + float e22 ; +} VlFrameEllipse ; + +/** @brief Oriented ellipse feature frame + ** The affine transformation transforms the ellipse shape into + ** a circular region. */ +typedef struct _VlFrameOrientedEllipse { + float x ; /**< center x-coordinate */ + float y ; /**< center y-coordinate */ + float a11 ; /**< */ + float a12 ; + float a21 ; + float a22 ; +} VlFrameOrientedEllipse; + +/** @brief Get the size of a frame structure + ** @param frameType identifier of the type of frame. + ** @return size of the corresponding frame structure in bytes. + **/ +VL_INLINE vl_size +vl_get_frame_size (VlFrameType frameType) { + switch (frameType) { + case VL_FRAMETYPE_DISC: return sizeof(VlFrameDisc); + case VL_FRAMETYPE_ORIENTED_DISC: return sizeof(VlFrameOrientedDisc); + case VL_FRAMETYPE_ELLIPSE: return sizeof(VlFrameEllipse); + case VL_FRAMETYPE_ORIENTED_ELLIPSE: return sizeof(VlFrameOrientedEllipse); + default: + assert(0); + break; + } + return 0; +} + +/** @brief Get the size of a frame structure + ** @param affineAdaptation whether the detector use affine adaptation. + ** @param orientation whether the detector estimates the feature orientation. + ** @return the type of extracted frame. + ** + ** Depedning on whether the detector estimate the affine shape + ** and orientation of a feature, different frame types + ** are extracted. */ + +VL_INLINE VlFrameType +vl_get_frame_type (vl_bool affineAdaptation, vl_bool orientation) +{ + if (affineAdaptation) { + if (orientation) { + return VL_FRAMETYPE_ORIENTED_ELLIPSE; + } else { + return VL_FRAMETYPE_ELLIPSE; + } + } else { + if (orientation) { + return VL_FRAMETYPE_ORIENTED_DISC; + } else { + return VL_FRAMETYPE_DISC; + } + } +} + +/* ---------------------------------------------------------------- */ +/* Covariant Feature Detector */ +/* ---------------------------------------------------------------- */ + +/** @brief A detected feature shape and location */ +typedef struct _VlCovDetFeature +{ + VlFrameOrientedEllipse frame ; /**< feature frame. */ + int o ; /**< Detected octave. */ + int s ; /**< Octave subdivision. */ + float peakScore ; /**< peak score. */ + float edgeScore ; /**< edge score. */ + float orientationScore ; /**< orientation score. */ + float laplacianScaleScore ; /**< Laplacian scale score. */ +} VlCovDetFeature ; + +/** @brief A detected feature orientation */ +typedef struct _VlCovDetFeatureOrientation +{ + double angle ; + double score ; +} VlCovDetFeatureOrientation ; + +/** @brief A detected feature Laplacian scale */ +typedef struct _VlCovDetFeatureLaplacianScale +{ + double scale ; + double score ; +} VlCovDetFeatureLaplacianScale ; + +/** @brief Covariant feature detection method */ +typedef enum _VlCovDetMethod +{ + VL_COVDET_METHOD_DOG = 1, + VL_COVDET_METHOD_HESSIAN, + VL_COVDET_METHOD_HESSIAN_LAPLACE, + VL_COVDET_METHOD_HARRIS_LAPLACE, + VL_COVDET_METHOD_MULTISCALE_HESSIAN, + VL_COVDET_METHOD_MULTISCALE_HARRIS, + VL_COVDET_METHOD_NUM +} VlCovDetMethod; + +/** @brief Mapping between strings and ::VlCovDetMethod values */ +VL_EXPORT VlEnumerator vlCovdetMethods [VL_COVDET_METHOD_NUM] ; + +#ifdef __DOXYGEN__ +/** @brief Covariant feature detector + ** @see @ref covdet */ +struct _VlCovDet { } +#endif + +/** @brief Covariant feature detector + ** @see @ref covdet */ +typedef struct _VlCovDet VlCovDet ; + +/** @name Create and destroy + ** @{ */ +VL_EXPORT VlCovDet * vl_covdet_new (VlCovDetMethod method) ; +VL_EXPORT void vl_covdet_delete (VlCovDet * self) ; +VL_EXPORT void vl_covdet_reset (VlCovDet * self) ; +/** @} */ + +/** @name Process data + ** @{ */ +VL_EXPORT int vl_covdet_put_image (VlCovDet * self, + float const * image, + vl_size width, vl_size height) ; + +VL_EXPORT void vl_covdet_detect (VlCovDet * self, vl_size max_num_features) ; +VL_EXPORT int vl_covdet_append_feature (VlCovDet * self, VlCovDetFeature const * feature) ; +VL_EXPORT void vl_covdet_extract_orientations (VlCovDet * self) ; +VL_EXPORT void vl_covdet_extract_laplacian_scales (VlCovDet * self) ; +VL_EXPORT void vl_covdet_extract_affine_shape (VlCovDet * self) ; + +VL_EXPORT VlCovDetFeatureOrientation * +vl_covdet_extract_orientations_for_frame (VlCovDet * self, + vl_size *numOrientations, + VlFrameOrientedEllipse frame) ; + +VL_EXPORT VlCovDetFeatureLaplacianScale * +vl_covdet_extract_laplacian_scales_for_frame (VlCovDet * self, + vl_size * numScales, + VlFrameOrientedEllipse frame) ; +VL_EXPORT int +vl_covdet_extract_affine_shape_for_frame (VlCovDet * self, + VlFrameOrientedEllipse * adapted, + VlFrameOrientedEllipse frame) ; + +VL_EXPORT vl_bool +vl_covdet_extract_patch_for_frame (VlCovDet * self, float * patch, + vl_size resolution, + double extent, + double sigma, + VlFrameOrientedEllipse frame) ; + +VL_EXPORT void +vl_covdet_drop_features_outside (VlCovDet * self, double margin) ; +/** @} */ + +/** @name Retrieve data and parameters + ** @{ */ +VL_EXPORT vl_size vl_covdet_get_num_features (VlCovDet const * self) ; +VL_EXPORT VlCovDetFeature * vl_covdet_get_features (VlCovDet * self) ; +VL_EXPORT vl_index vl_covdet_get_first_octave (VlCovDet const * self) ; +VL_EXPORT vl_size vl_covdet_get_octave_resolution (VlCovDet const * self) ; +VL_EXPORT double vl_covdet_get_peak_threshold (VlCovDet const * self) ; +VL_EXPORT double vl_covdet_get_edge_threshold (VlCovDet const * self) ; +VL_EXPORT double vl_covdeg_get_laplacian_peak_threshold (VlCovDet const * self) ; +VL_EXPORT vl_bool vl_covdet_get_transposed (VlCovDet const * self) ; +VL_EXPORT VlScaleSpace * vl_covdet_get_gss (VlCovDet const * self) ; +VL_EXPORT VlScaleSpace * vl_covdet_get_css (VlCovDet const * self) ; +VL_EXPORT vl_bool vl_covdet_get_aa_accurate_smoothing (VlCovDet const * self) ; +VL_EXPORT vl_size const * vl_covdet_get_laplacian_scales_statistics (VlCovDet const * self, vl_size * numScales) ; +VL_EXPORT double vl_covdet_get_non_extrema_suppression_threshold (VlCovDet const * self) ; +VL_EXPORT vl_size vl_covdet_get_num_non_extrema_suppressed (VlCovDet const * self) ; + +/** @} */ + +/** @name Set parameters + ** @{ */ +VL_EXPORT void vl_covdet_set_first_octave (VlCovDet * self, vl_index o) ; +VL_EXPORT void vl_covdet_set_octave_resolution (VlCovDet * self, vl_size r) ; +VL_EXPORT void vl_covdet_set_peak_threshold (VlCovDet * self, double peakThreshold) ; +VL_EXPORT void vl_covdet_set_edge_threshold (VlCovDet * self, double edgeThreshold) ; +VL_EXPORT void vl_covdet_set_laplacian_peak_threshold (VlCovDet * self, double peakThreshold) ; +VL_EXPORT void vl_covdet_set_transposed (VlCovDet * self, vl_bool t) ; +VL_EXPORT void vl_covdet_set_aa_accurate_smoothing (VlCovDet * self, vl_bool x) ; +VL_EXPORT void vl_covdet_set_non_extrema_suppression_threshold (VlCovDet * self, double x) ; +/** @} */ + +/* VL_COVDET_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/dsift.h b/colmap/include/colmap/lib/VLFeat/dsift.h new file mode 100644 index 0000000000000000000000000000000000000000..8bb301b7efe3ea91a1fb32a21e1f56765a4be639 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/dsift.h @@ -0,0 +1,354 @@ +/** @file dsift.h + ** @brief Dense SIFT (@ref dsift) + ** @author Andrea Vedaldi + ** @author Brian Fulkerson + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_DSIFT_H +#define VL_DSIFT_H + +#include "generic.h" + +/** @brief Dense SIFT keypoint */ +typedef struct VlDsiftKeypoint_ +{ + double x ; /**< x coordinate */ + double y ; /**< y coordinate */ + double s ; /**< scale */ + double norm ; /**< SIFT descriptor norm */ +} VlDsiftKeypoint ; + +/** @brief Dense SIFT descriptor geometry */ +typedef struct VlDsiftDescriptorGeometry_ +{ + int numBinT ; /**< number of orientation bins */ + int numBinX ; /**< number of bins along X */ + int numBinY ; /**< number of bins along Y */ + int binSizeX ; /**< size of bins along X */ + int binSizeY ; /**< size of bins along Y */ +} VlDsiftDescriptorGeometry ; + +/** @brief Dense SIFT filter */ +typedef struct VlDsiftFilter_ +{ + int imWidth ; /**< @internal @brief image width */ + int imHeight ; /**< @internal @brief image height */ + + int stepX ; /**< frame sampling step X */ + int stepY ; /**< frame sampling step Y */ + + int boundMinX ; /**< frame bounding box min X */ + int boundMinY ; /**< frame bounding box min Y */ + int boundMaxX ; /**< frame bounding box max X */ + int boundMaxY ; /**< frame bounding box max Y */ + + /** descriptor parameters */ + VlDsiftDescriptorGeometry geom ; + + int useFlatWindow ; /**< flag: whether to approximate the Gaussian window with a flat one */ + double windowSize ; /**< size of the Gaussian window */ + + int numFrames ; /**< number of sampled frames */ + int descrSize ; /**< size of a descriptor */ + VlDsiftKeypoint *frames ; /**< frame buffer */ + float *descrs ; /**< descriptor buffer */ + + int numBinAlloc ; /**< buffer allocated: descriptor size */ + int numFrameAlloc ; /**< buffer allocated: number of frames */ + int numGradAlloc ; /**< buffer allocated: number of orientations */ + + float **grads ; /**< gradient buffer */ + float *convTmp1 ; /**< temporary buffer */ + float *convTmp2 ; /**< temporary buffer */ +} VlDsiftFilter ; + +VL_EXPORT VlDsiftFilter *vl_dsift_new (int width, int height) ; +VL_EXPORT VlDsiftFilter *vl_dsift_new_basic (int width, int height, int step, int binSize) ; +VL_EXPORT void vl_dsift_delete (VlDsiftFilter *self) ; +VL_EXPORT void vl_dsift_process (VlDsiftFilter *self, float const* im) ; +VL_INLINE void vl_dsift_transpose_descriptor (float* dst, + float const* src, + int numBinT, + int numBinX, + int numBinY) ; + +/** @name Setting parameters + ** @{ + **/ +VL_INLINE void vl_dsift_set_steps (VlDsiftFilter *self, + int stepX, + int stepY) ; +VL_INLINE void vl_dsift_set_bounds (VlDsiftFilter *self, + int minX, + int minY, + int maxX, + int maxY) ; +VL_INLINE void vl_dsift_set_geometry (VlDsiftFilter *self, + VlDsiftDescriptorGeometry const* geom) ; +VL_INLINE void vl_dsift_set_flat_window (VlDsiftFilter *self, vl_bool useFlatWindow) ; +VL_INLINE void vl_dsift_set_window_size (VlDsiftFilter *self, double windowSize) ; +/** @} */ + +/** @name Retrieving data and parameters + ** @{ + **/ +VL_INLINE float const *vl_dsift_get_descriptors (VlDsiftFilter const *self) ; +VL_INLINE int vl_dsift_get_descriptor_size (VlDsiftFilter const *self) ; +VL_INLINE int vl_dsift_get_keypoint_num (VlDsiftFilter const *self) ; +VL_INLINE VlDsiftKeypoint const *vl_dsift_get_keypoints (VlDsiftFilter const *self) ; +VL_INLINE void vl_dsift_get_bounds (VlDsiftFilter const *self, + int* minX, + int* minY, + int* maxX, + int* maxY) ; +VL_INLINE void vl_dsift_get_steps (VlDsiftFilter const* self, + int* stepX, + int* stepY) ; +VL_INLINE VlDsiftDescriptorGeometry const* vl_dsift_get_geometry (VlDsiftFilter const *self) ; +VL_INLINE vl_bool vl_dsift_get_flat_window (VlDsiftFilter const *self) ; +VL_INLINE double vl_dsift_get_window_size (VlDsiftFilter const *self) ; +/** @} */ + +VL_EXPORT +void _vl_dsift_update_buffers (VlDsiftFilter *self) ; + +/** ------------------------------------------------------------------ + ** @brief Get descriptor size. + ** @param self DSIFT filter object. + ** @return size of a descriptor. + **/ + +int +vl_dsift_get_descriptor_size (VlDsiftFilter const *self) +{ + return self->descrSize ; +} + +/** ------------------------------------------------------------------ + ** @brief Get descriptors. + ** @param self DSIFT filter object. + ** @return descriptors. + **/ + +float const * +vl_dsift_get_descriptors (VlDsiftFilter const *self) +{ + return self->descrs ; +} + +/** ------------------------------------------------------------------ + ** @brief Get keypoints + ** @param self DSIFT filter object. + **/ + +VlDsiftKeypoint const * +vl_dsift_get_keypoints (VlDsiftFilter const *self) +{ + return self->frames ; +} + +/** ------------------------------------------------------------------ + ** @brief Get number of keypoints + ** @param self DSIFT filter object. + **/ + +int +vl_dsift_get_keypoint_num (VlDsiftFilter const *self) +{ + return self->numFrames ; +} + +/** ------------------------------------------------------------------ + ** @brief Get SIFT descriptor geometry + ** @param self DSIFT filter object. + ** @return DSIFT descriptor geometry. + **/ + +VlDsiftDescriptorGeometry const* vl_dsift_get_geometry (VlDsiftFilter const *self) +{ + return &self->geom ; +} + +/** ------------------------------------------------------------------ + ** @brief Get bounds + ** @param self DSIFT filter object. + ** @param minX bounding box minimum X coordinate. + ** @param minY bounding box minimum Y coordinate. + ** @param maxX bounding box maximum X coordinate. + ** @param maxY bounding box maximum Y coordinate. + **/ + +void +vl_dsift_get_bounds (VlDsiftFilter const* self, + int *minX, int *minY, int *maxX, int *maxY) +{ + *minX = self->boundMinX ; + *minY = self->boundMinY ; + *maxX = self->boundMaxX ; + *maxY = self->boundMaxY ; +} + +/** ------------------------------------------------------------------ + ** @brief Get flat window flag + ** @param self DSIFT filter object. + ** @return @c TRUE if the DSIFT filter uses a flat window. + **/ + +int +vl_dsift_get_flat_window (VlDsiftFilter const* self) +{ + return self->useFlatWindow ; +} + +/** ------------------------------------------------------------------ + ** @brief Get steps + ** @param self DSIFT filter object. + ** @param stepX sampling step along X. + ** @param stepY sampling step along Y. + **/ + +void +vl_dsift_get_steps (VlDsiftFilter const* self, + int* stepX, + int* stepY) +{ + *stepX = self->stepX ; + *stepY = self->stepY ; +} + +/** ------------------------------------------------------------------ + ** @brief Set steps + ** @param self DSIFT filter object. + ** @param stepX sampling step along X. + ** @param stepY sampling step along Y. + **/ + +void +vl_dsift_set_steps (VlDsiftFilter* self, + int stepX, + int stepY) +{ + self->stepX = stepX ; + self->stepY = stepY ; + _vl_dsift_update_buffers(self) ; +} + +/** ------------------------------------------------------------------ + ** @brief Set bounds + ** @param self DSIFT filter object. + ** @param minX bounding box minimum X coordinate. + ** @param minY bounding box minimum Y coordinate. + ** @param maxX bounding box maximum X coordinate. + ** @param maxY bounding box maximum Y coordinate. + **/ + +void +vl_dsift_set_bounds (VlDsiftFilter* self, + int minX, int minY, int maxX, int maxY) +{ + self->boundMinX = minX ; + self->boundMinY = minY ; + self->boundMaxX = maxX ; + self->boundMaxY = maxY ; + _vl_dsift_update_buffers(self) ; +} + +/** ------------------------------------------------------------------ + ** @brief Set SIFT descriptor geometry + ** @param self DSIFT filter object. + ** @param geom descriptor geometry parameters. + **/ + +void +vl_dsift_set_geometry (VlDsiftFilter *self, + VlDsiftDescriptorGeometry const *geom) +{ + self->geom = *geom ; + _vl_dsift_update_buffers(self) ; +} + +/** ------------------------------------------------------------------ + ** @brief Set flat window flag + ** @param self DSIFT filter object. + ** @param useFlatWindow @c true if the DSIFT filter should use a flat window. + **/ + +void +vl_dsift_set_flat_window (VlDsiftFilter* self, + vl_bool useFlatWindow) +{ + self->useFlatWindow = useFlatWindow ; +} + +/** ------------------------------------------------------------------ + ** @brief Transpose descriptor + ** + ** @param dst destination buffer. + ** @param src source buffer. + ** @param numBinT + ** @param numBinX + ** @param numBinY + ** + ** The function writes to @a dst the transpose of the SIFT descriptor + ** @a src. Let I be an image. The transpose operator + ** satisfies the equation transpose(dsift(I,x,y)) = + ** dsift(transpose(I),y,x) + **/ + +VL_INLINE void +vl_dsift_transpose_descriptor (float* dst, + float const* src, + int numBinT, + int numBinX, + int numBinY) +{ + int t, x, y ; + + for (y = 0 ; y < numBinY ; ++y) { + for (x = 0 ; x < numBinX ; ++x) { + int offset = numBinT * (x + y * numBinX) ; + int offsetT = numBinT * (y + x * numBinY) ; + + for (t = 0 ; t < numBinT ; ++t) { + int tT = numBinT / 4 - t ; + dst [offsetT + (tT + numBinT) % numBinT] = src [offset + t] ; + } + } + } +} + +/** ------------------------------------------------------------------ + ** @brief Set SIFT descriptor Gaussian window size + ** @param self DSIFT filter object. + ** @param windowSize window size. + **/ + +void +vl_dsift_set_window_size(VlDsiftFilter * self, double windowSize) +{ + assert(windowSize >= 0.0) ; + self->windowSize = windowSize ; +} + +/** ------------------------------------------------------------------ + ** @brief Get SIFT descriptor Gaussian window size + ** @param self DSIFT filter object. + ** @return window size. + **/ + +VL_INLINE double +vl_dsift_get_window_size(VlDsiftFilter const * self) +{ + return self->windowSize ; +} + +/* VL_DSIFT_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/fisher.h b/colmap/include/colmap/lib/VLFeat/fisher.h new file mode 100644 index 0000000000000000000000000000000000000000..907e66a48c5f3b775d4dfaf798994df6fb780113 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/fisher.h @@ -0,0 +1,57 @@ +/** @file fisher.h + ** @brief Fisher encoding (@ref fisher) + ** @author David Novotny + ** @author Andrea Vedaldi + ** @see @ref fisher + **/ + +/* +Copyright (C) 2013 David Novotny and Andrea Vedaldi. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_FISHER_H +#define VL_FISHER_H + +#include "generic.h" + +/** @name Fisher vector options + ** @{ */ +#define VL_FISHER_FLAG_SQUARE_ROOT (0x1 << 0) +#define VL_FISHER_FLAG_NORMALIZED (0x1 << 1) +#define VL_FISHER_FLAG_IMPROVED (VL_FISHER_FLAG_NORMALIZED|VL_FISHER_FLAG_SQUARE_ROOT) +#define VL_FISHER_FLAG_FAST (0x1 << 2) + +/** @def VL_FISHER_FLAG_SQUARE_ROOT + ** @brief Use signed squared-root (@ref fisher-normalization). + **/ + +/** @def VL_FISHER_FLAG_NORMALIZED + ** @brief Gobally normalize the Fisher vector in L2 norm (@ref fisher-normalization). + **/ + +/** @def VL_FISHER_FLAG_IMPROVED + ** @brief Improved Fisher vector. + ** This is the same as @c VL_FISHER_FLAG_SQUARE_ROOT|VL_FISHER_FLAG_NORMALIZED. + **/ + +/** @def VL_FISHER_FLAG_FAST + ** @brief Fast but more approximate calculations (@ref fisher-fast). + ** Keep only the larges data to cluster assignment (posterior). + **/ + +/** @} */ + +VL_EXPORT vl_size vl_fisher_encode +(void * enc, vl_type dataType, + void const * means, vl_size dimension, vl_size numClusters, + void const * covariances, + void const * priors, + void const * data, vl_size numData, + int flags) ; + +/* VL_FISHER_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/float.h b/colmap/include/colmap/lib/VLFeat/float.h new file mode 100644 index 0000000000000000000000000000000000000000..267ca98f81e11a18852e65a6114d17bab3f7ce9a --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/float.h @@ -0,0 +1,116 @@ +/** @file float.h + ** @brief Float - Template + ** @author Andrea Vedaldi + ** @author David Novotny + **/ + +/* +Copyright (C) 2014 Andrea Vedaldi. +Copyright (C) 2013 David Novotny. +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#include "generic.h" + +#undef T +#undef SFX +#undef VSIZE +#undef VSFX +#undef VTYPE +#undef VSIZEavx +#undef VSFXavx +#undef VTYPEavx + +#if (FLT == VL_TYPE_FLOAT) +# define T float +# define SFX f +#elif (FLT == VL_TYPE_DOUBLE) +# define T double +# define SFX d +#elif (FLT == VL_TYPE_UINT32) +# define T vl_uint32 +# define SFX ui32 +#elif (FLT == VL_TYPE_INT32) +# define T vl_int32 +# define SFX i32 +#endif + +/* ---------------------------------------------------------------- */ +/* AVX */ +/* ---------------------------------------------------------------- */ + +#ifdef __AVX__ + +#if (FLT == VL_TYPE_FLOAT) +# define VSIZEavx 8 +# define VSFXavx s +# define VTYPEavx __m256 +#elif (FLT == VL_TYPE_DOUBLE) +# define VSIZEavx 4 +# define VSFXavx d +# define VTYPEavx __m256d +#endif + +#define VALIGNEDavx(x) (! (((vl_uintptr)(x)) & 0x1F)) + +#define VMULavx VL_XCAT(_mm256_mul_p, VSFX) +#define VDIVavx VL_XCAT(_mm256_div_p, VSFX) +#define VADDavx VL_XCAT(_mm256_add_p, VSFX) +#define VHADDavx VL_XCAT(_mm_hadd_p, VSFX) +#define VHADD2avx VL_XCAT(_mm256_hadd_p, VSFX) +#define VSUBavx VL_XCAT(_mm256_sub_p, VSFX) +#define VSTZavx VL_XCAT(_mm256_setzero_p, VSFX) +#define VLD1avx VL_XCAT(_mm256_broadcast_s, VSFX) +#define VLDUavx VL_XCAT(_mm256_loadu_p, VSFX) +#define VST1avx VL_XCAT(_mm256_store_s, VSFX) +#define VST2avx VL_XCAT(_mm256_store_p, VSFX) +#define VST2Uavx VL_XCAT(_mm256_storeu_p, VSFX) +#define VPERMavx VL_XCAT(_mm256_permute2f128_p, VSFX) +//#define VCSTavx VL_XCAT( _mm256_castps256_ps128, VSFX) +#define VCSTavx VL_XCAT5(_mm256_castp,VSFX,256_p,VSFX,128) + +/* __AVX__ */ +#endif + +/* ---------------------------------------------------------------- */ +/* SSE2 */ +/* ---------------------------------------------------------------- */ + +#ifdef __SSE2__ + +#if (FLT == VL_TYPE_FLOAT) +# define VSIZE 4 +# define VSFX s +# define VTYPE __m128 +#elif (FLT == VL_TYPE_DOUBLE) +# define VSIZE 2 +# define VSFX d +# define VTYPE __m128d +#endif + +#define VALIGNED(x) (! (((vl_uintptr)(x)) & 0xF)) + +#define VMAX VL_XCAT(_mm_max_p, VSFX) +#define VMUL VL_XCAT(_mm_mul_p, VSFX) +#define VDIV VL_XCAT(_mm_div_p, VSFX) +#define VADD VL_XCAT(_mm_add_p, VSFX) +#define VSUB VL_XCAT(_mm_sub_p, VSFX) +#define VSTZ VL_XCAT(_mm_setzero_p, VSFX) +#define VLD1 VL_XCAT(_mm_load1_p, VSFX) +#define VLDU VL_XCAT(_mm_loadu_p, VSFX) +#define VST1 VL_XCAT(_mm_store_s, VSFX) +#define VSET1 VL_XCAT(_mm_set_s, VSFX) +#define VSHU VL_XCAT(_mm_shuffle_p, VSFX) +#define VNEQ VL_XCAT(_mm_cmpneq_p, VSFX) +#define VAND VL_XCAT(_mm_and_p, VSFX) +#define VANDN VL_XCAT(_mm_andnot_p, VSFX) +#define VST2 VL_XCAT(_mm_store_p, VSFX) +#define VST2U VL_XCAT(_mm_storeu_p, VSFX) + +/* __SSE2__ */ +#endif + diff --git a/colmap/include/colmap/lib/VLFeat/generic.h b/colmap/include/colmap/lib/VLFeat/generic.h new file mode 100644 index 0000000000000000000000000000000000000000..4a6296e8c198035042468c6718412a467427f4b9 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/generic.h @@ -0,0 +1,213 @@ +/** @file generic.h + ** @brief Generic (@ref generic) + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2013 Andrea Vedaldi. +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_GENERIC_H +#define VL_GENERIC_H + +#include "host.h" +#include "random.h" + +#include +#include +#include +#include + +/** @brief Library version string */ +#define VL_VERSION_STRING "0.9.20" + +/** @brief Maximum length (in characters) of an error message */ +#define VL_ERR_MSG_LEN 1024 + +/** @name Type identifiers for atomic data types + ** @{ */ + +#define VL_TYPE_FLOAT 1 /**< @c float type */ +#define VL_TYPE_DOUBLE 2 /**< @c double type */ +#define VL_TYPE_INT8 3 /**< @c ::vl_int8 type */ +#define VL_TYPE_UINT8 4 /**< @c ::vl_uint8 type */ +#define VL_TYPE_INT16 5 /**< @c ::vl_int16 type */ +#define VL_TYPE_UINT16 6 /**< @c ::vl_uint16 type */ +#define VL_TYPE_INT32 7 /**< @c ::vl_int32 type */ +#define VL_TYPE_UINT32 8 /**< @c ::vl_uint32 type */ +#define VL_TYPE_INT64 9 /**< @c ::vl_int64 type */ +#define VL_TYPE_UINT64 10 /**< @c ::vl_uint64 type */ + +typedef vl_uint32 vl_type ; + +/** @brief Get the name of a data type. + ** @param type data type. + ** @return data name of the data type. + ** + ** @c type is one of ::VL_TYPE_FLOAT, ::VL_TYPE_DOUBLE, + ** ::VL_TYPE_INT8, ::VL_TYPE_INT16, ::VL_TYPE_INT32, ::VL_TYPE_INT64, + ** ::VL_TYPE_UINT8, ::VL_TYPE_UINT16, ::VL_TYPE_UINT32, ::VL_TYPE_UINT64. + **/ + +void vl_constructor(); +void vl_destructor(); + +VL_INLINE char const * +vl_get_type_name (vl_type type) +{ + switch (type) { + case VL_TYPE_FLOAT : return "float" ; + case VL_TYPE_DOUBLE : return "double" ; + case VL_TYPE_INT8 : return "int8" ; + case VL_TYPE_INT16 : return "int16" ; + case VL_TYPE_INT32 : return "int32" ; + case VL_TYPE_INT64 : return "int64" ; + case VL_TYPE_UINT8 : return "int8" ; + case VL_TYPE_UINT16 : return "int16" ; + case VL_TYPE_UINT32 : return "int32" ; + case VL_TYPE_UINT64 : return "int64" ; + default: return NULL ; + } +} + +/** @brief Get data type size. + ** @param type data type. + ** @return size (in byte) + ** + ** @c type is one of ::VL_TYPE_FLOAT, ::VL_TYPE_DOUBLE, + ** ::VL_TYPE_INT8, ::VL_TYPE_INT16, ::VL_TYPE_INT32, ::VL_TYPE_INT64, + ** ::VL_TYPE_UINT8, ::VL_TYPE_UINT16, ::VL_TYPE_UINT32, ::VL_TYPE_UINT64. + **/ + +VL_INLINE vl_size +vl_get_type_size (vl_type type) +{ + vl_size dataSize = 0 ; + switch (type) { + case VL_TYPE_DOUBLE : dataSize = sizeof(double) ; break ; + case VL_TYPE_FLOAT : dataSize = sizeof(float) ; break ; + case VL_TYPE_INT64 : case VL_TYPE_UINT64 : dataSize = sizeof(vl_int64) ; break ; + case VL_TYPE_INT32 : case VL_TYPE_UINT32 : dataSize = sizeof(vl_int32) ; break ; + case VL_TYPE_INT16 : case VL_TYPE_UINT16 : dataSize = sizeof(vl_int16) ; break ; + case VL_TYPE_INT8 : case VL_TYPE_UINT8 : dataSize = sizeof(vl_int8) ; break ; + default: + abort() ; + } + return dataSize ; +} +/** @} */ + +VL_EXPORT char const * vl_get_version_string (void) ; +VL_EXPORT char * vl_configuration_to_string_copy (void) ; +VL_EXPORT void vl_set_simd_enabled (vl_bool x) ; +VL_EXPORT vl_bool vl_get_simd_enabled (void) ; +VL_EXPORT vl_bool vl_cpu_has_avx (void) ; +VL_EXPORT vl_bool vl_cpu_has_sse3 (void) ; +VL_EXPORT vl_bool vl_cpu_has_sse2 (void) ; +VL_EXPORT vl_size vl_get_num_cpus (void) ; +VL_EXPORT VlRand * vl_get_rand (void) ; + +/** @name Multi-thread computations + ** @{ */ +VL_EXPORT vl_size vl_get_max_threads (void) ; +VL_EXPORT void vl_set_num_threads (vl_size n) ; +VL_EXPORT vl_size vl_get_thread_limit (void) ; +/** @} (*/ + +/** ------------------------------------------------------------------ + ** @name Error handling + ** @{ */ +#define VL_ERR_OK 0 /**< No error */ +#define VL_ERR_OVERFLOW 1 /**< Buffer overflow error */ +#define VL_ERR_ALLOC 2 /**< Resource allocation error */ +#define VL_ERR_BAD_ARG 3 /**< Bad argument or illegal data error */ +#define VL_ERR_IO 4 /**< Input/output error */ +#define VL_ERR_EOF 5 /**< End-of-file or end-of-sequence error */ +#define VL_ERR_NO_MORE 5 /**< End-of-sequence @deprecated */ + +VL_EXPORT int vl_get_last_error (void) ; +VL_EXPORT char const * vl_get_last_error_message (void) ; +VL_EXPORT int vl_set_last_error (int error, char const * errorMessage, ...) ; +/** @} */ + +/** ------------------------------------------------------------------ + ** @name Memory allocation + ** @{ */ +VL_EXPORT void +vl_set_alloc_func (void *(*malloc_func) (size_t), + void *(*realloc_func) (void*,size_t), + void *(*calloc_func) (size_t, size_t), + void (*free_func) (void*)) ; +VL_EXPORT void *vl_malloc (size_t n) ; +VL_EXPORT void *vl_realloc (void *ptr, size_t n) ; +VL_EXPORT void *vl_calloc (size_t n, size_t size) ; +VL_EXPORT void *vl_memalign (size_t n, size_t size) ; +VL_EXPORT void vl_free (void* ptr) ; +/** @} */ + +/** ------------------------------------------------------------------ + ** @name Logging + ** @{ */ +/** @brief Customizable printf function pointer type */ +typedef int(*printf_func_t) (char const *format, ...) ; +VL_EXPORT void vl_set_printf_func (printf_func_t printf_func) ; +VL_EXPORT printf_func_t vl_get_printf_func (void) ; + +/** @def VL_PRINTF + ** @brief Call user-customizable @c printf function + ** + ** The function calls the user customizable @c printf. + **/ + +/** @def VL_PRINT + ** @brief Same as ::VL_PRINTF (legacy code) + **/ + +#define VL_PRINTF (*vl_get_printf_func()) +#define VL_PRINT (*vl_get_printf_func()) +/** @} */ + +/** ------------------------------------------------------------------ + ** @name Common operations + ** @{ */ + +/** @brief Compute the minimum between two values + ** @param x value + ** @param y value + ** @return the minimum of @a x and @a y. + **/ +#define VL_MIN(x,y) (((x)<(y))?(x):(y)) + +/** @brief Compute the maximum between two values + ** @param x value. + ** @param y value. + ** @return the maximum of @a x and @a y. + **/ +#define VL_MAX(x,y) (((x)>(y))?(x):(y)) + +/** @brief Signed left shift operation + ** @param x value. + ** @param n number of shift positions. + ** @return @c x << n . + ** The macro is equivalent to the builtin @c << operator, but it + ** supports negative shifts too. + **/ +#define VL_SHIFT_LEFT(x,n) (((n)>=0)?((x)<<(n)):((x)>>-(n))) +/* @} */ + +/** ------------------------------------------------------------------ + ** @name Measuring time + ** @{ + **/ +VL_EXPORT void vl_tic (void) ; +VL_EXPORT double vl_toc (void) ; +VL_EXPORT double vl_get_cpu_time (void) ; +/** @} */ + +/* VL_GENERIC_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/getopt_long.h b/colmap/include/colmap/lib/VLFeat/getopt_long.h new file mode 100644 index 0000000000000000000000000000000000000000..421213546357929bd04cb3a6aee2c6cf3d7b07a2 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/getopt_long.h @@ -0,0 +1,43 @@ +/** @file getopt_long.h + ** @brief getopt_long + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_GETOPT_LONG_H +#define VL_GETOPT_LONG_H + +#include "generic.h" + +VL_EXPORT int opterr ; /**< code of the last error occured while parsing an option */ +VL_EXPORT int optind ; /**< index of the next option to process in @c argv */ +VL_EXPORT int optopt ; /**< current option */ +VL_EXPORT char * optarg ; /**< argument of the current option */ +VL_EXPORT int optreset ; /**< reset flag */ + +/** @brief ::getopt_long option */ +struct option +{ + const char *name ; /**< option long name */ + int has_arg ; /**< flag indicating whether the option has no, required or optional argument */ + int *flag ; /**< pointer to a variable to set (if @c NULL, the value is returned instead) */ + int val ; /**< value to set or to return */ +} ; + +#define no_argument 0 /**< ::option with no argument */ +#define required_argument 1 /**< ::option with required argument */ +#define optional_argument 2 /**< ::option with optional argument */ + +VL_EXPORT int getopt_long(int argc, char * const argv[], + const char * optstring, + const struct option * longopts, int * longindex); + +/* VL_GETOPT_LONG_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/gmm.h b/colmap/include/colmap/lib/VLFeat/gmm.h new file mode 100644 index 0000000000000000000000000000000000000000..3562d74c220853c2e3d749233ddb7fc989691f3d --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/gmm.h @@ -0,0 +1,149 @@ +/** @file gmm.h + ** @brief GMM (@ref gmm) + ** @author David Novotny + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2013 David Novotny and Andrea Vedaldi. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_GMM_H +#define VL_GMM_H + +#include "kmeans.h" + +/** @brief GMM initialization algorithms */ +typedef enum _VlGMMInitialization +{ + VlGMMKMeans, /**< Initialize GMM from KMeans clustering. */ + VlGMMRand, /**< Initialize GMM parameters by selecting points at random. */ + VlGMMCustom /**< User specifies the initial GMM parameters. */ +} VlGMMInitialization ; + + +#ifndef __DOXYGEN__ +struct _VlGMM ; +typedef struct _VlGMM VlGMM ; +#else +/** @brief GMM quantizer */ +typedef OPAQUE VlGMM ; +#endif + +/** @name Create and destroy + ** @{ + **/ +VL_EXPORT VlGMM * vl_gmm_new (vl_type dataType, vl_size dimension, vl_size numComponents) ; +VL_EXPORT VlGMM * vl_gmm_new_copy (VlGMM const * gmm) ; +VL_EXPORT void vl_gmm_delete (VlGMM * self) ; +VL_EXPORT void vl_gmm_reset (VlGMM * self); +/** @} */ + +/** @name Basic data processing + ** @{ + **/ +VL_EXPORT double +vl_gmm_cluster +(VlGMM * self, + void const * data, + vl_size numData); +/** @} */ + +/** @name Fine grained data processing + ** @{ */ + +VL_EXPORT void +vl_gmm_init_with_rand_data +(VlGMM * self, + void const * data, + vl_size numData) ; + +VL_EXPORT void +vl_gmm_init_with_kmeans +(VlGMM * self, + void const * data, + vl_size numData, + VlKMeans * kmeansInit); + +VL_EXPORT double +vl_gmm_em +(VlGMM * self, + void const * data, + vl_size numData); +/** @} */ + +VL_EXPORT void +vl_gmm_set_means +(VlGMM * self, + void const * means); + +VL_EXPORT void +vl_gmm_set_covariances +(VlGMM * self, + void const * covariances); + +VL_EXPORT void +vl_gmm_set_priors +(VlGMM * self, + void const * priors); + +VL_EXPORT double +vl_get_gmm_data_posteriors_f(float * posteriors, + vl_size numClusters, + vl_size numData, + float const * priors, + float const * means, + vl_size dimension, + float const * covariances, + float const * data) ; + +VL_EXPORT double +vl_get_gmm_data_posteriors_d(double * posteriors, + vl_size numClusters, + vl_size numData, + double const * priors, + double const * means, + vl_size dimension, + double const * covariances, + double const * data) ; +/** @} */ + +/** @name Set parameters + ** @{ + **/ +VL_EXPORT void vl_gmm_set_num_repetitions (VlGMM * self, vl_size numRepetitions) ; +VL_EXPORT void vl_gmm_set_max_num_iterations (VlGMM * self, vl_size maxNumIterations) ; +VL_EXPORT void vl_gmm_set_verbosity (VlGMM * self, int verbosity) ; +VL_EXPORT void vl_gmm_set_initialization (VlGMM * self, VlGMMInitialization init); +VL_EXPORT void vl_gmm_set_kmeans_init_object (VlGMM * self, VlKMeans * kmeans); +VL_EXPORT void vl_gmm_set_covariance_lower_bounds (VlGMM * self, double const * bounds); +VL_EXPORT void vl_gmm_set_covariance_lower_bound (VlGMM * self, double bound) ; +/** @} */ + +/** @name Get parameters + ** @{ + **/ +VL_EXPORT void const * vl_gmm_get_means (VlGMM const * self); +VL_EXPORT void const * vl_gmm_get_covariances (VlGMM const * self); +VL_EXPORT void const * vl_gmm_get_priors (VlGMM const * self); +VL_EXPORT void const * vl_gmm_get_posteriors (VlGMM const * self); +VL_EXPORT vl_type vl_gmm_get_data_type (VlGMM const * self); +VL_EXPORT vl_size vl_gmm_get_dimension (VlGMM const * self); +VL_EXPORT vl_size vl_gmm_get_num_repetitions (VlGMM const * self); +VL_EXPORT vl_size vl_gmm_get_num_data (VlGMM const * self); +VL_EXPORT vl_size vl_gmm_get_num_clusters (VlGMM const * self); +VL_EXPORT double vl_gmm_get_loglikelihood (VlGMM const * self); +VL_EXPORT int vl_gmm_get_verbosity (VlGMM const * self); +VL_EXPORT vl_size vl_gmm_get_max_num_iterations (VlGMM const * self); +VL_EXPORT vl_size vl_gmm_get_num_repetitions (VlGMM const * self); +VL_EXPORT VlGMMInitialization vl_gmm_get_initialization (VlGMM const * self); +VL_EXPORT VlKMeans * vl_gmm_get_kmeans_init_object (VlGMM const * self); +VL_EXPORT double const * vl_gmm_get_covariance_lower_bounds (VlGMM const * self); +/** @} */ + +/* VL_GMM_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/heap-def.h b/colmap/include/colmap/lib/VLFeat/heap-def.h new file mode 100644 index 0000000000000000000000000000000000000000..bb56c6ceb0ea35a1df64aabfba1a44d75f7a1ca3 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/heap-def.h @@ -0,0 +1,464 @@ +/** @file heap-def.h + ** @brief Heap preprocessor metaprogram + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +/** @file heap-def.h + + A heap organizes an array of objects in a priority queue. This module + is a template metaprogram that defines heap operations on array of + generic objects, or even generic object containers. + + - @ref heap-def-overview "Overview" + - @ref heap-def-overview-general "General usage" + - @ref heap-def-tech "Technical details" + + + @section heap-def-overview Overview + + + To use @ref heap-def.h one must specify at least a prefix and the data + type for the heap elements: + + @code + #define VL_HEAP_prefix my_heap + #define VL_HEAP_type float + #include + @endcode + + This code fragment defines a number of functions prefixed by + ::VL_HEAP_prefix, such as @c my_heap_push (::VL_HEAP_push) and @c + my_heap_pop (::VL_HEAP_pop), that implement the heap operations. + These functions operate on an array that has type ::VL_HEAP_array. + By default, this is defined to be: + + @code + #define VL_HEAP_array VL_HEAP_type* + #define VL_HEAP_array_const VL_HEAP_type const* + @endcode + + The array itself is accessed uniquely by means of two functions: + + - ::VL_HEAP_cmp, that compares two array elements. The default + implementation assumes that ::VL_HEAP_type is numeric. + - ::VL_HEAP_swap, that swaps two array elements. The default + implementation assumes that ::VL_HEAP_type can be copied by the @c + = operator. + + The heap state is a integer @c numElements (of type ::vl_size) counting + the number of elements of the array that are currently part of the heap + and the content of the first @c numElements elements of the array. The + portion of the array that constitutes the heap satisfies a certain + invariant property (heap property, @ref heap-def-tech). From a user + viewpoint, the most important consequence is that the first element + of the array (the one of index 0) is also the smallest (according to + ::VL_HEAP_cmp). + + Elements are added to the heap by ::VL_HEAP_push and removed from the + heap by ::VL_HEAP_pop. A push operation adds to the heap the array + element immediately after the last element already in the heap + (i.e. the element of index @c numElements) and increases the number of + heap elements @c numElements. Elements in the heap are swapped as required in + order to maintain the heap consistency. Similarly, a pop operation + removes the first (smaller) element from the heap and decreases the + number of heap elements @c numElements. + + The values of nodes currently in the heap can be updated by + ::VL_HEAP_update. Notice however that using this function requires + knowing the index of the element that needs to be updated up to the + swapping operations that the heap performs to maintain + consistency. Typically, this requires redefining ::VL_HEAP_swap to + keep track of such changes (@ref heap-def-overview-general). + + + @subsection heap-def-overview-general General usage + + + The heap container may be mapped to any type by reimplementing + ::VL_HEAP_cmp and ::VL_HEAP_swap explicitly. For instance + the following code redefines ::VL_HEAP_cmp to deal with the case + in which the heap is an array of structures: + + @code + typedef struct _S { int x ; } S ; + int s_cmp (S const * v, vl_uindex a, vl_uindex b) { + return v[a].x - v[b].x ; + } + #define VL_HEAP_prefix s_heap + #define VL_HEAP_type S + #define VL_HEAP_cmp s_cmp + #include + @endcode + + In the following example, the heap itself is an arbitrary structure: + + @code + typedef struct _H { int* array ; } H ; + int h_cmp (H const * h, vl_uindex a, vl_uindex b) { + return h->array[a] - h->array[b] ; + } + int h_swap (H * h, vl_uindex a, vl_uindex b) { + int t = h->array[a] ; + h->array[a] = h->array[b] ; + h->array[b] = t ; + } + #define VL_HEAP_prefix h_heap + #define VL_HEAP_swap h_swap + #define VL_HEAP_cmp h_cmp + #include + @endcode + + + @section heap-def-tech Technical details + + + The heap is organised as a binary tree with the property (heap + property) that any node is not larger than any of its + children. In particular, the root is the smallest node. + + @ref heap-def.h uses the standard binary tree representation as a linear + array. Tree nodes are mapped to array elements as follows: + array[0] corresponds to the root, array[1] + and array[2] to the root left and right children and so + on. In this way, the tree structure is fully specified by the total + number of nodes N. + + Assuming that the heap has N nodes (from + array[0] to array[N-1]), adding the node + array[N] to the heap is done by a push down + operation: if the node array[N] is smaller than its + parent (violating the heap property) it is pushed down by swapping it + with the parent, and so on recursively. + + Removing the smallest element array[0] with an heap of + N nodes is done by swapping array[0] with + array[N-1]. If then array[0] is larger than + any of its children, it is swapped with the smallest of the two and + so on recursively (push up operation). + + Restoring the heap property after an element array[i] + has been modified can be done by a push up or push down operation on + that node. + + **/ + +#include "host.h" +#include + +#ifndef VL_HEAP_prefix +#error "VL_HEAP_prefix must be defined" +#endif + +#ifndef VL_HEAP_array +#ifndef VL_HEAP_type +#error "VL_HEAP_type must be defined if VL_HEAP_array is not" +#endif +#define VL_HEAP_array VL_HEAP_type* +#define VL_HEAP_array_const VL_HEAP_type const* +#endif + +#ifndef VL_HEAP_array_const +#define VL_HEAP_array_const VL_HEAP_array +#endif + +#ifdef __DOXYGEN__ +#define VL_HEAP_prefix HeapObject /**< Prefix of the heap functions */ +#define VL_HEAP_type HeapType /**< Data type of the heap elements */ +#define VL_HEAP_array HeapType* /**< Data type of the heap container */ +#define VL_HEAP_array HeapType const* /**< Const data type of the heap container */ +#endif + +/* ---------------------------------------------------------------- */ + +#ifndef VL_HEAP_DEF_H +#define VL_HEAP_DEF_H + +/** @internal @brief Get index of parent node + ** @param index a node index. + ** @return index of the parent node. + **/ + +VL_INLINE vl_uindex +vl_heap_parent (vl_uindex index) +{ + if (index == 0) return 0 ; + return (index - 1) / 2 ; +} + +/** @internal @brief Get index of left child + ** @param index a node index. + ** @return index of the left child. + **/ + +VL_INLINE vl_uindex +vl_heap_left_child (vl_uindex index) +{ + return 2 * index + 1 ; +} + +/** @internal @brief Get index of right child + ** @param index a node index. + ** @return index of the right child. + **/ + +VL_INLINE vl_uindex +vl_heap_right_child (vl_uindex index) +{ + return vl_heap_left_child (index) + 1 ; +} + +/* VL_HEAP_DEF_H */ +#endif + +/* ---------------------------------------------------------------- */ + +#if ! defined(VL_HEAP_cmp) || defined(__DOXYGEN__) +#define VL_HEAP_cmp VL_XCAT(VL_HEAP_prefix, _cmp) + +/** @brief Compare two heap elements + ** @param array heap array. + ** @param indexA index of the first element @c A to compare. + ** @param indexB index of the second element @c B to comapre. + ** @return a negative number if @c AB. + **/ + +VL_INLINE VL_HEAP_type +VL_HEAP_cmp +(VL_HEAP_array_const array, + vl_uindex indexA, + vl_uindex indexB) +{ + return array[indexA] - array[indexB] ; +} + +/* VL_HEAP_cmp */ +#endif + +/* ---------------------------------------------------------------- */ + +#if ! defined(VL_HEAP_swap) || defined(__DOXYGEN__) +#define VL_HEAP_swap VL_XCAT(VL_HEAP_prefix, _swap) + +/** @brief Swap two heap elements + ** @param array array of nodes. + ** @param array heap array. + ** @param indexA index of the first node to swap. + ** @param indexB index of the second node to swap. + ** + ** The function swaps the two heap elements @a a and @ b. The function + ** uses a temporary element and the copy operator, which must be + ** well defined for the heap elements. + **/ + +VL_INLINE void +VL_HEAP_swap +(VL_HEAP_array array, + vl_uindex indexA, + vl_uindex indexB) +{ + VL_HEAP_type t = array [indexA] ; + array [indexA] = array [indexB] ; + array [indexB] = t ; +} + +/* VL_HEAP_swap */ +#endif + +/* ---------------------------------------------------------------- */ + +#if ! defined(VL_HEAP_up) || defined(__DOXYGEN__) +#define VL_HEAP_up VL_XCAT(VL_HEAP_prefix, _up) + +/** @brief Heap up operation + ** @param array pointer to the heap array. + ** @param heapSize size of the heap. + ** @param index index of the node to push up. + **/ + +VL_INLINE void +VL_HEAP_up +(VL_HEAP_array array, vl_size heapSize, vl_uindex index) +{ + vl_uindex leftIndex = vl_heap_left_child (index) ; + vl_uindex rightIndex = vl_heap_right_child (index) ; + + /* no childer: stop */ + if (leftIndex >= heapSize) return ; + + /* only left childer: easy */ + if (rightIndex >= heapSize) { + if (VL_HEAP_cmp (array, index, leftIndex) > 0) { + VL_HEAP_swap (array, index, leftIndex) ; + } + return ; + } + + /* both childern */ + { + if (VL_HEAP_cmp (array, leftIndex, rightIndex) < 0) { + /* swap with left */ + if (VL_HEAP_cmp (array, index, leftIndex) > 0) { + VL_HEAP_swap (array, index, leftIndex) ; + VL_HEAP_up (array, heapSize, leftIndex) ; + } + } else { + /* swap with right */ + if (VL_HEAP_cmp (array, index, rightIndex) > 0) { + VL_HEAP_swap (array, index, rightIndex) ; + VL_HEAP_up (array, heapSize, rightIndex) ; + } + } + } +} + +/* VL_HEAP_up */ +#endif + +/* ---------------------------------------------------------------- */ + +#if ! defined(VL_HEAP_down) || defined(__DOXYGEN__) +#define VL_HEAP_down VL_XCAT(VL_HEAP_prefix, _down) + +/** @brief Heap down operation + ** @param array pointer to the heap node array. + ** @param index index of the node to push up. + **/ + +VL_INLINE void +VL_HEAP_down +(VL_HEAP_array array, vl_uindex index) +{ + vl_uindex parentIndex ; + + if (index == 0) return ; + + parentIndex = vl_heap_parent (index) ; + + if (VL_HEAP_cmp (array, index, parentIndex) < 0) { + VL_HEAP_swap (array, index, parentIndex) ; + VL_HEAP_down (array, parentIndex) ; + } +} + +/* VL_HEAP_down */ +#endif + +/* ---------------------------------------------------------------- */ + +#if ! defined(VL_HEAP_push) || defined(__DOXYGEN__) +#define VL_HEAP_push VL_XCAT(VL_HEAP_prefix, _push) + +/** @brief Heap push operation + ** @param array pointer to the heap array. + ** @param heapSize (in/out) size of the heap. + ** + ** The function adds to the heap the element of index @c heapSize + ** and increments @c heapSize. + **/ + +VL_INLINE void +VL_HEAP_push +(VL_HEAP_array array, vl_size *heapSize) +{ + VL_HEAP_down (array, *heapSize) ; + *heapSize += 1 ; +} + +/* VL_HEAP_push */ +#endif + +/* ---------------------------------------------------------------- */ + +#if ! defined(VL_HEAP_pop) || defined(__DOXYGEN__) +#define VL_HEAP_pop VL_XCAT(VL_HEAP_prefix, _pop) + +/** @brief Heap pop operation + ** @param array pointer to the heap array. + ** @param heapSize (in/out) size of the heap. + ** @return index of the popped element. + ** + ** The function extracts from the heap the element of index 0 + ** (the smallest element) and decreases @c heapSize. + ** + ** The element extracted is moved as the first element after + ** the heap end (thus it has index @c heapSize). For convenience, + ** this index is returned by the function. + ** + ** Popping from an empty heap is undefined. + **/ + +VL_INLINE vl_uindex +VL_HEAP_pop +(VL_HEAP_array array, vl_size *heapSize) +{ + assert (*heapSize) ; + + *heapSize -= 1 ; + + VL_HEAP_swap (array, 0, *heapSize) ; + + if (*heapSize > 1) { + VL_HEAP_up (array, *heapSize, 0) ; + } + + return *heapSize ; +} + +/* VL_HEAP_pop */ +#endif + +/* ---------------------------------------------------------------- */ + +#if ! defined(VL_HEAP_update) || defined(__DOXYGEN__) +#define VL_HEAP_update VL_XCAT(VL_HEAP_prefix, _update) + +/** @brief Heap update operation + ** @param array pointer to the heap array. + ** @param heapSize size of the heap. + ** @param index index of the node to update. + ** + ** The function updates the heap to account for a change to the + ** element of index @c index in the heap. + ** + ** Notice that using this + ** function requires knowing the index of the heap index of + ** element that was changed. Since the heap swaps elements in the + ** array, this is in general different from the index that that + ** element had originally. + **/ + +VL_INLINE void +VL_HEAP_update +(VL_HEAP_array array, + vl_size heapSize, + vl_uindex index) +{ + VL_HEAP_up (array, heapSize, index) ; + VL_HEAP_down (array, index) ; +} + +/* VL_HEAP_update */ +#endif + +/* ---------------------------------------------------------------- */ + +#undef VL_HEAP_cmp +#undef VL_HEAP_swap +#undef VL_HEAP_up +#undef VL_HEAP_down +#undef VL_HEAP_push +#undef VL_HEAP_pop +#undef VL_HEAP_update +#undef VL_HEAP_prefix +#undef VL_HEAP_type +#undef VL_HEAP_array +#undef VL_HEAP_array_const diff --git a/colmap/include/colmap/lib/VLFeat/hikmeans.h b/colmap/include/colmap/lib/VLFeat/hikmeans.h new file mode 100644 index 0000000000000000000000000000000000000000..5da4f5ec800f664d06bd6684b02c24db3a6908f0 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/hikmeans.h @@ -0,0 +1,80 @@ +/** @file hikmeans.h + ** @brief Hierarchical Integer K-Means Clustering + ** @author Brian Fulkerson + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_HIKMEANS_H +#define VL_HIKMEANS_H + +#include "generic.h" +#include "ikmeans.h" + +struct _VLHIKMTree ; +struct _VLHIKMNode ; + +/** @brief HIKM tree node + ** + ** The number of children @a K is not bigger than the @a K parameter + ** of the HIKM tree. + **/ +typedef struct _VlHIKMNode +{ + VlIKMFilt *filter ; /**< IKM filter for this node*/ + struct _VlHIKMNode **children ; /**< Node children (if any) */ +} VlHIKMNode ; + +/** @brief HIKM tree */ +typedef struct _VlHIKMTree { + vl_size M ; /**< IKM: data dimensionality */ + vl_size K ; /**< IKM: K */ + vl_size depth ; /**< Depth of the tree */ + vl_size max_niters ; /**< IKM: maximum # of iterations */ + int method ; /**< IKM: method */ + int verb ; /**< Verbosity level */ + VlHIKMNode * root; /**< Tree root node */ +} VlHIKMTree ; + +/** @name Create and destroy + ** @{ + **/ +VL_EXPORT VlHIKMTree *vl_hikm_new (int method) ; +VL_EXPORT void vl_hikm_delete (VlHIKMTree *f) ; +/** @} */ + +/** @name Retrieve data and parameters + ** @{ + **/ +VL_EXPORT vl_size vl_hikm_get_ndims (VlHIKMTree const *f) ; +VL_EXPORT vl_size vl_hikm_get_K (VlHIKMTree const *f) ; +VL_EXPORT vl_size vl_hikm_get_depth (VlHIKMTree const *f) ; +VL_EXPORT int vl_hikm_get_verbosity (VlHIKMTree const *f) ; +VL_EXPORT vl_size vl_hikm_get_max_niters (VlHIKMTree const *f) ; +VL_EXPORT VlHIKMNode const * vl_hikm_get_root (VlHIKMTree const *f) ; +/** @} */ + +/** @name Set parameters + ** @{ + **/ +VL_EXPORT void vl_hikm_set_verbosity (VlHIKMTree *f, int verb) ; +VL_EXPORT void vl_hikm_set_max_niters (VlHIKMTree *f, int max_niters) ; +/** @} */ + +/** @name Process data + ** @{ + **/ +VL_EXPORT void vl_hikm_init (VlHIKMTree *f, vl_size M, vl_size K, vl_size depth) ; +VL_EXPORT void vl_hikm_train (VlHIKMTree *f, vl_uint8 const *data, vl_size N) ; +VL_EXPORT void vl_hikm_push (VlHIKMTree *f, vl_uint32 *asgn, vl_uint8 const *data, vl_size N) ; +/** @} */ + + +/* VL_HIKMEANS_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/hog.h b/colmap/include/colmap/lib/VLFeat/hog.h new file mode 100644 index 0000000000000000000000000000000000000000..0a513d41e9df37fefec3659e7d446518d67b0468 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/hog.h @@ -0,0 +1,89 @@ +/** @file hog.h + ** @brief Histogram of Oriented Gradients (@ref hog) + ** @author Andrea Vedaldi + **/ + +/* + Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. + All rights reserved. + + This file is part of the VLFeat library and is made available under + the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_HOG_H +#define VL_HOG_H + +#include "generic.h" + +enum VlHogVariant_ { VlHogVariantDalalTriggs, VlHogVariantUoctti } ; + +typedef enum VlHogVariant_ VlHogVariant ; + +struct VlHog_ +{ + VlHogVariant variant ; + vl_size dimension ; + vl_size numOrientations ; + vl_bool transposed ; + vl_bool useBilinearOrientationAssigment ; + + /* left-right flip permutation */ + vl_index * permutation ; + + /* glyphs */ + float * glyphs ; + vl_size glyphSize ; + + /* helper vectors */ + float * orientationX ; + float * orientationY ; + + /* buffers */ + float * hog ; + float * hogNorm ; + vl_size hogWidth ; + vl_size hogHeight ; +} ; + +typedef struct VlHog_ VlHog ; + +VL_EXPORT VlHog * vl_hog_new (VlHogVariant variant, vl_size numOrientations, vl_bool transposed) ; +VL_EXPORT void vl_hog_delete (VlHog * self) ; +VL_EXPORT void vl_hog_process (VlHog * self, + float * features, + float const * image, + vl_size width, vl_size height, vl_size numChannels, + vl_size cellSize) ; + +VL_EXPORT void vl_hog_put_image (VlHog * self, + float const * image, + vl_size width, vl_size height, vl_size numChannels, + vl_size cellSize) ; + +VL_EXPORT void vl_hog_put_polar_field (VlHog * self, + float const * modulus, + float const * angle, + vl_bool directed, + vl_size width, vl_size height, vl_size cellSize) ; + +VL_EXPORT void vl_hog_extract (VlHog * self, float * features) ; +VL_EXPORT vl_size vl_hog_get_height (VlHog * self) ; +VL_EXPORT vl_size vl_hog_get_width (VlHog * self) ; + + +VL_EXPORT void vl_hog_render (VlHog const * self, + float * image, + float const * features, + vl_size width, + vl_size height) ; + +VL_EXPORT vl_size vl_hog_get_dimension (VlHog const * self) ; +VL_EXPORT vl_index const * vl_hog_get_permutation (VlHog const * self) ; +VL_EXPORT vl_size vl_hog_get_glyph_size (VlHog const * self) ; + +VL_EXPORT vl_bool vl_hog_get_use_bilinear_orientation_assignments (VlHog const * self) ; +VL_EXPORT void vl_hog_set_use_bilinear_orientation_assignments (VlHog * self, vl_bool x) ; + +/* VL_HOG_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/homkermap.h b/colmap/include/colmap/lib/VLFeat/homkermap.h new file mode 100644 index 0000000000000000000000000000000000000000..e16a7167ea57ff03870f0dfb8582c7f111ea1e77 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/homkermap.h @@ -0,0 +1,86 @@ +/** @file homkermap.h + ** @brief Homogeneous kernel map (@ref homkermap) + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_HOMKERMAP_H +#define VL_HOMKERMAP_H + +#include "generic.h" + +#include + +/** @brief Type of kernel */ +typedef enum { + VlHomogeneousKernelIntersection = 0, /**< intersection kernel */ + VlHomogeneousKernelChi2, /**< Chi2 kernel */ + VlHomogeneousKernelJS /**< Jensen-Shannon kernel */ +} VlHomogeneousKernelType ; + +/** @brief Type of spectral windowing function */ +typedef enum { + VlHomogeneousKernelMapWindowUniform = 0, /**< uniform window */ + VlHomogeneousKernelMapWindowRectangular = 1, /**< rectangular window */ +} VlHomogeneousKernelMapWindowType ; + +#ifndef __DOXYGEN__ +struct _VlHomogeneousKernelMap ; +typedef struct _VlHomogeneousKernelMap VlHomogeneousKernelMap ; +#else +/** @brief Homogeneous kernel map object */ +typedef OPAQUE VlHomogeneousKernelMap ; +#endif + +/** @name Create and destroy + ** @{ */ +VL_EXPORT VlHomogeneousKernelMap * +vl_homogeneouskernelmap_new (VlHomogeneousKernelType kernelType, + double gamma, + vl_size order, + double period, + VlHomogeneousKernelMapWindowType windowType) ; +VL_EXPORT void +vl_homogeneouskernelmap_delete (VlHomogeneousKernelMap * self) ; +/** @} */ + +/** @name Process data + ** @{ */ +VL_EXPORT void +vl_homogeneouskernelmap_evaluate_d (VlHomogeneousKernelMap const * self, + double * destination, + vl_size stride, + double x) ; + +VL_EXPORT void +vl_homogeneouskernelmap_evaluate_f (VlHomogeneousKernelMap const * self, + float * destination, + vl_size stride, + double x) ; +/** @} */ + + +/** @name Retrieve data and parameters + ** @{ */ +VL_EXPORT vl_size +vl_homogeneouskernelmap_get_order (VlHomogeneousKernelMap const * self) ; + +VL_EXPORT vl_size +vl_homogeneouskernelmap_get_dimension (VlHomogeneousKernelMap const * self) ; + +VL_EXPORT VlHomogeneousKernelType +vl_homogeneouskernelmap_get_kernel_type (VlHomogeneousKernelMap const * self) ; + +VL_EXPORT VlHomogeneousKernelMapWindowType +vl_homogeneouskernelmap_get_window_type (VlHomogeneousKernelMap const * self) ; +/** @} */ + +/* VL_HOMKERMAP_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/host.h b/colmap/include/colmap/lib/VLFeat/host.h new file mode 100644 index 0000000000000000000000000000000000000000..64a14378b7429741112e53f052b28feb35e18c1e --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/host.h @@ -0,0 +1,662 @@ +/** @file host.h + ** @brief Host + ** @author Andrea Vedaldi + ** @sa @ref portability + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_HOST_H +#define VL_HOST_H + +/** ------------------------------------------------------------------ + ** @name Configuration options + ** @{ */ + +#if defined(__DOXYGEN__) +#define VL_DISABLE_THREADS +#define VL_DISABLE_SSE2 +#define VL_DISABLE_OPENMP +#endif + +/** @} */ + +/** ------------------------------------------------------------------ + ** @name Defining functions + ** @{ */ + +#if defined(__DOXYGEN__) +#define VL_EXPORT +#define VL_INLINE +#endif + +/** @} */ + +/** ------------------------------------------------------------------ + ** @name C preprocessor helper macros + ** @{ */ + +/** @brief Convert the argument to a string + ** @param x value to be stringified. + ** + ** This macro stringifies the argument @a x by means of the + ** # prerpocessor operator. + ** + ** The standard C preprocessor does not prescan arguments which are + ** stringified, so + ** + ** @code + ** #define A B + ** char const * str = VL_STRINGIFY(A) ; + ** @endcode + ** + ** initializes str with a pointer to the string + ** "A", which mihgt be unexpected. To fix this issue, + ** you can use ::VL_XSTRINGIFY. + ** + ** @sa ::VL_XSTRINGIFY + **/ + +#define VL_STRINGIFY(x) # x + +/** @brief Expand and then convert the argument to a string + ** @param x value to be macro-expanded and converted. + ** + ** This macro macro-expands the argument @a x and stringifies the + ** result of the expansion. For instance + ** + ** @code + ** #define A B + ** char const * str = VL_STRINGIFY(A) ; + ** @endcode + ** + ** initializes str with a pointer to the string + ** "B". + ** + ** @sa ::VL_STRINGIFY + **/ + +#define VL_XSTRINGIFY(x) VL_STRINGIFY(x) + +/** @brief Concatenate two arguments into a lexical unit + ** @param x first argument to be concatenated. + ** @param y second argument to be concatenated. + ** + ** This macro concatenates its arguments into a single lexical unit + ** by means of the ## preprocessor operator. Notice that + ** arguments concatenated by ## are not pre-expanded by + ** the C preprocessor. To macro-expand the arguments and then + ** concatenate them,use ::VL_XCAT. + ** + ** @see ::VL_XCAT + **/ + +#define VL_CAT(x,y) x ## y + +/** @brief Expand and then concatenate two arguments into a lexical unit + ** @param x first argument to be concatenated. + ** @param y second argument to be concatenated. + ** + ** This macro is the same as ::VL_CAT, except that the arguments are + ** macro expanded before being concatenated. + ** + ** @see ::VL_CAT + **/ + +#define VL_XCAT(x,y) VL_CAT(x,y) + +/** @brief Expand and then concatenate three arguments into a lexical unit + ** @param x first argument to be concatenated. + ** @param y second argument to be concatenated. + ** @param z third argument to be concatenated. + ** + ** This macro is the same as ::VL_XCAT, except that it has three arguments. + ** + ** @see ::VL_XCAT + **/ + +#define VL_XCAT3(x,y,z) VL_XCAT(VL_XCAT(x,y),z) + +/** @brief Expand and then concatenate four arguments into a lexical unit + ** @param x first argument to be concatenated. + ** @param y second argument to be concatenated. + ** @param z third argument to be concatenated. + ** @param u fourth argument to be concatenated. + ** + ** This macro is the same as ::VL_XCAT, except that it has four arguments. + ** + ** @see ::VL_XCAT + **/ + +#define VL_XCAT4(x,y,z,u) VL_XCAT(VL_XCAT3(x,y,z),u) + +/** @brief Expand and then concatenate five arguments into a lexical unit + ** @param x first argument to be concatenated. + ** @param y second argument to be concatenated. + ** @param z third argument to be concatenated. + ** @param u fourth argument to be concatenated. + ** @param v fifth argument to be concatenated. + ** + ** This macro is the same as ::VL_XCAT, except that it has five arguments. + ** + ** @see ::VL_XCAT + **/ + +#define VL_XCAT5(x,y,z,u,v) VL_XCAT(VL_XCAT4(x,y,z,u),v) + +/** @brief Convert a boolean to "yes" or "no" strings + ** @param x boolean to convert. + ** + ** A pointer to either the string "yes" (if @a x is true) + ** or the string "no". + ** + ** @par Example + ** @code + ** VL_PRINTF("Is x true? %s.", VL_YESNO(x)) + ** @endcode + **/ + +#define VL_YESNO(x) ((x)?"yes":"no") + +/** @} */ + +/* + The following macros identify the host OS, architecture and compiler. + They are derived from http://predef.sourceforge.net/ + */ + +/** @name Identifying the host operating system + ** @{ */ +#if defined(linux) || \ + defined(__linux) || \ + defined(__linux__) || \ + defined(__DOXYGEN__) +#define VL_OS_LINUX 1 +#endif + +#if (defined(__APPLE__) & defined(__MACH__)) || \ + defined(__DOXYGEN__) +#define VL_OS_MACOSX 1 +#endif + +#if defined(__WIN32__) || \ + defined(_WIN32) || \ + defined(__DOXYGEN__) +#define VL_OS_WIN 1 +#endif + +#if defined(_WIN64) || \ + defined(__DOXYGEN__) +#define VL_OS_WIN64 1 +#endif +/** @} */ + +/** @name Identifying the host threading library + ** @{ */ +#if !defined(VL_OS_WIN) && !defined(VL_OS_WIN64) || \ +defined(__DOXYGEN__) +#define VL_THREADS_POSIX 1 +#endif + +#if defined(VL_OS_WIN) || defined(VL_OS_WIN64) || \ +defined(__DOXYGEN__) +#define VL_THREADS_WIN 1 +#endif +/** @} */ + +/** @name Identifying the host compiler + ** @{ */ +#if defined(__GNUC__) || defined(__DOXYGEN__) +# if defined(__GNUC_PATCHLEVEL__) +# define VL_COMPILER_GNUC (__GNUC__ * 10000 \ ++ __GNUC_MINOR__ * 100 \ ++ __GNUC_PATCHLEVEL__) +# else +# define VL_COMPILER_GNUC (__GNUC__ * 10000 \ ++ __GNUC_MINOR__ * 100) +# endif +#endif + +#if defined(_MSC_VER) || defined(__DOXYGEN__) +#define VL_COMPILER_MSC _MSC_VER +#endif + +#if defined(__LCC__) || defined(__DOXYGEN__) +#warning "LCC support is experimental!" +#define VL_COMPILER_LCC 1 +#endif + +/** @} */ + +/** @name Identifying the host CPU architecture + ** @{ */ +#if defined(i386) || \ + defined(__i386__) || \ + defined(__DOXYGEN__) +#define VL_ARCH_IX86 300 +#elif defined(__i486__) +#define VL_ARCH_IX86 400 +#elif defined(__i586__) +#define VL_ARCH_IX86 500 +#elif defined(__i686__) +#define VL_ARCH_IX86 600 +#elif defined(_M_IX86) +#define VL_ARCH_IX86 _M_IX86 +#endif + +#if defined(_M_X64) || \ + defined(__amd64__) || \ + defined(__amd64) || \ + defined(__x86_64) || \ + defined(__x86_64) +#define VL_ARCH_X64 +#endif + +#if defined(__ia64__) || \ + defined(_IA64) || \ + defined(__IA64) || \ + defined(__ia64) || \ + defined(_M_IA64) || \ + defined(__DOXYGEN__) +#define VL_ARCH_IA64 +#endif +/** @} */ + +/** @name Identifying the host data model + ** @{ */ +#if defined(__LLP64__) || \ + defined(__LLP64) || \ + defined(__LLP64) || \ + (defined(VL_COMPILER_MSC) & defined(VL_OS_WIN64)) || \ + (defined(VL_COMPILER_LCC) & defined(VL_OS_WIN64)) || \ + defined(__DOXYGEN__) +#define VL_COMPILER_LLP64 +#endif + +#if defined(__LP64__) || \ + defined(__LP64) || \ + defined(__LP64) || \ + (defined(VL_OS_MACOSX) & defined(VL_ARCH_IA64)) || \ + defined(__DOXYGEN__) +#define VL_COMPILER_LP64 +#endif + +#if (!defined(VL_COMPILER_LLP64) & !defined(VL_COMPILER_LP64)) || \ + defined(__DOXYGEN__) +#define VL_COMPILER_ILP32 +#endif +/** @} */ + +/** @name Identifying the host endianness + ** @{ */ +#if defined(__LITTLE_ENDIAN__) || \ + defined(VL_ARCH_IX86) || \ + defined(VL_ARCH_IA64) || \ + defined(VL_ARCH_X64) || \ + defined(__DOXYGEN__) +#define VL_ARCH_LITTLE_ENDIAN +#endif + +#if defined(__DOXYGEN__) || \ + !defined(VL_ARCH_LITTLE_ENDIAN) +#define VL_ARCH_BIG_ENDIAN +#endif +/** @} */ + +#if defined(VL_COMPILER_MSC) & ! defined(__DOXYGEN__) +# define VL_UNUSED +# define VL_INLINE static __inline +#if defined _MSC_VER && _MSC_VER < 1900 +# define snprintf _snprintf +#endif +# define isnan _isnan +# ifdef VL_BUILD_DLL +# ifdef __cplusplus +# define VL_EXPORT extern "C" __declspec(dllexport) +# else +# define VL_EXPORT extern __declspec(dllexport) +# endif +# else +# ifdef __cplusplus +# define VL_EXPORT extern "C" +# else +# define VL_EXPORT extern +# endif +# endif +#endif + +#if defined(VL_COMPILER_LCC) & ! defined(__DOXYGEN__) +# define VL_UNUSED +# define VL_INLINE static __inline +# define snprintf _snprintf +# define isnan _isnan +VL_INLINE float fabsf(float x) { return (float) fabs((double) x) ; } +# ifdef VL_BUILD_DLL +# define VL_EXPORT extern __declspec(dllexport) +# else +# define VL_EXPORT extern +# endif +#endif + +#if defined(VL_COMPILER_GNUC) & ! defined(__DOXYGEN__) +# define VL_UNUSED __attribute__((unused)) +# define VL_INLINE static __inline__ +# ifdef VL_BUILD_DLL +# ifdef __cplusplus +# define VL_EXPORT __attribute__((visibility ("default"))) extern "C" +# else +# define VL_EXPORT __attribute__((visibility ("default"))) extern +# endif +# else +# ifdef __cplusplus +# define VL_EXPORT extern "C" +# else +# define VL_EXPORT extern +# endif +# endif +#endif + +VL_EXPORT char * vl_static_configuration_to_string_copy () ; + +/** ------------------------------------------------------------------ + ** @name Atomic data types + ** @{ + **/ + +#define VL_TRUE 1 /**< @brief @c true (1) constant */ +#define VL_FALSE 0 /**< @brief @c false (0) constant */ + +#if defined(VL_COMPILER_LP64) || defined(VL_COMPILER_LLP64) +typedef long long vl_int64 ; /**< @brief Signed 64-bit integer. */ +typedef int vl_int32 ; /**< @brief Signed 32-bit integer. */ +typedef short vl_int16 ; /**< @brief Signed 16-bit integer. */ +typedef char vl_int8 ; /**< @brief Signed 8-bit integer. */ + +typedef long long unsigned vl_uint64 ; /**< @brief Unsigned 64-bit integer. */ +typedef int unsigned vl_uint32 ; /**< @brief Unsigned 32-bit integer. */ +typedef short unsigned vl_uint16 ; /**< @brief Unsigned 16-bit integer. */ +typedef char unsigned vl_uint8 ; /**< @brief Unsigned 8-bit integer. */ + +typedef int vl_int ; /**< @brief Same as @c int. */ +typedef unsigned int vl_uint ; /**< @brief Same as unsigned int. */ + +typedef int vl_bool ; /**< @brief Boolean. */ +typedef vl_int64 vl_intptr ; /**< @brief Integer holding a pointer. */ +typedef vl_uint64 vl_uintptr ; /**< @brief Unsigned integer holding a pointer. */ +typedef vl_uint64 vl_size ; /**< @brief Unsigned integer holding the size of a memory block. */ +typedef vl_int64 vl_index ; /**< @brief Signed version of ::vl_size and ::vl_uindex */ +typedef vl_uint64 vl_uindex ; /**< @brief Same as ::vl_size */ +#endif + +#if defined(VL_COMPILER_ILP32) + +#ifdef VL_COMPILER_MSC +typedef __int64 vl_int64 ; +#else +typedef long long vl_int64 ; +#endif + +typedef int vl_int32 ; +typedef short vl_int16 ; +typedef char vl_int8 ; + +#ifdef VL_COMPILER_MSC +typedef __int64 unsigned vl_uint64 ; +#else +typedef long long unsigned vl_uint64 ; +#endif +typedef int unsigned vl_uint32 ; +typedef short unsigned vl_uint16 ; +typedef char unsigned vl_uint8 ; + +typedef int vl_int ; +typedef unsigned int vl_uint ; + +typedef int vl_bool ; +typedef vl_int32 vl_intptr ; +typedef vl_uint32 vl_uintptr ; +typedef vl_uint32 vl_size ; +typedef vl_int32 vl_index ; +typedef vl_uint32 vl_uindex ; +#endif +/** @} */ + +/** @name Creating integer constants + ** @{ */ +#if defined(VL_COMPILER_LP64) || defined(__DOXYGEN__) +#define VL_INT8_C(x) x +#define VL_INT16_C(x) x +#define VL_INT32_C(x) x +#define VL_INT64_C(x) x ## L + +#define VL_UINT8_C(x) x +#define VL_UINT16_C(x) x +#define VL_UINT32_C(x) x ## U +#define VL_UINT64_C(x) x ## UL +#endif + +#if (defined(VL_COMPILER_LLP64) || defined(VL_COMPILER_ILP32)) \ + & !defined(__DOXYGEN__) +#define VL_INT8_C(x) x +#define VL_INT16_C(x) x +#define VL_INT32_C(x) x +#define VL_INT64_C(x) x ## LL + +#define VL_UINT8_C(x) x +#define VL_UINT16_C(x) x +#define VL_UINT32_C(x) x ## U +#define VL_UINT64_C(x) x ## ULL +#endif +/** @} */ + +/** ------------------------------------------------------------------ + ** @name Printing the atomic data types + ** @{ */ + +/* Lengths only: */ + +/** @def VL_FL_INT64 + ** @brief @c printf length flag for ::vl_int64 and ::vl_uint64. + **/ + +/** @def VL_FL_INT32 + ** @brief @c printf length flag for ::vl_int32 and ::vl_uint32. + **/ + +/** @def VL_FL_INT16 + ** @brief @c printf length flag for ::vl_int16 and ::vl_uint16. + **/ + +/** @def VL_FL_INT8 + ** @brief @c printf length flag for ::vl_int8 and ::vl_uint8. + **/ + +/** @def VL_FL_INDEX + ** @brief @c printf length flag for ::vl_index and ::vl_uindex + **/ + +#ifdef VL_COMPILER_MSC +#define VL_FL_INT64 "I64" +#else +#define VL_FL_INT64 "ll" +#endif +#define VL_FL_INT32 "" +#define VL_FL_INT16 "h" +#define VL_FL_INT8 "hh" + +#if defined(VL_COMPILER_LP64) || defined(VL_COMPILER_LLP64) +#define VL_FL_INDEX VL_FL_INT64 +#endif + +#if defined(VL_COMPILER_ILP32) +#define VL_FL_INDEX VL_FL_INT32 +#endif + +/* Formats (but not conversions!): */ + +/** @def VL_FMT_SIZE + ** @brief @c printf flag for ::vl_size + **/ + +/** @def VL_FMT_INDEX + ** @brief @c printf flag for ::vl_index + **/ + +/** @def VL_FMT_UINDEX + ** @brief @c printf flag for ::vl_uindex + **/ + +/** @def VL_FMT_INTPTR + ** @brief @c printf flag for ::vl_intptr + **/ + +/** @def VL_FMT_UINTPTR + ** @brief @c printf flag for ::vl_uintptr + **/ + +#define VL_FMT_INDEX VL_FL_INDEX "d" +#define VL_FMT_INTPTR VL_FMT_INDEX +#define VL_FMT_UINDEX VL_FL_INDEX "u" +#define VL_FMT_SIZE VL_FMT_UINDEX +#define VL_FMT_UINTPTR VL_FMT_UINDEX + +/** @} */ + +/** ------------------------------------------------------------------ + ** @name Atomic data types limits + ** @{ */ + +/** @brief Largest integer (math constant) */ +#define VL_BIG_INT 0x7FFFFFFFL + +/** @brief Smallest integer (math constant) */ +#define VL_SMALL_INT (- VL_BIG_INT - 1) + +/** @brief Largest unsigned integer (math constant) */ +#define VL_BIG_UINT 0xFFFFFFFFUL + +/** @} */ + +/** ------------------------------------------------------------------ + ** @name Endianness detection and conversion + ** @{ + **/ +VL_INLINE void vl_swap_host_big_endianness_8 (void *dst, void* src) ; +VL_INLINE void vl_swap_host_big_endianness_4 (void *dst, void* src) ; +VL_INLINE void vl_swap_host_big_endianness_2 (void *dst, void* src) ; +/** @} */ + +/** ------------------------------------------------------------------ + ** @name Obtaining host info at run time + ** @{ */ + +typedef struct _VlX86CpuInfo +{ + union { + char string [0x20] ; + vl_uint32 words [0x20 / 4] ; + } vendor ; + vl_bool hasAVX ; + vl_bool hasSSE42 ; + vl_bool hasSSE41 ; + vl_bool hasSSE3 ; + vl_bool hasSSE2 ; + vl_bool hasSSE ; + vl_bool hasMMX ; +} VlX86CpuInfo ; + +void _vl_x86cpu_info_init (VlX86CpuInfo *self) ; +char * _vl_x86cpu_info_to_string_copy (VlX86CpuInfo const *self) ; + +/** @} */ + +/** ------------------------------------------------------------------ + ** @brief Host <-> big endian transformation for 8-bytes value + ** + ** @param dst destination 8-byte buffer. + ** @param src source 8-byte bufffer. + ** @see @ref host-arch-endianness. + **/ + +VL_INLINE void +vl_swap_host_big_endianness_8 (void *dst, void* src) +{ + char *dst_ = (char*) dst ; + char *src_ = (char*) src ; +#if defined(VL_ARCH_BIG_ENDIAN) + dst_ [0] = src_ [0] ; + dst_ [1] = src_ [1] ; + dst_ [2] = src_ [2] ; + dst_ [3] = src_ [3] ; + dst_ [4] = src_ [4] ; + dst_ [5] = src_ [5] ; + dst_ [6] = src_ [6] ; + dst_ [7] = src_ [7] ; +#else + dst_ [0] = src_ [7] ; + dst_ [1] = src_ [6] ; + dst_ [2] = src_ [5] ; + dst_ [3] = src_ [4] ; + dst_ [4] = src_ [3] ; + dst_ [5] = src_ [2] ; + dst_ [6] = src_ [1] ; + dst_ [7] = src_ [0] ; +#endif +} + +/** ------------------------------------------------------------------ + ** @brief Host <-> big endian transformation for 4-bytes value + ** + ** @param dst destination 4-byte buffer. + ** @param src source 4-byte bufffer. + ** @sa @ref host-arch-endianness. + **/ + +VL_INLINE void +vl_swap_host_big_endianness_4 (void *dst, void* src) +{ + char *dst_ = (char*) dst ; + char *src_ = (char*) src ; +#if defined(VL_ARCH_BIG_ENDIAN) + dst_ [0] = src_ [0] ; + dst_ [1] = src_ [1] ; + dst_ [2] = src_ [2] ; + dst_ [3] = src_ [3] ; +#else + dst_ [0] = src_ [3] ; + dst_ [1] = src_ [2] ; + dst_ [2] = src_ [1] ; + dst_ [3] = src_ [0] ; +#endif +} + +/** ------------------------------------------------------------------ + ** @brief Host <-> big endian transformation for 2-bytes value + ** + ** @param dst destination 2-byte buffer. + ** @param src source 2-byte bufffer. + ** @see @ref host-arch-endianness. + **/ + +VL_INLINE void +vl_swap_host_big_endianness_2 (void *dst, void* src) +{ + char *dst_ = (char*) dst ; + char *src_ = (char*) src ; +#if defined(VL_ARCH_BIG_ENDIAN) + dst_ [0] = src_ [0] ; + dst_ [1] = src_ [1] ; +#else + dst_ [0] = src_ [1] ; + dst_ [1] = src_ [0] ; +#endif +} + +/* VL_HOST_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/ikmeans.h b/colmap/include/colmap/lib/VLFeat/ikmeans.h new file mode 100644 index 0000000000000000000000000000000000000000..e7990805667b5b8d4a75f5d97fd2df5e8f2cda47 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/ikmeans.h @@ -0,0 +1,87 @@ +/** @file ikmeans.h + ** @brief Integer K-Means clustering + ** @author Brian Fulkerson + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2014 Andrea Vedaldi. +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_IKMEANS_H +#define VL_IKMEANS_H + +#include "generic.h" +#include "random.h" + +#if 0 +typedef vl_int64 vl_ikmacc_t ; /**< IKM accumulator data type */ +#define VL_IKMACC_MAX 0x7fffffffffffffffULL +#else +typedef vl_int32 vl_ikmacc_t ; /**< IKM accumulator data type */ +#define VL_IKMACC_MAX 0x7fffffffUL +#endif + + +/** ------------------------------------------------------------------ + ** @brief IKM algorithms + **/ + +enum VlIKMAlgorithms { + VL_IKM_LLOYD, /**< Lloyd algorithm */ + VL_IKM_ELKAN, /**< Elkan algorithm */ +} ; + +/** ------------------------------------------------------------------ + ** @brief IKM quantizer + **/ + +typedef struct _VlIKMFilt +{ + vl_size M ; /**< data dimensionality */ + vl_size K ; /**< number of centers */ + vl_size max_niters ; /**< Lloyd: maximum number of iterations */ + int method ; /**< Learning method */ + int verb ; /**< verbosity level */ + vl_ikmacc_t *centers ; /**< centers */ + vl_ikmacc_t *inter_dist ; /**< centers inter-distances */ +} VlIKMFilt ; + +/** @name Create and destroy + ** @{ */ +VL_EXPORT VlIKMFilt *vl_ikm_new (int method) ; +VL_EXPORT void vl_ikm_delete (VlIKMFilt *f) ; +/** @} */ + +/** @name Process data + ** @{ */ +VL_EXPORT void vl_ikm_init (VlIKMFilt *f, vl_ikmacc_t const *centers, vl_size M, vl_size K) ; +VL_EXPORT void vl_ikm_init_rand (VlIKMFilt *f, vl_size M, vl_size K) ; +VL_EXPORT void vl_ikm_init_rand_data (VlIKMFilt *f, vl_uint8 const *data, vl_size M, vl_size N, vl_size K) ; +VL_EXPORT int vl_ikm_train (VlIKMFilt *f, vl_uint8 const *data, vl_size N) ; +VL_EXPORT void vl_ikm_push (VlIKMFilt *f, vl_uint32 *asgn, vl_uint8 const *data, vl_size N) ; +VL_EXPORT vl_uint vl_ikm_push_one (vl_ikmacc_t const *centers, vl_uint8 const *data, vl_size M, vl_size K) ; +/** @} */ + +/** @name Retrieve data and parameters + ** @{ */ +VL_EXPORT vl_size vl_ikm_get_ndims (VlIKMFilt const *f) ; +VL_EXPORT vl_size vl_ikm_get_K (VlIKMFilt const *f) ; +VL_EXPORT int vl_ikm_get_verbosity (VlIKMFilt const *f) ; +VL_EXPORT vl_size vl_ikm_get_max_niters (VlIKMFilt const *f) ; +VL_EXPORT vl_ikmacc_t const *vl_ikm_get_centers (VlIKMFilt const *f) ; +/** @} */ + +/** @name Set parameters + ** @{ */ +VL_EXPORT void vl_ikm_set_verbosity (VlIKMFilt *f, int verb) ; +VL_EXPORT void vl_ikm_set_max_niters (VlIKMFilt *f, vl_size max_niters) ; +/** @} */ + +/* VL_IKMEANS_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/imopv.h b/colmap/include/colmap/lib/VLFeat/imopv.h new file mode 100644 index 0000000000000000000000000000000000000000..ca127ca3cefaf3157490149d0a225437a1e23f45 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/imopv.h @@ -0,0 +1,164 @@ +/** @file imopv.h + ** @brief Vectorized image operations + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_IMOPV_H +#define VL_IMOPV_H + +#include "generic.h" + +/** @name Image convolution flags + ** @{ */ +#define VL_PAD_BY_ZERO (0x0 << 0) /**< @brief Pad with zeroes. */ +#define VL_PAD_BY_CONTINUITY (0x1 << 0) /**< @brief Pad by continuity. */ +#define VL_PAD_MASK (0x3) /**< @brief Padding field selector. */ +#define VL_TRANSPOSE (0x1 << 2) /**< @brief Transpose result. */ +/** @} */ + +/** @name Image convolution + ** @{ */ +VL_EXPORT +void vl_imconvcol_vf (float* dst, vl_size dst_stride, + float const* src, + vl_size src_width, vl_size src_height, vl_size src_stride, + float const* filt, vl_index filt_begin, vl_index filt_end, + int step, unsigned int flags) ; + +VL_EXPORT +void vl_imconvcol_vd (double* dst, vl_size dst_stride, + double const* src, + vl_size src_width, vl_size src_height, vl_size src_stride, + double const* filt, vl_index filt_begin, vl_index filt_end, + int step, unsigned int flags) ; + +VL_EXPORT +void vl_imconvcoltri_f (float * dest, vl_size destStride, + float const * image, + vl_size imageWidth, vl_size imageHeight, vl_size imageStride, + vl_size filterSize, + vl_size step, int unsigned flags) ; + +VL_EXPORT +void vl_imconvcoltri_d (double * dest, vl_size destStride, + double const * image, + vl_size imageWidth, vl_size imageHeight, vl_size imageStride, + vl_size filterSize, + vl_size step, int unsigned flags) ; +/** @} */ + +/** @name Integral image + ** @{ */ +VL_EXPORT +void vl_imintegral_f (float * integral, vl_size integralStride, + float const * image, + vl_size imageWidth, vl_size imageHeight, vl_size imageStride) ; + +VL_EXPORT +void vl_imintegral_d (double * integral, vl_size integralStride, + double const * image, + vl_size imageWidth, vl_size imageHeight, vl_size imageStride) ; + +VL_EXPORT +void vl_imintegral_i32 (vl_int32 * integral, vl_size integralStride, + vl_int32 const * image, + vl_size imageWidth, vl_size imageHeight, vl_size imageStride) ; + +VL_EXPORT +void vl_imintegral_ui32 (vl_uint32 * integral, vl_size integralStride, + vl_uint32 const * image, + vl_size imageWidth, vl_size imageHeight, vl_size imageStride) ; +/** @} */ + +/** @name Distance transform */ +/** @{ */ + +VL_EXPORT void +vl_image_distance_transform_d (double const * image, + vl_size numColumns, + vl_size numRows, + vl_size columnStride, + vl_size rowStride, + double * distanceTransform, + vl_uindex * indexes, + double coeff, + double offset) ; + +VL_EXPORT void +vl_image_distance_transform_f (float const * image, + vl_size numColumns, + vl_size numRows, + vl_size columnStride, + vl_size rowStride, + float * distanceTransform, + vl_uindex * indexes, + float coeff, + float offset) ; + +/** @} */ + +/* ---------------------------------------------------------------- */ +/** @name Image smoothing */ +/** @{ */ + +VL_EXPORT void +vl_imsmooth_f (float *smoothed, vl_size smoothedStride, + float const *image, vl_size width, vl_size height, vl_size stride, + double sigmax, double sigmay) ; + +VL_EXPORT void +vl_imsmooth_d (double *smoothed, vl_size smoothedStride, + double const *image, vl_size width, vl_size height, vl_size stride, + double sigmax, double sigmay) ; + +/** @} */ + +/* ---------------------------------------------------------------- */ +/** @name Image gradients */ +/** @{ */ +VL_EXPORT void +vl_imgradient_polar_f (float* amplitudeGradient, float* angleGradient, + vl_size gradWidthStride, vl_size gradHeightStride, + float const* image, + vl_size imageWidth, vl_size imageHeight, + vl_size imageStride); + +VL_EXPORT void +vl_imgradient_polar_d (double* amplitudeGradient, double* angleGradient, + vl_size gradWidthStride, vl_size gradHeightStride, + double const* image, + vl_size imageWidth, vl_size imageHeight, + vl_size imageStride); + +VL_EXPORT void +vl_imgradient_f (float* xGradient, float* yGradient, + vl_size gradWidthStride, vl_size gradHeightStride, + float const *image, + vl_size imageWidth, vl_size imageHeight, vl_size imageStride); + +VL_EXPORT void +vl_imgradient_d(double* xGradient, double* yGradient, + vl_size gradWidthStride, vl_size gradHeightStride, + double const *image, + vl_size imageWidth, vl_size imageHeight, vl_size imageStride); + +VL_EXPORT void +vl_imgradient_polar_f_callback(float const *sourceImage, + int sourceImageWidth, int sourceImageHeight, + float *dstImage, + int dstWidth, int dstHeight, + int octave, int level, + void *params); + +/** @} */ + +/* VL_IMOPV_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/imopv_sse2.h b/colmap/include/colmap/lib/VLFeat/imopv_sse2.h new file mode 100644 index 0000000000000000000000000000000000000000..0f8da374e3d56fd16822fbe3266ff5516e7cc338 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/imopv_sse2.h @@ -0,0 +1,54 @@ +/** @file imopv_sse2.h + ** @brief Vectorized image operations - SSE2 + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_IMOPV_SSE2_H +#define VL_IMOPV_SSE2_H + +#include "generic.h" + +#ifndef VL_DISABLE_SSE2 + +VL_EXPORT +void _vl_imconvcol_vf_sse2 (float* dst, vl_size dst_stride, + float const* src, + vl_size src_width, vl_size src_height, vl_size src_stride, + float const* filt, vl_index filt_begin, vl_index filt_end, + int step, unsigned int flags) ; + +VL_EXPORT +void _vl_imconvcol_vd_sse2 (double* dst, vl_size dst_stride, + double const* src, + vl_size src_width, vl_size src_height, vl_size src_stride, + double const* filt, vl_index filt_begin, vl_index filt_end, + int step, unsigned int flags) ; + +/* +VL_EXPORT +void _vl_imconvcoltri_vf_sse2 (float* dst, int dst_stride, + float const* src, + int src_width, int src_height, int src_stride, + int filt_size, + int step, unsigned int flags) ; + +VL_EXPORT +void _vl_imconvcoltri_vd_sse2 (double* dst, int dst_stride, + double const* src, + int src_width, int src_height, int src_stride, + int filt_size, + int step, unsigned int flags) ; +*/ + +#endif + +/* VL_IMOPV_SSE2_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/kdtree.h b/colmap/include/colmap/lib/VLFeat/kdtree.h new file mode 100644 index 0000000000000000000000000000000000000000..8c8f212be720fcee6b594a03116ab6c6239d6b02 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/kdtree.h @@ -0,0 +1,185 @@ +/** @file kdtree.h + ** @brief KD-tree (@ref kdtree) + ** @author Andrea Vedaldi, David Novotny + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_KDTREE_H +#define VL_KDTREE_H + +#include "generic.h" +#include "mathop.h" + +#define VL_KDTREE_SPLIT_HEAP_SIZE 5 +#define VL_KDTREE_VARIANCE_EST_NUM_SAMPLES 1024 + +typedef struct _VlKDTreeNode VlKDTreeNode ; +typedef struct _VlKDTreeSplitDimension VlKDTreeSplitDimension ; +typedef struct _VlKDTreeDataIndexEntry VlKDTreeDataIndexEntry ; +typedef struct _VlKDForestSearchState VlKDForestSearchState ; + +struct _VlKDTreeNode +{ + vl_uindex parent ; + vl_index lowerChild ; + vl_index upperChild ; + unsigned int splitDimension ; + double splitThreshold ; + double lowerBound ; + double upperBound ; +} ; + +struct _VlKDTreeSplitDimension +{ + unsigned int dimension ; + double mean ; + double variance ; +} ; + +struct _VlKDTreeDataIndexEntry +{ + vl_index index ; + double value ; +} ; + +/** @brief Thresholding method */ +typedef enum _VlKDTreeThresholdingMethod +{ + VL_KDTREE_MEDIAN, + VL_KDTREE_MEAN +} VlKDTreeThresholdingMethod ; + +/** @brief Neighbor of a query point */ +typedef struct _VlKDForestNeighbor { + double distance ; /**< distance to the query point */ + vl_uindex index ; /**< index of the neighbor in the KDTree data */ +} VlKDForestNeighbor ; + +typedef struct _VlKDTree +{ + VlKDTreeNode * nodes ; + vl_size numUsedNodes ; + vl_size numAllocatedNodes ; + VlKDTreeDataIndexEntry * dataIndex ; + unsigned int depth ; +} VlKDTree ; + +struct _VlKDForestSearchState +{ + VlKDTree * tree ; + vl_uindex nodeIndex ; + double distanceLowerBound ; +} ; + +struct _VlKDForestSearcher; + +/** @brief KDForest object */ +typedef struct _VlKDForest +{ + vl_size dimension ; + + /* random number generator */ + VlRand * rand ; + + /* indexed data */ + vl_type dataType ; + void const * data ; + vl_size numData ; + VlVectorComparisonType distance; + void (*distanceFunction)(void) ; + + /* tree structure */ + VlKDTree ** trees ; + vl_size numTrees ; + + /* build */ + VlKDTreeThresholdingMethod thresholdingMethod ; + VlKDTreeSplitDimension splitHeapArray [VL_KDTREE_SPLIT_HEAP_SIZE] ; + vl_size splitHeapNumNodes ; + vl_size splitHeapSize ; + vl_size maxNumNodes; + + /* query */ + vl_size searchMaxNumComparisons ; + vl_size numSearchers; + struct _VlKDForestSearcher * headSearcher ; /* head of the double linked list with searchers */ + +} VlKDForest ; + +/** @brief ::VlKDForest searcher object */ +typedef struct _VlKDForestSearcher +{ + /* maintain a linked list of searchers for later disposal*/ + struct _VlKDForestSearcher * next; + struct _VlKDForestSearcher * previous; + + vl_uindex * searchIdBook ; + VlKDForestSearchState * searchHeapArray ; + VlKDForest * forest; + + vl_size searchNumComparisons; + vl_size searchNumRecursions ; + vl_size searchNumSimplifications ; + + vl_size searchHeapNumNodes ; + vl_uindex searchId ; +} VlKDForestSearcher ; + +/** @name Creating, copying and disposing + ** @{ */ +VL_EXPORT VlKDForest * vl_kdforest_new (vl_type dataType, + vl_size dimension, vl_size numTrees, VlVectorComparisonType normType) ; +VL_EXPORT VlKDForestSearcher * vl_kdforest_new_searcher (VlKDForest * kdforest); +VL_EXPORT void vl_kdforest_delete (VlKDForest * self) ; +VL_EXPORT void vl_kdforestsearcher_delete (VlKDForestSearcher * searcher) ; +/** @} */ + +/** @name Building and querying + ** @{ */ +VL_EXPORT void vl_kdforest_build (VlKDForest * self, + vl_size numData, + void const * data) ; + +VL_EXPORT vl_size vl_kdforest_query (VlKDForest * self, + VlKDForestNeighbor * neighbors, + vl_size numNeighbors, + void const * query) ; + +VL_EXPORT vl_size vl_kdforest_query_with_array (VlKDForest * self, + vl_uint32 * index, + vl_size numNeighbors, + vl_size numQueries, + void * distance, + void const * queries) ; + +VL_EXPORT vl_size vl_kdforestsearcher_query (VlKDForestSearcher * self, + VlKDForestNeighbor * neighbors, + vl_size numNeighbors, + void const * query) ; +/** @} */ + +/** @name Retrieving and setting parameters + ** @{ */ +VL_EXPORT vl_size vl_kdforest_get_depth_of_tree (VlKDForest const * self, vl_uindex treeIndex) ; +VL_EXPORT vl_size vl_kdforest_get_num_nodes_of_tree (VlKDForest const * self, vl_uindex treeIndex) ; +VL_EXPORT vl_size vl_kdforest_get_num_trees (VlKDForest const * self) ; +VL_EXPORT vl_size vl_kdforest_get_data_dimension (VlKDForest const * self) ; +VL_EXPORT vl_type vl_kdforest_get_data_type (VlKDForest const * self) ; +VL_EXPORT void vl_kdforest_set_max_num_comparisons (VlKDForest * self, vl_size n) ; +VL_EXPORT vl_size vl_kdforest_get_max_num_comparisons (VlKDForest * self) ; +VL_EXPORT void vl_kdforest_set_thresholding_method (VlKDForest * self, VlKDTreeThresholdingMethod method) ; +VL_EXPORT VlKDTreeThresholdingMethod vl_kdforest_get_thresholding_method (VlKDForest const * self) ; +VL_EXPORT VlKDForest * vl_kdforest_searcher_get_forest (VlKDForestSearcher const * self) ; +VL_EXPORT VlKDForestSearcher * vl_kdforest_get_searcher (VlKDForest const * self, vl_uindex pos) ; +/** @} */ + + +/* VL_KDTREE_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/kmeans.h b/colmap/include/colmap/lib/VLFeat/kmeans.h new file mode 100644 index 0000000000000000000000000000000000000000..a3cd176314dcb6dfe2479cc32235f67835b0920d --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/kmeans.h @@ -0,0 +1,435 @@ +/** @file kmeans.h + ** @brief K-means (@ref kmeans) + ** @author Andrea Vedaldi + ** @author David Novotny + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +Copyright (C) 2013 Andrea Vedaldi and David Novotny. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_KMEANS_H +#define VL_KMEANS_H + +#include "generic.h" +#include "random.h" +#include "mathop.h" +#include "kdtree.h" + +/* ---------------------------------------------------------------- */ + +/** @brief K-means algorithms */ + +typedef enum _VlKMeansAlgorithm { + VlKMeansLloyd, /**< Lloyd algorithm */ + VlKMeansElkan, /**< Elkan algorithm */ + VlKMeansANN /**< Approximate nearest neighbors */ +} VlKMeansAlgorithm ; + +/** @brief K-means initialization algorithms */ + +typedef enum _VlKMeansInitialization { + VlKMeansRandomSelection, /**< Randomized selection */ + VlKMeansPlusPlus /**< Plus plus raondomized selection */ +} VlKMeansInitialization ; + +/** ------------------------------------------------------------------ + ** @brief K-means quantizer + **/ + +typedef struct _VlKMeans +{ + + vl_type dataType ; /**< Data type. */ + vl_size dimension ; /**< Data dimensionality. */ + vl_size numCenters ; /**< Number of centers. */ + vl_size numTrees ; /**< Number of trees in forest when using ANN-kmeans. */ + vl_size maxNumComparisons ; /**< Maximum number of comparisons when using ANN-kmeans. */ + + VlKMeansInitialization initialization ; /**< Initalization algorithm. */ + VlKMeansAlgorithm algorithm ; /**< Clustring algorithm. */ + VlVectorComparisonType distance ; /**< Distance. */ + vl_size maxNumIterations ; /**< Maximum number of refinement iterations. */ + double minEnergyVariation ; /**< Minimum energy variation. */ + vl_size numRepetitions ; /**< Number of clustering repetitions. */ + int verbosity ; /**< Verbosity level. */ + + void * centers ; /**< Centers */ + void * centerDistances ; /**< Centers inter-distances. */ + + double energy ; /**< Current solution energy. */ + VlFloatVectorComparisonFunction floatVectorComparisonFn ; + VlDoubleVectorComparisonFunction doubleVectorComparisonFn ; +} VlKMeans ; + +/** @name Create and destroy + ** @{ + **/ +VL_EXPORT VlKMeans * vl_kmeans_new (vl_type dataType, VlVectorComparisonType distance) ; +VL_EXPORT VlKMeans * vl_kmeans_new_copy (VlKMeans const * kmeans) ; +VL_EXPORT void vl_kmeans_delete (VlKMeans * self) ; +/** @} */ + +/** @name Basic data processing + ** @{ + **/ +VL_EXPORT void vl_kmeans_reset (VlKMeans * self) ; + +VL_EXPORT double vl_kmeans_cluster (VlKMeans * self, + void const * data, + vl_size dimension, + vl_size numData, + vl_size numCenters) ; + +VL_EXPORT void vl_kmeans_quantize (VlKMeans * self, + vl_uint32 * assignments, + void * distances, + void const * data, + vl_size numData) ; + +VL_EXPORT void vl_kmeans_quantize_ANN (VlKMeans * self, + vl_uint32 * assignments, + void * distances, + void const * data, + vl_size numData, + vl_size iteration ); +/** @} */ + +/** @name Advanced data processing + ** @{ + **/ +VL_EXPORT void vl_kmeans_set_centers (VlKMeans * self, + void const * centers, + vl_size dimension, + vl_size numCenters) ; + +VL_EXPORT void vl_kmeans_init_centers_with_rand_data + (VlKMeans * self, + void const * data, + vl_size dimensions, + vl_size numData, + vl_size numCenters) ; + +VL_EXPORT void vl_kmeans_init_centers_plus_plus + (VlKMeans * self, + void const * data, + vl_size dimensions, + vl_size numData, + vl_size numCenters) ; + +VL_EXPORT double vl_kmeans_refine_centers (VlKMeans * self, + void const * data, + vl_size numData) ; + +/** @} */ + +/** @name Retrieve data and parameters + ** @{ + **/ +VL_INLINE vl_type vl_kmeans_get_data_type (VlKMeans const * self) ; +VL_INLINE VlVectorComparisonType vl_kmeans_get_distance (VlKMeans const * self) ; + +VL_INLINE VlKMeansAlgorithm vl_kmeans_get_algorithm (VlKMeans const * self) ; +VL_INLINE VlKMeansInitialization vl_kmeans_get_initialization (VlKMeans const * self) ; +VL_INLINE vl_size vl_kmeans_get_num_repetitions (VlKMeans const * self) ; + +VL_INLINE vl_size vl_kmeans_get_dimension (VlKMeans const * self) ; +VL_INLINE vl_size vl_kmeans_get_num_centers (VlKMeans const * self) ; + +VL_INLINE int vl_kmeans_get_verbosity (VlKMeans const * self) ; +VL_INLINE vl_size vl_kmeans_get_max_num_iterations (VlKMeans const * self) ; +VL_INLINE double vl_kmeans_get_min_energy_variation (VlKMeans const * self) ; +VL_INLINE vl_size vl_kmeans_get_max_num_comparisons (VlKMeans const * self) ; +VL_INLINE vl_size vl_kmeans_get_num_trees (VlKMeans const * self) ; +VL_INLINE double vl_kmeans_get_energy (VlKMeans const * self) ; +VL_INLINE void const * vl_kmeans_get_centers (VlKMeans const * self) ; +/** @} */ + +/** @name Set parameters + ** @{ + **/ +VL_INLINE void vl_kmeans_set_algorithm (VlKMeans * self, VlKMeansAlgorithm algorithm) ; +VL_INLINE void vl_kmeans_set_initialization (VlKMeans * self, VlKMeansInitialization initialization) ; +VL_INLINE void vl_kmeans_set_num_repetitions (VlKMeans * self, vl_size numRepetitions) ; +VL_INLINE void vl_kmeans_set_max_num_iterations (VlKMeans * self, vl_size maxNumIterations) ; +VL_INLINE void vl_kmeans_set_min_energy_variation (VlKMeans * self, double minEnergyVariation) ; +VL_INLINE void vl_kmeans_set_verbosity (VlKMeans * self, int verbosity) ; +VL_INLINE void vl_kmeans_set_max_num_comparisons (VlKMeans * self, vl_size maxNumComparisons) ; +VL_INLINE void vl_kmeans_set_num_trees (VlKMeans * self, vl_size numTrees) ; +/** @} */ + +/** ------------------------------------------------------------------ + ** @brief Get data type + ** @param self KMeans object instance. + ** @return data type. + **/ + +VL_INLINE vl_type +vl_kmeans_get_data_type (VlKMeans const * self) +{ + return self->dataType ; +} + +/** @brief Get data dimension + ** @param self KMeans object instance. + ** @return data dimension. + **/ + +VL_INLINE vl_size +vl_kmeans_get_dimension (VlKMeans const * self) +{ + return self->dimension ; +} + +/** @brief Get data type + ** @param self KMeans object instance. + ** @return data type. + **/ + +VL_INLINE VlVectorComparisonType +vl_kmeans_get_distance (VlKMeans const * self) +{ + return self->distance ; +} + +/** @brief Get the number of centers (K) + ** @param self KMeans object instance. + ** @return number of centers. + **/ + +VL_INLINE vl_size +vl_kmeans_get_num_centers (VlKMeans const * self) +{ + return self->numCenters ; +} + +/** @brief Get the number energy of the current fit + ** @param self KMeans object instance. + ** @return energy. + **/ + +VL_INLINE double +vl_kmeans_get_energy (VlKMeans const * self) +{ + return self->energy ; +} + +/** ------------------------------------------------------------------ + ** @brief Get verbosity level + ** @param self KMeans object instance. + ** @return verbosity level. + **/ + +VL_INLINE int +vl_kmeans_get_verbosity (VlKMeans const * self) +{ + return self->verbosity ; +} + +/** @brief Set verbosity level + ** @param self KMeans object instance. + ** @param verbosity verbosity level. + **/ + +VL_INLINE void +vl_kmeans_set_verbosity (VlKMeans * self, int verbosity) +{ + self->verbosity = verbosity ; +} + +/** ------------------------------------------------------------------ + ** @brief Get centers + ** @param self KMeans object instance. + ** @return cluster centers. + **/ + +VL_INLINE void const * +vl_kmeans_get_centers (VlKMeans const * self) +{ + return self->centers ; +} + +/** ------------------------------------------------------------------ + ** @brief Get maximum number of iterations + ** @param self KMeans object instance. + ** @return maximum number of iterations. + **/ + +VL_INLINE vl_size +vl_kmeans_get_max_num_iterations (VlKMeans const * self) +{ + return self->maxNumIterations ; +} + +/** @brief Set maximum number of iterations + ** @param self KMeans filter. + ** @param maxNumIterations maximum number of iterations. + **/ + +VL_INLINE void +vl_kmeans_set_max_num_iterations (VlKMeans * self, vl_size maxNumIterations) +{ + self->maxNumIterations = maxNumIterations ; +} + +/** ------------------------------------------------------------------ + ** @brief Get maximum number of repetitions. + ** @param self KMeans object instance. + ** @return current number of repretitions for quantization. + **/ + +VL_INLINE vl_size +vl_kmeans_get_num_repetitions (VlKMeans const * self) +{ + return self->numRepetitions ; +} + +/** @brief Set maximum number of repetitions + ** @param self KMeans object instance. + ** @param numRepetitions maximum number of repetitions. + ** The number of repetitions cannot be smaller than 1. + **/ + +VL_INLINE void +vl_kmeans_set_num_repetitions (VlKMeans * self, + vl_size numRepetitions) +{ + assert (numRepetitions >= 1) ; + self->numRepetitions = numRepetitions ; +} + +/** ------------------------------------------------------------------ + ** @brief Get the minimum relative energy variation for convergence. + ** @param self KMeans object instance. + ** @return minimum energy variation. + **/ + +VL_INLINE double +vl_kmeans_get_min_energy_variation (VlKMeans const * self) +{ + return self->minEnergyVariation ; +} + +/** @brief Set the maximum relative energy variation for convergence. + ** @param self KMeans object instance. + ** @param minEnergyVariation maximum number of repetitions. + ** The variation cannot be negative. + ** + ** The relative energy variation is calculated after the $t$-th update + ** to the parameters as: + ** + ** \[ \epsilon_t = \frac{E_{t-1} - E_t}{E_0 - E_t} \] + ** + ** Note that this quantitiy is non-negative since $E_{t+1} \leq E_t$. + ** Hence, $\epsilon_t$ is the improvement to the energy made in the last + ** iteration compared to the total improvement so far. The algorithm + ** stops if this value is less or equal than @a minEnergyVariation. + ** + ** This test is applied only to the LLoyd and ANN algorithms. + **/ + +VL_INLINE void +vl_kmeans_set_min_energy_variation (VlKMeans * self, + double minEnergyVariation) +{ + assert (minEnergyVariation >= 0) ; + self->minEnergyVariation = minEnergyVariation ; +} + +/** ------------------------------------------------------------------ + ** @brief Get K-means algorithm + ** @param self KMeans object. + ** @return algorithm. + **/ + +VL_INLINE VlKMeansAlgorithm +vl_kmeans_get_algorithm (VlKMeans const * self) +{ + return self->algorithm ; +} + +/** @brief Set K-means algorithm + ** @param self KMeans object. + ** @param algorithm K-means algorithm. + **/ + +VL_INLINE void +vl_kmeans_set_algorithm (VlKMeans * self, VlKMeansAlgorithm algorithm) +{ + self->algorithm = algorithm ; +} + +/** ------------------------------------------------------------------ + ** @brief Get K-means initialization algorithm + ** @param self KMeans object. + ** @return algorithm. + **/ + +VL_INLINE VlKMeansInitialization +vl_kmeans_get_initialization (VlKMeans const * self) +{ + return self->initialization ; +} + +/** @brief Set K-means initialization algorithm + ** @param self KMeans object. + ** @param initialization initialization. + **/ + +VL_INLINE void +vl_kmeans_set_initialization (VlKMeans * self, + VlKMeansInitialization initialization) +{ + self->initialization = initialization ; +} + +/** ------------------------------------------------------------------ + ** @brief Get the maximum number of comparisons in the KD-forest ANN algorithm. + ** @param self KMeans object instance. + ** @return maximum number of comparisons. + **/ + +VL_INLINE vl_size +vl_kmeans_get_max_num_comparisons (VlKMeans const * self) +{ + return self->maxNumComparisons ; +} + +/** @brief Set maximum number of comparisons in ANN-KD-Tree. + ** @param self KMeans filter. + ** @param maxNumComparisons maximum number of comparisons. + **/ + +VL_INLINE void +vl_kmeans_set_max_num_comparisons (VlKMeans * self, + vl_size maxNumComparisons) +{ + self->maxNumComparisons = maxNumComparisons; +} + +/** ------------------------------------------------------------------ + ** @brief Set the number of trees in the KD-forest ANN algorithm + ** @param self KMeans object instance. + ** @param numTrees number of trees to use. + **/ + +VL_INLINE void +vl_kmeans_set_num_trees (VlKMeans * self, vl_size numTrees) +{ + self->numTrees = numTrees; +} + +VL_INLINE vl_size +vl_kmeans_get_num_trees (VlKMeans const * self) +{ + return self->numTrees; +} + + +/* VL_IKMEANS_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/lbp.h b/colmap/include/colmap/lib/VLFeat/lbp.h new file mode 100644 index 0000000000000000000000000000000000000000..d33715b5ece8a44791ac03291e4b69fe5ada14a4 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/lbp.h @@ -0,0 +1,44 @@ +/** @file lbp.h + ** @brief Local Binary Patterns (LBP) descriptor (@ref lbp) + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_LBP_H +#define VL_LBP_H + +#include "generic.h" + +/** @brief Type of quantization for the LBP descriptors + ** @see @ref lbp-quantization + **/ +typedef enum _VlLbpMappingType +{ + VlLbpUniform /**< Uniform local binary patterns. */ +} VlLbpMappingType ; + +/** @brief Local Binary Pattern extractor */ +typedef struct VlLbp_ +{ + vl_size dimension ; + vl_uint8 mapping [256] ; + vl_bool transposed ; +} VlLbp ; + +VL_EXPORT VlLbp * vl_lbp_new(VlLbpMappingType type, vl_bool transposed) ; +VL_EXPORT void vl_lbp_delete(VlLbp * self) ; +VL_EXPORT void vl_lbp_process(VlLbp * self, + float * features, + float * image, vl_size width, vl_size height, + vl_size cellSize) ; +VL_EXPORT vl_size vl_lbp_get_dimension(VlLbp * self) ; + +/* VL_LBP_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/liop.h b/colmap/include/colmap/lib/VLFeat/liop.h new file mode 100644 index 0000000000000000000000000000000000000000..d96de915d1762d54ca504aae1b1f975d44cf1f76 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/liop.h @@ -0,0 +1,78 @@ +/** @file liop.h + ** @brief Local Intensity Order Pattern (LIOP) descriptor (@ref liop) + ** @author Hana Sarbortova + ** @author Andrea Vedaldi + ** @see @ref liop + **/ + +/* +Copyright (C) 2013 Hana Sarbortova and Andrea Vedaldi. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_LIOP_H +#define VL_LIOP_H + +#include "generic.h" + +/** @brief LIOP descriptor extractor object */ +typedef struct _VlLiopDesc +{ + vl_int numNeighbours; /**< Number of neighbours. */ + vl_int numSpatialBins; /**< Number of bins. */ + float intensityThreshold; /**< Weight threshold. */ + vl_size dimension; /**< LIOP descriptor size. */ + + /* Pixels in the circular patch */ + vl_size patchSideLength ; + vl_size patchSize ; /* only circular neighbourhood */ + vl_uindex * patchPixels ; + float * patchIntensities ; + vl_uindex * patchPermutation ; + + /* Neighbourhoods of each pixel (samples in a circle) */ + float neighRadius; /**< Point to neighbour radius (distance). */ + + float * neighIntensities ; + vl_uindex * neighPermutation ; + double * neighSamplesX ; + double * neighSamplesY ; + +} VlLiopDesc ; + +/** @name Construct and destroy + ** @{ */ +VL_EXPORT +VlLiopDesc * vl_liopdesc_new (vl_int numNeighbours, + vl_int numSpatialBins, + float radius, + vl_size sideLength) ; + +VL_EXPORT +VlLiopDesc * vl_liopdesc_new_basic (vl_size sideLength) ; + +VL_EXPORT +void vl_liopdesc_delete (VlLiopDesc * self) ; +/** @} */ + +/** @name Get data and parameters + ** @{ */ +VL_EXPORT vl_size vl_liopdesc_get_dimension (VlLiopDesc const * self) ; +VL_EXPORT vl_size vl_liopdesc_get_num_neighbours (VlLiopDesc const * self) ; +VL_EXPORT float vl_liopdesc_get_intensity_threshold (VlLiopDesc const * self) ; +VL_EXPORT vl_size vl_liopdesc_get_num_spatial_bins (VlLiopDesc const * self) ; +VL_EXPORT double vl_liopdesc_get_neighbourhood_radius (VlLiopDesc const * self) ; +VL_EXPORT void vl_liopdesc_set_intensity_threshold (VlLiopDesc * self, float x) ; +/** @} */ + +/** @name Compute LIOP descriptor + ** @{ */ +VL_EXPORT +void vl_liopdesc_process (VlLiopDesc * liop, float * desc, float const * patch) ; +/** @} */ + +/* VL_LIOP_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/mathop.h b/colmap/include/colmap/lib/VLFeat/mathop.h new file mode 100644 index 0000000000000000000000000000000000000000..6a1df8058e528ff2163157bb8fcccc75b2c59f86 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/mathop.h @@ -0,0 +1,719 @@ +/** @file mathop.h + ** @brief Math operations (@ref mathop) + ** @author Andrea Vedaldi, David Novotny + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_MATHOP_H +#define VL_MATHOP_H + +#include "generic.h" +#include +#include + +/** @brief Euler constant*/ +#define VL_E 2.718281828459045 + +/** @brief Logarithm of 2 (math constant)*/ +#define VL_LOG_OF_2 0.693147180559945 + +/** @brief Pi (math constant) */ +#define VL_PI 3.141592653589793 + +/** @brief IEEE single precision epsilon (math constant) + ** + ** 1.0F + VL_EPSILON_F is the smallest representable + ** single precision number greater than @c 1.0F. Numerically, + ** ::VL_EPSILON_F is equal to @f$ 2^{-23} @f$. + ** + **/ +#define VL_EPSILON_F 1.19209290E-07F + +/** @brief IEEE double precision epsilon (math constant) + ** + ** 1.0 + VL_EPSILON_D is the smallest representable + ** double precision number greater than @c 1.0. Numerically, + ** ::VL_EPSILON_D is equal to @f$ 2^{-52} @f$. + **/ +#define VL_EPSILON_D 2.220446049250313e-16 + +/* + For the code below: An ANSI C compiler takes the two expressions, + LONG_VAR and CHAR_VAR, and implicitly casts them to the type of the + first member of the union. Refer to K&R Second Edition Page 148, + last paragraph. +*/ + +/** @internal @brief IEEE single precision quiet NaN constant */ +static union { vl_uint32 raw ; float value ; } + const vl_nan_f = + { 0x7FC00000UL } ; + +/** @internal @brief IEEE single precision infinity constant */ +static union { vl_uint32 raw ; float value ; } + const vl_infinity_f = + { 0x7F800000UL } ; + +/** @internal @brief IEEE double precision quiet NaN constant */ +static union { vl_uint64 raw ; double value ; } + const vl_nan_d = +#ifdef VL_COMPILER_MSC + { 0x7FF8000000000000ui64 } ; +#else + { 0x7FF8000000000000ULL } ; +#endif + +/** @internal @brief IEEE double precision infinity constant */ +static union { vl_uint64 raw ; double value ; } + const vl_infinity_d = +#ifdef VL_COMPILER_MSC + { 0x7FF0000000000000ui64 } ; +#else + { 0x7FF0000000000000ULL } ; +#endif + +/** @brief IEEE single precision NaN (not signaling) */ +#define VL_NAN_F (vl_nan_f.value) + +/** @brief IEEE single precision positive infinity (not signaling) */ +#define VL_INFINITY_F (vl_infinity_f.value) + +/** @brief IEEE double precision NaN (not signaling) */ +#define VL_NAN_D (vl_nan_d.value) + +/** @brief IEEE double precision positive infinity (not signaling) */ +#define VL_INFINITY_D (vl_infinity_d.value) + +/* ---------------------------------------------------------------- */ + +/** @brief Fast mod(x, 2 * VL_PI) + ** + ** @param x input value. + ** @return mod(x, 2 * VL_PI) + ** + ** The function is optimized for small absolute values of @a x. + ** + ** The result is guaranteed to be not smaller than 0. However, due to + ** finite numerical precision and rounding errors, the result can be + ** equal to 2 * VL_PI (for instance, if @c x is a very small negative + ** number). + **/ + +VL_INLINE float +vl_mod_2pi_f (float x) +{ + while (x > (float)(2 * VL_PI)) x -= (float) (2 * VL_PI) ; + while (x < 0.0F) x += (float) (2 * VL_PI); + return x ; +} + +/** @brief Fast mod(x, 2 * VL_PI) + ** @see vl_mod_2pi_f + **/ + +VL_INLINE double +vl_mod_2pi_d (double x) +{ + while (x > 2.0 * VL_PI) x -= 2 * VL_PI ; + while (x < 0.0) x += 2 * VL_PI ; + return x ; +} + +/** @brief Floor and convert to integer + ** @param x argument. + ** @return Similar to @c (int) floor(x) + **/ + +VL_INLINE long int +vl_floor_f (float x) +{ + long int xi = (long int) x ; + if (x >= 0 || (float) xi == x) return xi ; + else return xi - 1 ; +} + +/** @brief Floor and convert to integer + ** @see vl_floor_f + **/ + +VL_INLINE long int +vl_floor_d (double x) +{ + long int xi = (long int) x ; + if (x >= 0 || (double) xi == x) return xi ; + else return xi - 1 ; +} + +/** @brief Ceil and convert to integer + ** @param x argument. + ** @return @c lceilf(x) + **/ + +VL_INLINE long int +vl_ceil_f (float x) +{ +#ifdef VL_COMPILER_GNUC + return (long int) __builtin_ceilf(x) ; +#else + return (long int) ceilf(x) ; +#endif +} + +/** @brief Ceil and convert to integer + ** @see vl_ceil_f + **/ + +VL_INLINE long int +vl_ceil_d (double x) +{ +#ifdef VL_COMPILER_GNUC + return __builtin_ceil(x) ; +#else + return (long int) ceil(x) ; +#endif +} + +/** @brief Round + ** @param x argument. + ** @return @c lroundf(x) + ** This function is either the same or similar to C99 @c lroundf(). + **/ + +VL_INLINE long int +vl_round_f (float x) +{ +#ifdef VL_COMPILER_GNUC + return __builtin_lroundf(x) ; +#elif VL_COMPILER_MSC + if (x >= 0.0F) { + return vl_floor_f(x + 0.5F) ; + } else { + return vl_ceil_f(x - 0.5F) ; + } +#else + return lroundf(x) ; +#endif +} + +/** @brief Round + ** @param x argument. + ** @return @c lround(x) + ** This function is either the same or similar to C99 @c lround(). + **/ + +VL_INLINE long int +vl_round_d (double x) +{ +#ifdef VL_COMPILER_GNUC + return __builtin_lround(x) ; +#elif VL_COMPILER_MSC + if (x >= 0.0) { + return vl_floor_d(x + 0.5) ; + } else { + return vl_ceil_d(x - 0.5) ; + } +#else + return lround(x) ; +#endif +} + +/** @brief Fast @c abs(x) + ** @param x argument. + ** @return @c abs(x) + **/ + +VL_INLINE float +vl_abs_f (float x) +{ +#ifdef VL_COMPILER_GNUC + return __builtin_fabsf (x) ; +#else + return fabsf(x) ; +#endif +} + +/** @brief Fast @c abs(x) + ** @sa vl_abs_f + **/ + +VL_INLINE double +vl_abs_d (double x) +{ +#ifdef VL_COMPILER_GNUC + return __builtin_fabs (x) ; +#else + return fabs(x) ; +#endif +} + +/** @brief Base-2 logaritghm + ** @param x argument. + ** @return @c log(x). + **/ + +VL_INLINE double +vl_log2_d (double x) +{ +#ifdef VL_COMPILER_GNUC + return __builtin_log2(x) ; +#elif VL_COMPILER_MSC + return log(x) / 0.693147180559945 ; +#else + return log2(x) ; +#endif +} + +/** @copydoc vl_log2_d */ +VL_INLINE float +vl_log2_f (float x) +{ +#ifdef VL_COMPILER_GNUC + return __builtin_log2f (x) ; +#elif VL_COMPILER_MSC + return logf(x) / 0.6931472F ; +#else + return log2(x) ; +#endif +} + +/** @brief Square root. + ** @param x argument. + ** @return @c sqrt(x). + **/ + +VL_INLINE double +vl_sqrt_d (double x) +{ +#ifdef VL_COMPILER_GNUC + return __builtin_sqrt(x) ; +#else + return sqrt(x) ; +#endif +} + +/** @copydoc vl_sqrt_d */ +VL_INLINE float +vl_sqrt_f (float x) +{ +#ifdef VL_COMPILER_GNUC + return __builtin_sqrtf(x) ; +#else + return sqrtf(x) ; +#endif +} + + +/** @brief Check whether a floating point value is NaN + ** @param x argument. + ** @return true if @a x is NaN. + **/ +VL_INLINE vl_bool +vl_is_nan_f (float x) +{ +#ifdef VL_COMPILER_GNUC + return __builtin_isnan (x) ; +#elif VL_COMPILER_MSC + return _isnan(x) ; +#else + return isnan(x) ; +#endif +} + +/** @copydoc vl_is_nan_f */ +VL_INLINE vl_bool +vl_is_nan_d (double x) +{ +#ifdef VL_COMPILER_GNUC + return __builtin_isnan (x) ; +#elif VL_COMPILER_MSC + return _isnan(x) ; +#else + return isnan(x) ; +#endif +} + +/** @brief Check whether a floating point value is infinity + ** @param x argument. + ** @return true if @a x is infinity. + **/ +VL_INLINE vl_bool +vl_is_inf_f (float x) +{ +#ifdef VL_COMPILER_GNUC + return __builtin_isinf (x) ; +#elif VL_COMPILER_MSC + return ! _finite(x) ; +#else + return isinf(x) ; +#endif +} + +/** @copydoc vl_is_inf_f */ +VL_INLINE vl_bool +vl_is_inf_d (double x) +{ +#ifdef VL_COMPILER_GNUC + return __builtin_isinf (x) ; +#elif VL_COMPILER_MSC + return ! _finite(x) ; +#else + return isinf(x) ; +#endif +} + +/** ------------------------------------------------------------------ + ** @brief Fast @c atan2 approximation + ** @param y argument. + ** @param x argument. + ** + ** The function computes a relatively rough but fast approximation of + ** @c atan2(y,x). + ** + ** @par Algorithm + ** + ** The algorithm approximates the function @f$ f(r)=atan((1-r)/(1+r)) + ** @f$, @f$ r \in [-1,1] @f$ with a third order polynomial @f$ + ** f(r)=c_0 + c_1 r + c_2 r^2 + c_3 r^3 @f$. To fit the polynomial + ** we impose the constraints + ** + ** @f{eqnarray*} + ** f(+1) &=& c_0 + c_1 + c_2 + c_3 = atan(0) = 0,\\ + ** f(-1) &=& c_0 - c_1 + c_2 - c_3 = atan(\infty) = \pi/2,\\ + ** f(0) &=& c_0 = atan(1) = \pi/4. + ** @f} + ** + ** The last degree of freedom is fixed by minimizing the @f$ + ** l^{\infty} @f$ error, which yields + ** + ** @f[ + ** c_0=\pi/4, \quad + ** c_1=-0.9675, \quad + ** c_2=0, \quad + ** c_3=0.1821, + ** @f] + ** + ** with maximum error of 0.0061 radians at 0.35 degrees. + ** + ** @return Approximation of @c atan2(y,x). + **/ + +VL_INLINE float +vl_fast_atan2_f (float y, float x) +{ + float angle, r ; + float const c3 = 0.1821F ; + float const c1 = 0.9675F ; + float abs_y = vl_abs_f (y) + VL_EPSILON_F ; + + if (x >= 0) { + r = (x - abs_y) / (x + abs_y) ; + angle = (float) (VL_PI / 4) ; + } else { + r = (x + abs_y) / (abs_y - x) ; + angle = (float) (3 * VL_PI / 4) ; + } + angle += (c3*r*r - c1) * r ; + return (y < 0) ? - angle : angle ; +} + +/** @brief Fast @c atan2 approximation + ** @sa vl_fast_atan2_f + **/ + +VL_INLINE double +vl_fast_atan2_d (double y, double x) +{ + double angle, r ; + double const c3 = 0.1821 ; + double const c1 = 0.9675 ; + double abs_y = vl_abs_d (y) + VL_EPSILON_D ; + + if (x >= 0) { + r = (x - abs_y) / (x + abs_y) ; + angle = VL_PI / 4 ; + } else { + r = (x + abs_y) / (abs_y - x) ; + angle = 3 * VL_PI / 4 ; + } + angle += (c3*r*r - c1) * r ; + return (y < 0) ? - angle : angle ; +} + +/** ------------------------------------------------------------------ + ** @brief Fast @c resqrt approximation + ** @param x argument. + ** @return approximation of @c resqrt(x). + ** + ** The function quickly computes an approximation of @f$ x^{-1/2} + ** @f$. + ** + ** @par Algorithm + ** + ** The goal is to compute @f$ y = x^{-1/2} @f$, which we do by + ** finding the solution of @f$ 0 = f(y) = y^{-2} - x @f$ by two Newton + ** steps. Each Newton iteration is given by + ** + ** @f[ + ** y \leftarrow + ** y - \frac{f(y)}{\frac{df(y)}{dy}} = + ** y + \frac{1}{2} (y-xy^3) = + ** \frac{y}{2} \left( 3 - xy^2 \right) + ** @f] + ** + ** which yields a simple polynomial update rule. + ** + ** The clever bit (attributed to either J. Carmack or G. Tarolli) is + ** the way an initial guess @f$ y \approx x^{-1/2} @f$ is chosen. + ** + ** @see Inverse Sqare Root. + ** + **/ + +VL_INLINE float +vl_fast_resqrt_f (float x) +{ + /* 32-bit version */ + union { + float x ; + vl_int32 i ; + } u ; + + float xhalf = (float) 0.5 * x ; + + /* convert floating point value in RAW integer */ + u.x = x ; + + /* gives initial guess y0 */ + u.i = 0x5f3759df - (u.i >> 1); + /*u.i = 0xdf59375f - (u.i>>1);*/ + + /* two Newton steps */ + u.x = u.x * ( (float) 1.5 - xhalf*u.x*u.x) ; + u.x = u.x * ( (float) 1.5 - xhalf*u.x*u.x) ; + return u.x ; +} + +/** @brief Fast @c resqrt approximation + ** @sa vl_fast_resqrt_d + **/ + +VL_INLINE double +vl_fast_resqrt_d (double x) +{ + /* 64-bit version */ + union { + double x ; + vl_int64 i ; + } u ; + + double xhalf = (double) 0.5 * x ; + + /* convert floating point value in RAW integer */ + u.x = x ; + + /* gives initial guess y0 */ +#ifdef VL_COMPILER_MSC + u.i = 0x5fe6ec85e7de30dai64 - (u.i >> 1) ; +#else + u.i = 0x5fe6ec85e7de30daLL - (u.i >> 1) ; +#endif + + /* two Newton steps */ + u.x = u.x * ( (double) 1.5 - xhalf*u.x*u.x) ; + u.x = u.x * ( (double) 1.5 - xhalf*u.x*u.x) ; + return u.x ; +} + +/** ------------------------------------------------------------------ + ** @brief Fast @c sqrt approximation + ** @param x argument. + ** @return approximation of @c sqrt(x). + ** + ** The function uses ::vl_fast_resqrt_f + ** (or ::vl_fast_resqrt_d) to compute x * + ** vl_fast_resqrt_f(x). + **/ + +VL_INLINE float +vl_fast_sqrt_f (float x) +{ + return (x < 1e-8) ? 0 : x * vl_fast_resqrt_f (x) ; +} + +/** @brief Fast @c sqrt approximation + ** @copydoc vl_fast_sqrt_f + **/ + +VL_INLINE double +vl_fast_sqrt_d (float x) +{ + return (x < 1e-8) ? 0 : x * vl_fast_resqrt_d (x) ; +} + +/** @brief Fast integer @c sqrt approximation + ** @param x non-negative integer. + ** @return largest integer $y$ such that $y^2 \leq x$. + ** @sa @ref mathop-sqrti "Algorithm" + **/ +VL_INLINE vl_uint64 vl_fast_sqrt_ui64 (vl_uint64 x) ; + +/** @brief Fast @c sqrt approximation + ** @copydoc vl_fast_sqrt_ui64 */ +VL_INLINE vl_uint32 vl_fast_sqrt_ui32 (vl_uint32 x) ; + +/** @brief Fast @c sqrt approximation + ** @copydoc vl_fast_sqrt_ui64 */ +VL_INLINE vl_uint16 vl_fast_sqrt_ui16 (vl_uint16 x) ; + +/** @brief Fast @c sqrt approximation + ** @copydoc vl_fast_sqrt_ui64 */ +VL_INLINE vl_uint8 vl_fast_sqrt_ui8 (vl_uint8 x) ; + +#define VL_FAST_SQRT_UI(T,SFX) \ +VL_INLINE T \ +vl_fast_sqrt_ ## SFX (T x) \ +{ \ + T y = 0 ; \ + T tmp = 0 ; \ + int twice_k ; \ + for (twice_k = 8 * sizeof(T) - 2 ; \ + twice_k >= 0 ; twice_k -= 2) { \ + y <<= 1 ; /* y = 2 * y */ \ + tmp = (2*y + 1) << twice_k ; \ + if (x >= tmp) { \ + x -= tmp ; \ + y += 1 ; \ + } \ + } \ + return y ; \ +} + +VL_FAST_SQRT_UI(vl_uint64,ui64) +VL_FAST_SQRT_UI(vl_uint32,ui32) +VL_FAST_SQRT_UI(vl_uint16,ui16) +VL_FAST_SQRT_UI(vl_uint8,ui8) + +/* ---------------------------------------------------------------- */ +/* Vector distances and similarities */ +/* ---------------------------------------------------------------- */ + +/** @typedef VlFloatVectorComparisonFunction + ** @brief Pointer to a function to compare vectors of floats + **/ +typedef float (*VlFloatVectorComparisonFunction)(vl_size dimension, float const * X, float const * Y) ; + +/** @typedef VlDoubleVectorComparisonFunction + ** @brief Pointer to a function to compare vectors of doubles + **/ +typedef double (*VlDoubleVectorComparisonFunction)(vl_size dimension, double const * X, double const * Y) ; + +/** @typedef VlFloatVector3ComparisonFunction + ** @brief Pointer to a function to compare 3 vectors of doubles + **/ +typedef float (*VlFloatVector3ComparisonFunction)(vl_size dimension, float const * X, float const * Y, float const * Z) ; + +/** @typedef VlDoubleVector3ComparisonFunction + ** @brief Pointer to a function to compare 3 vectors of doubles + **/ +typedef double (*VlDoubleVector3ComparisonFunction)(vl_size dimension, double const * X, double const * Y, double const * Z) ; + +/** @brief Vector comparison types */ +enum _VlVectorComparisonType { + VlDistanceL1, /**< l1 distance (squared intersection metric) */ + VlDistanceL2, /**< squared l2 distance */ + VlDistanceChi2, /**< squared Chi2 distance */ + VlDistanceHellinger, /**< squared Hellinger's distance */ + VlDistanceJS, /**< squared Jensen-Shannon distance */ + VlDistanceMahalanobis, /**< squared mahalanobis distance */ + VlKernelL1, /**< intersection kernel */ + VlKernelL2, /**< l2 kernel */ + VlKernelChi2, /**< Chi2 kernel */ + VlKernelHellinger, /**< Hellinger's kernel */ + VlKernelJS /**< Jensen-Shannon kernel */ +} ; + +/** @brief Vector comparison types */ +typedef enum _VlVectorComparisonType VlVectorComparisonType ; + +/** @brief Get the symbolic name of a vector comparison type + ** @param type vector comparison type. + ** @return data symbolic name. + **/ + +VL_INLINE char const * +vl_get_vector_comparison_type_name (int type) +{ + switch (type) { + case VlDistanceL1 : return "l1" ; + case VlDistanceL2 : return "l2" ; + case VlDistanceChi2 : return "chi2" ; + case VlDistanceMahalanobis : return "mahalanobis" ; + case VlKernelL1 : return "kl1" ; + case VlKernelL2 : return "kl2" ; + case VlKernelChi2 : return "kchi2" ; + default: return NULL ; + } +} + +VL_EXPORT VlFloatVectorComparisonFunction +vl_get_vector_comparison_function_f (VlVectorComparisonType type) ; + +VL_EXPORT VlDoubleVectorComparisonFunction +vl_get_vector_comparison_function_d (VlVectorComparisonType type) ; + +VL_EXPORT VlFloatVector3ComparisonFunction +vl_get_vector_3_comparison_function_f (VlVectorComparisonType type) ; + +VL_EXPORT VlDoubleVector3ComparisonFunction +vl_get_vector_3_comparison_function_d (VlVectorComparisonType type) ; + + +VL_EXPORT void +vl_eval_vector_comparison_on_all_pairs_f (float * result, vl_size dimension, + float const * X, vl_size numDataX, + float const * Y, vl_size numDataY, + VlFloatVectorComparisonFunction function) ; + +VL_EXPORT void +vl_eval_vector_comparison_on_all_pairs_d (double * result, vl_size dimension, + double const * X, vl_size numDataX, + double const * Y, vl_size numDataY, + VlDoubleVectorComparisonFunction function) ; + +/* ---------------------------------------------------------------- */ +/* Numerical analysis */ +/* ---------------------------------------------------------------- */ + +VL_EXPORT void +vl_svd2 (double* S, double *U, double *V, double const *M) ; + +VL_EXPORT void +vl_lapack_dlasv2 (double *smin, + double *smax, + double *sv, + double *cv, + double *su, + double *cu, + double f, + double g, + double h) ; + + +VL_EXPORT int +vl_solve_linear_system_3 (double * x, double const * A, double const *b) ; + +VL_EXPORT int +vl_solve_linear_system_2 (double * x, double const * A, double const *b) ; + +VL_EXPORT int +vl_gaussian_elimination (double * A, vl_size numRows, vl_size numColumns) ; + +/* VL_MATHOP_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/mathop_avx.h b/colmap/include/colmap/lib/VLFeat/mathop_avx.h new file mode 100644 index 0000000000000000000000000000000000000000..2acebf0fd9d5a59b9410cd29f7637f65df73fc86 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/mathop_avx.h @@ -0,0 +1,61 @@ +/** @file mathop_avx.h + ** @brief mathop for avx + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +/* ---------------------------------------------------------------- */ +#ifndef VL_MATHOP_AVX_H_INSTANTIATING + +#ifndef VL_MATHOP_AVX_H +#define VL_MATHOP_AVX_H + +#undef FLT +#define FLT VL_TYPE_DOUBLE +#define VL_MATHOP_AVX_H_INSTANTIATING +#include "mathop_avx.h" + +#undef FLT +#define FLT VL_TYPE_FLOAT +#define VL_MATHOP_AVX_H_INSTANTIATING +#include "mathop_avx.h" + +/* VL_MATHOP_AVX_H */ +#endif + +/* ---------------------------------------------------------------- */ +/* VL_MATHOP_AVX_H_INSTANTIATING */ +#else + +#ifndef VL_DISABLE_AVX +#include "generic.h" +#include "float.h" + +VL_EXPORT T +VL_XCAT(_vl_distance_mahalanobis_sq_avx_, SFX) +(vl_size dimension, T const * X, T const * MU, T const * S); + +VL_EXPORT T +VL_XCAT(_vl_distance_l2_avx_, SFX) +(vl_size dimension, T const * X, T const * Y); + +VL_EXPORT void +VL_XCAT(_vl_weighted_sigma_avx_, SFX) +(vl_size dimension, T * S, T const * X, T const * Y, T const W); + +VL_EXPORT void +VL_XCAT(_vl_weighted_mean_avx_, SFX) +(vl_size dimension, T * MU, T const * X, T const W); + +/* ! VL_DISABLE_AVX */ +#endif + +#undef VL_MATHOP_AVX_H_INSTANTIATING +#endif diff --git a/colmap/include/colmap/lib/VLFeat/mathop_sse2.h b/colmap/include/colmap/lib/VLFeat/mathop_sse2.h new file mode 100644 index 0000000000000000000000000000000000000000..553cea12ff1292db7808f01cb6ad03dca901b8e5 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/mathop_sse2.h @@ -0,0 +1,85 @@ +/** @file mathop_sse2.h + ** @brief mathop for sse2 + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +/* ---------------------------------------------------------------- */ +#ifndef VL_MATHOP_SSE2_H_INSTANTIATING + +#ifndef VL_MATHOP_SSE2_H +#define VL_MATHOP_SSE2_H + +#undef FLT +#define FLT VL_TYPE_DOUBLE +#define VL_MATHOP_SSE2_H_INSTANTIATING +#include "mathop_sse2.h" + +#undef FLT +#define FLT VL_TYPE_FLOAT +#define VL_MATHOP_SSE2_H_INSTANTIATING +#include "mathop_sse2.h" + +/* VL_MATHOP_SSE2_H */ +#endif + +/* ---------------------------------------------------------------- */ +/* VL_MATHOP_SSE2_H_INSTANTIATING */ +#else + +#ifndef VL_DISABLE_SSE2 + +#include "generic.h" +#include "float.h" + +VL_EXPORT T +VL_XCAT(_vl_dot_sse2_, SFX) +(vl_size dimension, T const * X, T const * Y) ; + +VL_EXPORT T +VL_XCAT(_vl_distance_l2_sse2_, SFX) +(vl_size dimension, T const * X, T const * Y) ; + +VL_EXPORT T +VL_XCAT(_vl_distance_l1_sse2_, SFX) +(vl_size dimension, T const * X, T const * Y) ; + +VL_EXPORT T +VL_XCAT(_vl_distance_chi2_sse2_, SFX) +(vl_size dimension, T const * X, T const * Y) ; + +VL_EXPORT T +VL_XCAT(_vl_kernel_l2_sse2_, SFX) +(vl_size dimension, T const * X, T const * Y) ; + +VL_EXPORT T +VL_XCAT(_vl_kernel_l1_sse2_, SFX) +(vl_size dimension, T const * X, T const * Y) ; + +VL_EXPORT T +VL_XCAT(_vl_kernel_chi2_sse2_, SFX) +(vl_size dimension, T const * X, T const * Y) ; + +VL_EXPORT T +VL_XCAT(_vl_distance_mahalanobis_sq_sse2_, SFX) +(vl_size dimension, T const * X, T const * MU, T const * S); + +VL_EXPORT void +VL_XCAT(_vl_weighted_sigma_sse2_, SFX) +(vl_size dimension, T * S, T const * X, T const * Y, T const W); + +VL_EXPORT void +VL_XCAT(_vl_weighted_mean_sse2_, SFX) +(vl_size dimension, T * MU, T const * X, T const W); + +/* ! VL_DISABLE_SSE2 */ +#endif +#undef VL_MATHOP_SSE2_INSTANTIATING +#endif diff --git a/colmap/include/colmap/lib/VLFeat/mser.h b/colmap/include/colmap/lib/VLFeat/mser.h new file mode 100644 index 0000000000000000000000000000000000000000..bd63cedb1bfe80a5a1c7ef6c019382599d91a1ff --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/mser.h @@ -0,0 +1,424 @@ +/** @file mser.h + ** @brief MSER (@ref mser) + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_MSER +#define VL_MSER + +#include "generic.h" + +/** @brief MSER image data type + ** + ** This is the data type of the image pixels. It has to be an + ** integer. + **/ +typedef vl_uint8 vl_mser_pix ; + +/** @brief Maximum value + ** + ** Maximum value of the integer type ::vl_mser_pix. + **/ +#define VL_MSER_PIX_MAXVAL 256 + +/** @brief MSER Filter + ** + ** The MSER filter computes the Maximally Stable Extremal Regions of + ** an image. + ** + ** @sa @ref mser + **/ +typedef struct _VlMserFilt VlMserFilt ; + +/** @brief MSER filter statistics */ +typedef struct _VlMserStats VlMserStats ; + +/** @brief MSER filter statistics definition */ +struct _VlMserStats +{ + int num_extremal ; /**< number of extremal regions */ + int num_unstable ; /**< number of unstable extremal regions */ + int num_abs_unstable ; /**< number of regions that failed the absolute stability test */ + int num_too_big ; /**< number of regions that failed the maximum size test */ + int num_too_small ; /**< number of regions that failed the minimum size test */ + int num_duplicates ; /**< number of regions that failed the duplicate test */ +} ; + +/** @name Construction and Destruction + ** @{ + **/ +VL_EXPORT VlMserFilt* vl_mser_new (int ndims, int const* dims) ; +VL_EXPORT void vl_mser_delete (VlMserFilt *f) ; +/** @} */ + +/** @name Processing + ** @{ + **/ +VL_EXPORT void vl_mser_process (VlMserFilt *f, + vl_mser_pix const *im) ; +VL_EXPORT void vl_mser_ell_fit (VlMserFilt *f) ; +/** @} */ + +/** @name Retrieving data + ** @{ + **/ +VL_INLINE vl_uint vl_mser_get_regions_num (VlMserFilt const *f) ; +VL_INLINE vl_uint const* vl_mser_get_regions (VlMserFilt const *f) ; +VL_INLINE float const* vl_mser_get_ell (VlMserFilt const *f) ; +VL_INLINE vl_uint vl_mser_get_ell_num (VlMserFilt const *f) ; +VL_INLINE vl_uint vl_mser_get_ell_dof (VlMserFilt const *f) ; +VL_INLINE VlMserStats const* vl_mser_get_stats (VlMserFilt const *f) ; +/** @} */ + +/** @name Retrieving parameters + ** @{ + **/ +VL_INLINE vl_mser_pix vl_mser_get_delta (VlMserFilt const *f) ; +VL_INLINE double vl_mser_get_min_area (VlMserFilt const *f) ; +VL_INLINE double vl_mser_get_max_area (VlMserFilt const *f) ; +VL_INLINE double vl_mser_get_max_variation (VlMserFilt const *f) ; +VL_INLINE double vl_mser_get_min_diversity (VlMserFilt const *f) ; +/** @} */ + +/** @name Setting parameters + ** @{ + **/ +VL_INLINE void vl_mser_set_delta (VlMserFilt *f, vl_mser_pix x) ; +VL_INLINE void vl_mser_set_min_area (VlMserFilt *f, double x) ; +VL_INLINE void vl_mser_set_max_area (VlMserFilt *f, double x) ; +VL_INLINE void vl_mser_set_max_variation (VlMserFilt *f, double x) ; +VL_INLINE void vl_mser_set_min_diversity (VlMserFilt *f, double x) ; +/** @} */ + +/* ==================================================================== + * INLINE DEFINITIONS + * ================================================================== */ + +/** @internal + ** @brief MSER accumulator data type + ** + ** This is a large integer type. It should be large enough to contain + ** a number equal to the area (volume) of the image by the image + ** width by the image height (for instance, if the image is a square + ** of side 256, the maximum value is 256 x 256 x 256). + **/ +typedef float vl_mser_acc ; + +/** @internal @brief Basic region flag: null region */ +#ifdef VL_COMPILER_MSC +#define VL_MSER_VOID_NODE ((1ui64<<32) - 1) +#else +#define VL_MSER_VOID_NODE ((1ULL<<32) - 1) +#endif + +/* ----------------------------------------------------------------- */ +/** @internal + ** @brief MSER: basic region (declaration) + ** + ** Extremal regions and maximally stable extremal regions are + ** instances of image regions. + ** + ** There is an image region for each pixel of the image. Each region + ** is represented by an instance of this structure. Regions are + ** stored into an array in pixel order. + ** + ** Regions are arranged into a forest. VlMserReg::parent points to + ** the parent node, or to the node itself if the node is a root. + ** VlMserReg::parent is the index of the node in the node array + ** (which therefore is also the index of the corresponding + ** pixel). VlMserReg::height is the distance of the fartest leaf. If + ** the node itself is a leaf, then VlMserReg::height is zero. + ** + ** VlMserReg::area is the area of the image region corresponding to + ** this node. + ** + ** VlMserReg::region is the extremal region identifier. Not all + ** regions are extremal regions however; if the region is NOT + ** extremal, this field is set to .... + **/ +struct _VlMserReg +{ + vl_uint parent ; /**< points to the parent region. */ + vl_uint shortcut ; /**< points to a region closer to a root. */ + vl_uint height ; /**< region height in the forest. */ + vl_uint area ; /**< area of the region. */ +} ; + +/** @internal @brief MSER: basic region */ +typedef struct _VlMserReg VlMserReg ; + +/* ----------------------------------------------------------------- */ +/** @internal + ** @brief MSER: extremal region (declaration) + ** + ** Extremal regions (ER) are extracted from the region forest. Each + ** region is represented by an instance of this structure. The + ** structures are stored into an array, in arbitrary order. + ** + ** ER are arranged into a tree. @a parent points to the parent ER, or + ** to itself if the ER is the root. + ** + ** An instance of the structure represents the extremal region of the + ** level set of intensity VlMserExtrReg::value and containing the + ** pixel VlMserExtReg::index. + ** + ** VlMserExtrReg::area is the area of the extremal region and + ** VlMserExtrReg::area_top is the area of the extremal region + ** containing this region in the level set of intensity + ** VlMserExtrReg::area + @c delta. + ** + ** VlMserExtrReg::variation is the relative area variation @c + ** (area_top-area)/area. + ** + ** VlMserExtrReg::max_stable is a flag signaling whether this extremal + ** region is also maximally stable. + **/ +struct _VlMserExtrReg +{ + int parent ; /**< index of the parent region */ + int index ; /**< index of pivot pixel */ + vl_mser_pix value ; /**< value of pivot pixel */ + vl_uint shortcut ; /**< shortcut used when building a tree */ + vl_uint area ; /**< area of the region */ + float variation ; /**< rel. area variation */ + vl_uint max_stable ; /**< max stable number (=0 if not maxstable) */ +} ; + +/** @internal + ** @brief MSER: extremal region */ +typedef struct _VlMserExtrReg VlMserExtrReg ; + +/* ----------------------------------------------------------------- */ +/** @internal + ** @brief MSER filter + ** @see @ref mser + **/ +struct _VlMserFilt +{ + + /** @name Image data and meta data @internal */ + /*@{*/ + int ndims ; /**< number of dimensions */ + int *dims ; /**< dimensions */ + int nel ; /**< number of image elements (pixels) */ + int *subs ; /**< N-dimensional subscript */ + int *dsubs ; /**< another subscript */ + int *strides ; /**< strides to move in image data */ + /*@}*/ + + vl_uint *perm ; /**< pixel ordering */ + vl_uint *joins ; /**< sequence of join ops */ + int njoins ; /**< number of join ops */ + + /** @name Regions */ + /*@{*/ + VlMserReg *r ; /**< basic regions */ + VlMserExtrReg *er ; /**< extremal tree */ + vl_uint *mer ; /**< maximally stable extremal regions */ + int ner ; /**< number of extremal regions */ + int nmer ; /**< number of maximally stable extr. reg. */ + int rer ; /**< size of er buffer */ + int rmer ; /**< size of mer buffer */ + /*@}*/ + + /** @name Ellipsoids fitting */ + /*@{*/ + float *acc ; /**< moment accumulator. */ + float *ell ; /**< ellipsoids list. */ + int rell ; /**< size of ell buffer */ + int nell ; /**< number of ellipsoids extracted */ + int dof ; /**< number of dof of ellipsoids. */ + + /*@}*/ + + /** @name Configuration */ + /*@{*/ + vl_bool verbose ; /**< be verbose */ + int delta ; /**< delta filter parameter */ + double max_area ; /**< badness test parameter */ + double min_area ; /**< badness test parameter */ + double max_variation ; /**< badness test parameter */ + double min_diversity ; /**< minimum diversity */ + /*@}*/ + + VlMserStats stats ; /** run statistic */ +} ; + +/* ----------------------------------------------------------------- */ +/** @brief Get delta + ** @param f MSER filter. + ** @return value of @c delta. + **/ +VL_INLINE vl_mser_pix +vl_mser_get_delta (VlMserFilt const *f) +{ + return f-> delta ; +} + +/** @brief Set delta + ** @param f MSER filter. + ** @param x value of @c delta. + **/ +VL_INLINE void +vl_mser_set_delta (VlMserFilt *f, vl_mser_pix x) +{ + f-> delta = x ; +} + +/* ----------------------------------------------------------------- */ +/** @brief Get minimum diversity + ** @param f MSER filter. + ** @return value of @c minimum diversity. + **/ +VL_INLINE double +vl_mser_get_min_diversity (VlMserFilt const *f) +{ + return f-> min_diversity ; +} + +/** @brief Set minimum diversity + ** @param f MSER filter. + ** @param x value of @c minimum diversity. + **/ +VL_INLINE void +vl_mser_set_min_diversity (VlMserFilt *f, double x) +{ + f-> min_diversity = x ; +} + +/* ----------------------------------------------------------------- */ +/** @brief Get statistics + ** @param f MSER filter. + ** @return statistics. + **/ +VL_INLINE VlMserStats const* +vl_mser_get_stats (VlMserFilt const *f) +{ + return & f-> stats ; +} + +/* ----------------------------------------------------------------- */ +/** @brief Get maximum region area + ** @param f MSER filter. + ** @return maximum region area. + **/ +VL_INLINE double +vl_mser_get_max_area (VlMserFilt const *f) +{ + return f-> max_area ; +} + +/** @brief Set maximum region area + ** @param f MSER filter. + ** @param x maximum region area. + **/ +VL_INLINE void +vl_mser_set_max_area (VlMserFilt *f, double x) +{ + f-> max_area = x ; +} + +/* ----------------------------------------------------------------- */ +/** @brief Get minimum region area + ** @param f MSER filter. + ** @return minimum region area. + **/ +VL_INLINE double +vl_mser_get_min_area (VlMserFilt const *f) +{ + return f-> min_area ; +} + +/** @brief Set minimum region area + ** @param f MSER filter. + ** @param x minimum region area. + **/ +VL_INLINE void +vl_mser_set_min_area (VlMserFilt *f, double x) +{ + f-> min_area = x ; +} + +/* ----------------------------------------------------------------- */ +/** @brief Get maximum region variation + ** @param f MSER filter. + ** @return maximum region variation. + **/ +VL_INLINE double +vl_mser_get_max_variation (VlMserFilt const *f) +{ + return f-> max_variation ; +} + +/** @brief Set maximum region variation + ** @param f MSER filter. + ** @param x maximum region variation. + **/ +VL_INLINE void +vl_mser_set_max_variation (VlMserFilt *f, double x) +{ + f-> max_variation = x ; +} + +/* ----------------------------------------------------------------- */ +/** @brief Get maximally stable extremal regions + ** @param f MSER filter. + ** @return array of MSER pivots. + **/ +VL_INLINE vl_uint const * +vl_mser_get_regions (VlMserFilt const* f) +{ + return f-> mer ; +} + +/** @brief Get number of maximally stable extremal regions + ** @param f MSER filter. + ** @return number of MSERs. + **/ +VL_INLINE vl_uint +vl_mser_get_regions_num (VlMserFilt const* f) +{ + return f-> nmer ; +} + +/* ----------------------------------------------------------------- */ +/** @brief Get ellipsoids + ** @param f MSER filter. + ** @return ellipsoids. + **/ +VL_INLINE float const * +vl_mser_get_ell (VlMserFilt const* f) +{ + return f-> ell ; +} + +/** @brief Get number of degrees of freedom of ellipsoids + ** @param f MSER filter. + ** @return number of degrees of freedom. + **/ +VL_INLINE vl_uint +vl_mser_get_ell_dof (VlMserFilt const* f) +{ + return f-> dof ; +} + +/** @brief Get number of ellipsoids + ** @param f MSER filter. + ** @return number of ellipsoids + **/ +VL_INLINE vl_uint +vl_mser_get_ell_num (VlMserFilt const* f) +{ + return f-> nell ; +} + +/* VL_MSER */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/pgm.h b/colmap/include/colmap/lib/VLFeat/pgm.h new file mode 100644 index 0000000000000000000000000000000000000000..80b0ef0f1298b793a454434ceaf7fec370e0757e --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/pgm.h @@ -0,0 +1,73 @@ +/** @file pgm.h + ** @brief Portable graymap format (PGM) parser + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_PGM_H +#define VL_PGM_H + +#include "generic.h" +#include "mathop.h" +#include + +/** @name PGM parser error codes + ** @{ */ +#define VL_ERR_PGM_INV_HEAD 101 /**< Invalid PGM header section. */ +#define VL_ERR_PGM_INV_META 102 /**< Invalid PGM meta section. */ +#define VL_ERR_PGM_INV_DATA 103 /**< Invalid PGM data section.*/ +#define VL_ERR_PGM_IO 104 /**< Generic I/O error. */ +/** @} */ + +/** @brief PGM image meta data + ** + ** A PGM image is a 2-D array of pixels of width #width and height + ** #height. Each pixel is an integer one or two bytes wide, depending + ** whether #max_value is smaller than 256. + **/ + +typedef struct _VlPgmImage +{ + vl_size width ; /**< image width. */ + vl_size height ; /**< image height. */ + vl_size max_value ; /**< pixel maximum value (<= 2^16-1). */ + vl_bool is_raw ; /**< is RAW format? */ +} VlPgmImage ; + +/** @name Core operations + ** @{ */ +VL_EXPORT int vl_pgm_extract_head (FILE *f, VlPgmImage *im) ; +VL_EXPORT int vl_pgm_extract_data (FILE *f, VlPgmImage const *im, void *data) ; +VL_EXPORT int vl_pgm_insert (FILE *f, + VlPgmImage const *im, + void const*data ) ; +VL_EXPORT vl_size vl_pgm_get_npixels (VlPgmImage const *im) ; +VL_EXPORT vl_size vl_pgm_get_bpp (VlPgmImage const *im) ; +/** @} */ + +/** @name Helper functions + ** @{ */ +VL_EXPORT int vl_pgm_write (char const *name, + vl_uint8 const *data, + int width, int height) ; +VL_EXPORT int vl_pgm_write_f (char const *name, + float const *data, + int width, int height) ; +VL_EXPORT int vl_pgm_read_new (char const *name, + VlPgmImage *im, + vl_uint8 **data) ; +VL_EXPORT int vl_pgm_read_new_f (char const *name, + VlPgmImage *im, + float **data) ; + +/** @} */ + +/* VL_PGM_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/qsort-def.h b/colmap/include/colmap/lib/VLFeat/qsort-def.h new file mode 100644 index 0000000000000000000000000000000000000000..63b61c837b8b4218901b4f49406d8f06c47362f7 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/qsort-def.h @@ -0,0 +1,200 @@ +/** @file qsort-def.h + ** @brief QSort preprocessor metaprogram + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +/** @file qsort-def.h + +@section qsort-def-overview Overview + +@ref qsort-def.h is a metaprogram to define specialized instances +of the quick-sort algorithm. + +@section qsort-def-usage Usage + +@ref qsort-def.h is used to define a specialization of the +::VL_QSORT_sort function that operates +on a given type of array. For instance the code + +@code +#define VL_QSORT_type float +#define VL_QSORT_prefix my_qsort +#include +@endcode + +defines a function @c my_qsort_sort that operates on an array of floats. + +@todo large array compatibility. +**/ + +#include "host.h" +#include + +#ifndef VL_QSORT_prefix +#error "VL_QSORT_prefix must be defined" +#endif + +#ifndef VL_QSORT_array +#ifndef VL_QSORT_type +#error "VL_QSORT_type must be defined if VL_QSORT_array is not" +#endif +#define VL_QSORT_array VL_QSORT_type* +#define VL_QSORT_array_const VL_QSORT_type const* +#endif + +#ifdef __DOXYGEN__ +#define VL_QSORT_prefix QSortPrefix /**< Prefix of the qsort functions */ +#define VL_QSORT_type QSortType /**< Data type of the qsort elements */ +#define VL_QSORT_array QSortType* /**< Data type of the qsort container */ +#endif + +/* ---------------------------------------------------------------- */ + +#if ! defined(VL_QSORT_cmp) || defined(__DOXYGEN__) +#define VL_QSORT_cmp VL_XCAT(VL_QSORT_prefix, _cmp) + +/** @brief Compare two array elements + ** @param array qsort array. + ** @param indexA index of the first element @c A to compare. + ** @param indexB index of the second element @c B to comapre. + ** @return a negative number if @c AB. + **/ + +VL_INLINE VL_QSORT_type +VL_QSORT_cmp +(VL_QSORT_array_const array, + vl_uindex indexA, + vl_uindex indexB) +{ + return array[indexA] - array[indexB] ; +} + +/* VL_QSORT_cmp */ +#endif + +/* ---------------------------------------------------------------- */ + +#if ! defined(VL_QSORT_swap) || defined(__DOXYGEN__) +#define VL_QSORT_swap VL_XCAT(VL_QSORT_prefix, _swap) + +/** @brief Swap two array elements + ** @param array qsort array. + ** @param indexA index of the first element to swap. + ** @param indexB index of the second element to swap. + ** + ** The function swaps the two elements @a a and @ b. The function + ** uses a temporary element of type ::VL_QSORT_type + ** and the copy operator @c =. + **/ + +VL_INLINE void +VL_QSORT_swap +(VL_QSORT_array array, + vl_uindex indexA, + vl_uindex indexB) +{ + VL_QSORT_type t = array [indexA] ; + array [indexA] = array [indexB] ; + array [indexB] = t ; +} + +/* VL_QSORT_swap */ +#endif + +/* ---------------------------------------------------------------- */ +#if ! defined(VL_QSORT_sort_recursive) || defined(__DOXYGEN__) +#define VL_QSORT_sort_recursive VL_XCAT(VL_QSORT_prefix, _sort_recursive) + +/** @brief Sort portion of an array using quicksort + ** @param array (in/out) pointer to the array. + ** @param begin first element of the array portion. + ** @param end last element of the array portion. + ** + ** The function sorts the array using quick-sort. Note that + ** @c begin must be not larger than @c end. + **/ + +VL_INLINE void +VL_QSORT_sort_recursive +(VL_QSORT_array array, vl_uindex begin, vl_uindex end) +{ + vl_uindex pivot = (end + begin) / 2 ; + vl_uindex lowPart, i ; + + assert (begin <= end) ; + + /* swap pivot with last */ + VL_QSORT_swap (array, pivot, end) ; + pivot = end ; + + /* + Now scan from left to right, moving all element smaller + or equal than the pivot to the low part + array[0], array[1], ..., array[lowPart - 1]. + */ + lowPart = begin ; + for (i = begin; i < end ; ++i) { /* one less */ + if (VL_QSORT_cmp (array, i, pivot) <= 0) { + /* array[i] must be moved into the low part */ + VL_QSORT_swap (array, lowPart, i) ; + lowPart ++ ; + } + } + + /* the pivot should also go into the low part */ + VL_QSORT_swap (array, lowPart, pivot) ; + pivot = lowPart ; + + /* do recursion */ + if (pivot > begin) { + /* note that pivot-1 stays non-negative */ + VL_QSORT_sort_recursive (array, begin, pivot - 1) ; + } + if (pivot < end) { + VL_QSORT_sort_recursive (array, pivot + 1, end) ; + } +} + +/* VL_QSORT_sort_recursive */ +#endif + +/* ---------------------------------------------------------------- */ + +#if ! defined(VL_QSORT_sort) || defined(__DOXYGEN__) +#define VL_QSORT_sort VL_XCAT(VL_QSORT_prefix, _sort) + +/** @brief Sort array using quicksort + ** @param array (in/out) pointer to the array. + ** @param size size of the array. + ** + ** The function sorts the array using quick-sort. + **/ + +VL_INLINE void +VL_QSORT_sort +(VL_QSORT_array array, vl_size size) +{ + assert (size >= 1) ; + VL_QSORT_sort_recursive (array, 0, size - 1) ; +} + +/* VL_QSORT_qsort */ +#endif + +#undef VL_QSORT_prefix +#undef VL_QSORT_swap +#undef VL_QSORT_sort +#undef VL_QSORT_sort_recursive +#undef VL_QSORT_type +#undef VL_QSORT_array +#undef VL_QSORT_cmp + diff --git a/colmap/include/colmap/lib/VLFeat/quickshift.h b/colmap/include/colmap/lib/VLFeat/quickshift.h new file mode 100644 index 0000000000000000000000000000000000000000..d7bec8bf61b4e4ef1ed10eea4bfe35a560ed9535 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/quickshift.h @@ -0,0 +1,211 @@ +/** @file quickshift.h + ** @brief Quick shift (@ref quickshift) + ** @author Andrea Vedaldi + ** @author Brian Fulkerson + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_QUICKSHIFT_H +#define VL_QUICKSHIFT_H + +#include "generic.h" +#include "mathop.h" + +/** @brief quick shift datatype */ +typedef double vl_qs_type ; + +/** @brief quick shift infinity constant */ +#define VL_QS_INF VL_INFINITY_D /* Change to _F for float math */ + +/** ------------------------------------------------------------------ + ** @brief quick shift results + ** + ** This implements quick shift mode seeking. + **/ + +typedef struct _VlQS +{ + vl_qs_type *image ; /**< height x width x channels feature image */ + int height; /**< height of the image */ + int width; /**< width of the image */ + int channels; /**< number of channels in the image */ + + vl_bool medoid; + vl_qs_type sigma; + vl_qs_type tau; + + int *parents ; + vl_qs_type *dists ; + vl_qs_type *density ; +} VlQS ; + +/** @name Create and destroy + ** @{ + **/ +VL_EXPORT +VlQS* vl_quickshift_new (vl_qs_type const * im, int height, int width, + int channels); + +VL_EXPORT +void vl_quickshift_delete (VlQS *q) ; +/** @} */ + +/** @name Process data + ** @{ + **/ + +VL_EXPORT +void vl_quickshift_process (VlQS *q) ; + +/** @} */ + +/** @name Retrieve data and parameters + ** @{ + **/ +VL_INLINE vl_qs_type vl_quickshift_get_max_dist (VlQS const *q) ; +VL_INLINE vl_qs_type vl_quickshift_get_kernel_size (VlQS const *q) ; +VL_INLINE vl_bool vl_quickshift_get_medoid (VlQS const *q) ; + +VL_INLINE int * vl_quickshift_get_parents (VlQS const *q) ; +VL_INLINE vl_qs_type * vl_quickshift_get_dists (VlQS const *q) ; +VL_INLINE vl_qs_type * vl_quickshift_get_density (VlQS const *q) ; +/** @} */ + +/** @name Set parameters + ** @{ + **/ +VL_INLINE void vl_quickshift_set_max_dist (VlQS *f, vl_qs_type tau) ; +VL_INLINE void vl_quickshift_set_kernel_size (VlQS *f, vl_qs_type sigma) ; +VL_INLINE void vl_quickshift_set_medoid (VlQS *f, vl_bool medoid) ; +/** @} */ + +/* ------------------------------------------------------------------- + * Inline functions implementation + * ---------------------------------------------------------------- */ + +/** ------------------------------------------------------------------ + ** @brief Get tau. + ** @param q quick shift object. + ** @return the maximum distance in the feature space between nodes in the + ** quick shift tree. + **/ + +VL_INLINE vl_qs_type +vl_quickshift_get_max_dist (VlQS const *q) +{ + return q->tau ; +} + +/** ------------------------------------------------------------------ + ** @brief Get sigma. + ** @param q quick shift object. + ** @return the standard deviation of the kernel used in the Parzen density + ** estimate. + **/ + +VL_INLINE vl_qs_type +vl_quickshift_get_kernel_size (VlQS const *q) +{ + return q->sigma ; +} + +/** ------------------------------------------------------------------ + ** @brief Get medoid. + ** @param q quick Shift object. + ** @return @c true if medoid shift is used instead of quick shift. + **/ + +VL_INLINE vl_bool +vl_quickshift_get_medoid (VlQS const *q) +{ + return q->medoid ; +} + +/** ------------------------------------------------------------------ + ** @brief Get parents. + ** @param q quick shift object. + ** @return a @c height x @c width matrix where each element contains the + ** linear index of its parent node. The node is a root if its + ** value is its own linear index. + **/ + +VL_INLINE int * +vl_quickshift_get_parents (VlQS const *q) +{ + return q->parents ; +} + +/** ------------------------------------------------------------------ + ** @brief Get dists. + ** @param q quick shift object. + ** @return for each pixel, the distance in feature space to the pixel + ** that is its parent in the quick shift tree. The distance is + ** set to 'inf' if the pixel is a root node. + **/ + +VL_INLINE vl_qs_type * +vl_quickshift_get_dists (VlQS const *q) +{ + return q->dists ; +} + +/** ------------------------------------------------------------------ + ** @brief Get density. + ** @param q quick shift object. + ** @return the estimate of the density at each pixel. + **/ + +VL_INLINE vl_qs_type * +vl_quickshift_get_density (VlQS const *q) +{ + return q->density ; +} + +/** ------------------------------------------------------------------ + ** @brief Set sigma + ** @param q quick shift object. + ** @param sigma standard deviation of the kernel used in the Parzen density + ** estimate. + **/ + +VL_INLINE void +vl_quickshift_set_kernel_size (VlQS *q, vl_qs_type sigma) +{ + q -> sigma = sigma ; +} + +/** ------------------------------------------------------------------ + ** @brief Set max distance + ** @param q quick shift object. + ** @param tau the maximum distance in the feature space between nodes in the + ** quick shift tree. + **/ + +VL_INLINE void +vl_quickshift_set_max_dist (VlQS *q, vl_qs_type tau) +{ + q -> tau = tau ; +} + +/** ------------------------------------------------------------------ + ** @brief Set medoid + ** @param q quick shift object. + ** @param medoid @c true to use kernelized medoid shift, @c false (default) uses + ** quick shift. + **/ + +VL_INLINE void +vl_quickshift_set_medoid (VlQS *q, vl_bool medoid) +{ + q -> medoid = medoid ; +} + + +#endif diff --git a/colmap/include/colmap/lib/VLFeat/random.h b/colmap/include/colmap/lib/VLFeat/random.h new file mode 100644 index 0000000000000000000000000000000000000000..f464fc47be3ce76512bfe9519d80e49915c31fa8 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/random.h @@ -0,0 +1,162 @@ +/** @file random.h + ** @brief Random number generator (@ref random) + ** @author Andrea Vedaldi + ** @see @ref random + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_RANDOM_H +#define VL_RANDOM_H + +#include "host.h" + +/** @brief Random numbber generator state */ +typedef struct _VlRand { + vl_uint32 mt [624] ; + vl_uint32 mti ; +} VlRand ; + +/** @name Setting and reading the state + ** + ** @{ */ +VL_EXPORT void vl_rand_init (VlRand * self) ; +VL_EXPORT void vl_rand_seed (VlRand * self, vl_uint32 s) ; +VL_EXPORT void vl_rand_seed_by_array (VlRand * self, + vl_uint32 const key [], + vl_size keySize) ; +/** @} */ + +/** @name Generate random numbers + ** + ** @{ */ +VL_INLINE vl_uint64 vl_rand_uint64 (VlRand * self) ; +VL_INLINE vl_int64 vl_rand_int63 (VlRand * self) ; +VL_EXPORT vl_uint32 vl_rand_uint32 (VlRand * self) ; +VL_INLINE vl_int32 vl_rand_int31 (VlRand * self) ; +VL_INLINE double vl_rand_real1 (VlRand * self) ; +VL_INLINE double vl_rand_real2 (VlRand * self) ; +VL_INLINE double vl_rand_real3 (VlRand * self) ; +VL_INLINE double vl_rand_res53 (VlRand * self) ; +VL_INLINE vl_uindex vl_rand_uindex (VlRand * self, vl_uindex range) ; +/** @} */ + +VL_EXPORT void vl_rand_permute_indexes (VlRand * self, vl_index* array, vl_size size) ; + +/* ---------------------------------------------------------------- */ + +/** @brief Generate a random index in a given range + ** @param self random number generator. + ** @param range range. + ** @return an index sampled uniformly at random in the interval [0, @c range - 1] + ** + ** @remark Currently, this function uses a simple algorithm that + ** may yield slightly biased samples if @c range is not a power of + ** two. + **/ + +VL_INLINE vl_uindex +vl_rand_uindex (VlRand * self, vl_uindex range) +{ + if (range <= 0xffffffff) { + /* 32-bit version */ + return (vl_rand_uint32 (self) % (vl_uint32)range) ; + } else { + /* 64-bit version */ + return (vl_rand_uint64 (self) % range) ; + } +} + +/** @brief Generate a random UINT64 + ** @param self random number generator. + ** @return a random number in [0, 0xffffffffffffffff]. + **/ + +VL_INLINE vl_uint64 +vl_rand_uint64 (VlRand * self) +{ + vl_uint64 a = vl_rand_uint32 (self) ; + vl_uint64 b = vl_rand_uint32 (self) ; + return (a << 32) | b ; +} + +/** @brief Generate a random INT63 + ** @param self random number generator. + ** @return a random number in [0, 0x7fffffffffffffff]. + **/ + +VL_INLINE vl_int64 +vl_rand_int63 (VlRand * self) +{ + return (vl_int64)(vl_rand_uint64 (self) >> 1) ; +} + +/** @brief Generate a random INT31 + ** @param self random number generator. + ** @return a random number in [0, 0x7fffffff]. + **/ + +VL_INLINE vl_int32 +vl_rand_int31 (VlRand * self) +{ + return (vl_int32)(vl_rand_uint32 (self) >> 1) ; +} + +/** @brief Generate a random number in [0,1] + ** @param self random number generator. + ** @return a random number. + **/ + +VL_INLINE double +vl_rand_real1 (VlRand * self) +{ + return vl_rand_uint32(self)*(1.0/4294967295.0); + /* divided by 2^32-1 */ +} + +/** @brief Generate a random number in [0,1) + ** @param self random number generator. + ** @return a random number. + **/ + +VL_INLINE double +vl_rand_real2 (VlRand * self) +{ + return vl_rand_uint32(self)*(1.0/4294967296.0); + /* divided by 2^32 */ +} + +/** @brief Generate a random number in (0,1) + ** @param self random number generator. + ** @return a random number. + **/ + +VL_INLINE double +vl_rand_real3 (VlRand * self) +{ + return (((double)vl_rand_uint32(self)) + 0.5)*(1.0/4294967296.0); + /* divided by 2^32 */ +} + +/** @brief Generate a random number in [0,1) with 53-bit resolution + ** @param self random number generator. + ** @return a random number. + **/ + +VL_INLINE double +vl_rand_res53 (VlRand * self) +{ + vl_uint32 + a = vl_rand_uint32(self) >> 5, + b = vl_rand_uint32(self) >> 6 ; + return (a * 67108864.0 + b) * (1.0 / 9007199254740992.0) ; +} + +/* VL_RANDOM_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/rodrigues.h b/colmap/include/colmap/lib/VLFeat/rodrigues.h new file mode 100644 index 0000000000000000000000000000000000000000..d752725ba9983772220bd14dfe8ff06abacd4ec2 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/rodrigues.h @@ -0,0 +1,33 @@ +/** @file rodrigues.h + ** @brief Rodrigues formulas + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-13 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +/** @file rodrigues.h + +@section rodrigues Rodrigues formulas + +- Use ::vl_rodrigues to compute the Rodrigues formula and its derivative. +- Use ::vl_irodrigues to compute the inverse Rodrigues formula and + its derivative. + +**/ + +#ifndef VL_RODRIGUES +#define VL_RODRIGUES + +#include "generic.h" + +VL_EXPORT void vl_rodrigues (double* R_pt, double* dR_pt, const double* om_pt) ; +VL_EXPORT void vl_irodrigues (double* om_pt, double* dom_pt, const double* R_pt) ; + +/* VL_RODRIGUES */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/scalespace.h b/colmap/include/colmap/lib/VLFeat/scalespace.h new file mode 100644 index 0000000000000000000000000000000000000000..3f7af38d89c1d028aaa8d7a337a5c09adebb6317 --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/scalespace.h @@ -0,0 +1,99 @@ +/** @file scalespace.h + ** @brief Scale Space (@ref scalespace) + ** @author Andrea Vedaldi + ** @author Karel Lenc + ** @author Michal Perdoch + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_SCALESPACE_H +#define VL_SCALESPACE_H + +#include "generic.h" +#include "imopv.h" +#include "mathop.h" + +/* ---------------------------------------------------------------- */ +/* VlScaleSpaceGeometry */ +/* ---------------------------------------------------------------- */ + +/** @brief Geometry of a scale space + ** + ** There are a few restrictions on the valid geometrties. + */ +typedef struct _VlScaleSpaceGeometry +{ + vl_size width ; /**< Image width */ + vl_size height ; /**< Image height */ + vl_index firstOctave ; /**< Index of the fisrt octave */ + vl_index lastOctave ; /**< Index of the last octave */ + vl_size octaveResolution ; /**< Number of octave subdivisions */ + vl_index octaveFirstSubdivision ; /**< Index of the first octave subdivision */ + vl_index octaveLastSubdivision ; /**< Index of the last octave subdivision */ + double baseScale ; /**< Base smoothing (smoothing of octave 0, level 0) */ + double nominalScale ; /**< Nominal smoothing of the original image */ +} VlScaleSpaceGeometry ; + +VL_EXPORT +vl_bool vl_scalespacegeometry_is_equal (VlScaleSpaceGeometry a, + VlScaleSpaceGeometry b) ; + +/* ---------------------------------------------------------------- */ +/* VlScaleSpaceOctaveGeometry */ +/* ---------------------------------------------------------------- */ + +/** @brief Geometry of one octave of a scale space */ +typedef struct _VlScaleSpaceOctaveGeometry +{ + vl_size width ; /**< Width (number of pixels) */ + vl_size height ; /**< Height (number of pixels) */ + double step ; /**< Sampling step (size of a pixel) */ +} VlScaleSpaceOctaveGeometry ; + +/* ---------------------------------------------------------------- */ +/* VlScaleSpace */ +/* ---------------------------------------------------------------- */ + +typedef struct _VlScaleSpace VlScaleSpace ; + +/** @name Create and destroy + ** @{ + **/ +VL_EXPORT VlScaleSpaceGeometry vl_scalespace_get_default_geometry(vl_size width, vl_size height) ; +VL_EXPORT VlScaleSpace * vl_scalespace_new (vl_size width, vl_size height) ; +VL_EXPORT VlScaleSpace * vl_scalespace_new_with_geometry (VlScaleSpaceGeometry geom) ; +VL_EXPORT VlScaleSpace * vl_scalespace_new_copy (VlScaleSpace* src); +VL_EXPORT VlScaleSpace * vl_scalespace_new_shallow_copy (VlScaleSpace* src); +VL_EXPORT void vl_scalespace_delete (VlScaleSpace *self) ; +/** @} */ + +/** @name Process data + ** @{ + **/ +VL_EXPORT void +vl_scalespace_put_image (VlScaleSpace *self, float const* image); +/** @} */ + +/** @name Retrieve data and parameters + ** @{ + **/ +VL_EXPORT VlScaleSpaceGeometry vl_scalespace_get_geometry (VlScaleSpace const * self) ; +VL_EXPORT VlScaleSpaceOctaveGeometry vl_scalespace_get_octave_geometry (VlScaleSpace const * self, vl_index o) ; +VL_EXPORT float * +vl_scalespace_get_level (VlScaleSpace * self, vl_index o, vl_index s) ; +VL_EXPORT float const * +vl_scalespace_get_level_const (VlScaleSpace const * self, vl_index o, vl_index s) ; +VL_EXPORT double +vl_scalespace_get_level_sigma (VlScaleSpace const *self, vl_index o, vl_index s) ; +/** @} */ + +/* VL_SCALESPACE_H */ +#endif + diff --git a/colmap/include/colmap/lib/VLFeat/shuffle-def.h b/colmap/include/colmap/lib/VLFeat/shuffle-def.h new file mode 100644 index 0000000000000000000000000000000000000000..4b74ea6bdd397a397d792ae9671a23a055da51ba --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/shuffle-def.h @@ -0,0 +1,101 @@ +/** @file shuffle-def.h + ** @brief Shuffle preprocessor metaprogram + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +/** @file shuffle-def.h + + @todo large array compatibility. + **/ + +#include "host.h" +#include "random.h" +#include + +#ifndef VL_SHUFFLE_prefix +#error "VL_SHUFFLE_prefix must be defined" +#endif + +#ifndef VL_SHUFFLE_array +#ifndef VL_SHUFFLE_type +#error "VL_SHUFFLE_type must be defined if VL_SHUFFLE_array is not" +#endif +#define VL_SHUFFLE_array VL_SHUFFLE_type* +#endif + +#ifdef __DOXYGEN__ +#define VL_SHUFFLE_prefix ShufflePrefix /**< Prefix of the shuffle functions */ +#define VL_SHUFFLE_type ShuffleType /**< Data type of the shuffle elements */ +#define VL_SHUFFLE_array ShuffleType* /**< Data type of the shuffle container */ +#endif + +/* ---------------------------------------------------------------- */ + +#if ! defined(VL_SHUFFLE_swap) || defined(__DOXYGEN__) +#define VL_SHUFFLE_swap VL_XCAT(VL_SHUFFLE_prefix, _swap) + +/** @brief Swap two array elements + ** @param array shuffle array. + ** @param indexA index of the first element to swap. + ** @param indexB index of the second element to swap. + ** + ** The function swaps the two elements @a a and @ b. The function + ** uses a temporary element of type ::VL_SHUFFLE_type + ** and the copy operator @c =. + **/ + +VL_INLINE void +VL_SHUFFLE_swap +(VL_SHUFFLE_array array, + vl_uindex indexA, + vl_uindex indexB) +{ + VL_SHUFFLE_type t = array [indexA] ; + array [indexA] = array [indexB] ; + array [indexB] = t ; +} + +/* VL_SHUFFLE_swap */ +#endif + +/* ---------------------------------------------------------------- */ + +#if ! defined(VL_SHUFFLE_shuffle) || defined(__DOXYGEN__) +#define VL_SHUFFLE_shuffle VL_XCAT(VL_SHUFFLE_prefix, _shuffle) + +/** @brief Shuffle + ** @param array (in/out) pointer to the array. + ** @param size size of the array. + ** @param rand random number generator to use. + ** + ** The function randomly permutes the array. + **/ + +VL_INLINE void +VL_SHUFFLE_shuffle +(VL_SHUFFLE_array array, vl_size size, VlRand * rand) +{ + vl_uindex n = size ; + while (n > 1) { + vl_uindex k = vl_rand_uindex (rand, n) ; + n -- ; + VL_SHUFFLE_swap (array, n, k) ; + } +} + +/* VL_SHUFFLE_shuffle */ +#endif + +#undef VL_SHUFFLE_prefix +#undef VL_SHUFFLE_swap +#undef VL_SHUFFLE_shuffle +#undef VL_SHUFFLE_type +#undef VL_SHUFFLE_array diff --git a/colmap/include/colmap/lib/VLFeat/sift.h b/colmap/include/colmap/lib/VLFeat/sift.h new file mode 100644 index 0000000000000000000000000000000000000000..50e03f434718b31b39f42960c6047921bdc92cfe --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/sift.h @@ -0,0 +1,418 @@ +/** @file sift.h + ** @brief SIFT (@ref sift) + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_SIFT_H +#define VL_SIFT_H + +#include +#include "generic.h" + +/** @brief SIFT filter pixel type */ +typedef float vl_sift_pix ; + +/** ------------------------------------------------------------------ + ** @brief SIFT filter keypoint + ** + ** This structure represent a keypoint as extracted by the SIFT + ** filter ::VlSiftFilt. + **/ + +typedef struct _VlSiftKeypoint +{ + int o ; /**< o coordinate (octave). */ + + int ix ; /**< Integer unnormalized x coordinate. */ + int iy ; /**< Integer unnormalized y coordinate. */ + int is ; /**< Integer s coordinate. */ + + float x ; /**< x coordinate. */ + float y ; /**< y coordinate. */ + float s ; /**< s coordinate. */ + float sigma ; /**< scale. */ +} VlSiftKeypoint ; + +/** ------------------------------------------------------------------ + ** @brief SIFT filter + ** + ** This filter implements the SIFT detector and descriptor. + **/ + +typedef struct _VlSiftFilt +{ + double sigman ; /**< nominal image smoothing. */ + double sigma0 ; /**< smoothing of pyramid base. */ + double sigmak ; /**< k-smoothing */ + double dsigma0 ; /**< delta-smoothing. */ + + int width ; /**< image width. */ + int height ; /**< image height. */ + int O ; /**< number of octaves. */ + int S ; /**< number of levels per octave. */ + int o_min ; /**< minimum octave index. */ + int s_min ; /**< minimum level index. */ + int s_max ; /**< maximum level index. */ + int o_cur ; /**< current octave. */ + + vl_sift_pix *temp ; /**< temporary pixel buffer. */ + vl_sift_pix *octave ; /**< current GSS data. */ + vl_sift_pix *dog ; /**< current DoG data. */ + int octave_width ; /**< current octave width. */ + int octave_height ; /**< current octave height. */ + + vl_sift_pix *gaussFilter ; /**< current Gaussian filter */ + double gaussFilterSigma ; /**< current Gaussian filter std */ + vl_size gaussFilterWidth ; /**< current Gaussian filter width */ + + VlSiftKeypoint* keys ;/**< detected keypoints. */ + int nkeys ; /**< number of detected keypoints. */ + int keys_res ; /**< size of the keys buffer. */ + + double peak_thresh ; /**< peak threshold. */ + double edge_thresh ; /**< edge threshold. */ + double norm_thresh ; /**< norm threshold. */ + double magnif ; /**< magnification factor. */ + double windowSize ; /**< size of Gaussian window (in spatial bins) */ + + vl_sift_pix *grad ; /**< GSS gradient data. */ + int grad_o ; /**< GSS gradient data octave. */ + +} VlSiftFilt ; + +/** @name Create and destroy + ** @{ + **/ +VL_EXPORT +VlSiftFilt* vl_sift_new (int width, int height, + int noctaves, int nlevels, + int o_min) ; +VL_EXPORT +void vl_sift_delete (VlSiftFilt *f) ; +/** @} */ + +/** @name Process data + ** @{ + **/ + +VL_EXPORT +int vl_sift_process_first_octave (VlSiftFilt *f, + vl_sift_pix const *im) ; + +VL_EXPORT +int vl_sift_process_next_octave (VlSiftFilt *f) ; + +VL_EXPORT +void vl_sift_detect (VlSiftFilt *f) ; + +VL_EXPORT +int vl_sift_calc_keypoint_orientations (VlSiftFilt *f, + double angles [4], + VlSiftKeypoint const*k); +VL_EXPORT +void vl_sift_calc_keypoint_descriptor (VlSiftFilt *f, + vl_sift_pix *descr, + VlSiftKeypoint const* k, + double angle) ; + +VL_EXPORT +void vl_sift_calc_raw_descriptor (VlSiftFilt const *f, + vl_sift_pix const* image, + vl_sift_pix *descr, + int widht, int height, + double x, double y, + double s, double angle0) ; + +VL_EXPORT +void vl_sift_keypoint_init (VlSiftFilt const *f, + VlSiftKeypoint *k, + double x, + double y, + double sigma) ; +/** @} */ + +/** @name Retrieve data and parameters + ** @{ + **/ +VL_INLINE int vl_sift_get_octave_index (VlSiftFilt const *f) ; +VL_INLINE int vl_sift_get_noctaves (VlSiftFilt const *f) ; +VL_INLINE int vl_sift_get_octave_first (VlSiftFilt const *f) ; +VL_INLINE int vl_sift_get_octave_width (VlSiftFilt const *f) ; +VL_INLINE int vl_sift_get_octave_height (VlSiftFilt const *f) ; +VL_INLINE int vl_sift_get_nlevels (VlSiftFilt const *f) ; +VL_INLINE int vl_sift_get_nkeypoints (VlSiftFilt const *f) ; +VL_INLINE double vl_sift_get_peak_thresh (VlSiftFilt const *f) ; +VL_INLINE double vl_sift_get_edge_thresh (VlSiftFilt const *f) ; +VL_INLINE double vl_sift_get_norm_thresh (VlSiftFilt const *f) ; +VL_INLINE double vl_sift_get_magnif (VlSiftFilt const *f) ; +VL_INLINE double vl_sift_get_window_size (VlSiftFilt const *f) ; + +VL_INLINE vl_sift_pix *vl_sift_get_octave (VlSiftFilt const *f, int s) ; +VL_INLINE VlSiftKeypoint const *vl_sift_get_keypoints (VlSiftFilt const *f) ; +/** @} */ + +/** @name Set parameters + ** @{ + **/ +VL_INLINE void vl_sift_set_peak_thresh (VlSiftFilt *f, double t) ; +VL_INLINE void vl_sift_set_edge_thresh (VlSiftFilt *f, double t) ; +VL_INLINE void vl_sift_set_norm_thresh (VlSiftFilt *f, double t) ; +VL_INLINE void vl_sift_set_magnif (VlSiftFilt *f, double m) ; +VL_INLINE void vl_sift_set_window_size (VlSiftFilt *f, double m) ; +/** @} */ + +/* ------------------------------------------------------------------- + * Inline functions implementation + * ---------------------------------------------------------------- */ + +/** ------------------------------------------------------------------ + ** @brief Get current octave index. + ** @param f SIFT filter. + ** @return index of the current octave. + **/ + +VL_INLINE int +vl_sift_get_octave_index (VlSiftFilt const *f) +{ + return f-> o_cur ; +} + +/** ------------------------------------------------------------------ + ** @brief Get number of octaves. + ** @param f SIFT filter. + ** @return number of octaves. + **/ + +VL_INLINE int +vl_sift_get_noctaves (VlSiftFilt const *f) +{ + return f-> O ; +} + +/**------------------------------------------------------------------- + ** @brief Get first octave. + ** @param f SIFT filter. + ** @return index of the first octave. + **/ + +VL_INLINE int +vl_sift_get_octave_first (VlSiftFilt const *f) +{ + return f-> o_min ; +} + +/** ------------------------------------------------------------------ + ** @brief Get current octave width + ** @param f SIFT filter. + ** @return current octave width. + **/ + +VL_INLINE int +vl_sift_get_octave_width (VlSiftFilt const *f) +{ + return f-> octave_width ; +} + +/** ------------------------------------------------------------------ + ** @brief Get current octave height + ** @param f SIFT filter. + ** @return current octave height. + **/ + +VL_INLINE int +vl_sift_get_octave_height (VlSiftFilt const *f) +{ + return f-> octave_height ; +} + +/** ------------------------------------------------------------------ + ** @brief Get current octave data + ** @param f SIFT filter. + ** @param s level index. + ** + ** The level index @a s ranges in the interval s_min = -1 + ** and s_max = S + 2, where @c S is the number of levels + ** per octave. + ** + ** @return pointer to the octave data for level @a s. + **/ + +VL_INLINE vl_sift_pix * +vl_sift_get_octave (VlSiftFilt const *f, int s) +{ + int w = vl_sift_get_octave_width (f) ; + int h = vl_sift_get_octave_height (f) ; + return f->octave + w * h * (s - f->s_min) ; +} + +/** ------------------------------------------------------------------ + ** @brief Get number of levels per octave + ** @param f SIFT filter. + ** @return number of leves per octave. + **/ + +VL_INLINE int +vl_sift_get_nlevels (VlSiftFilt const *f) +{ + return f-> S ; +} + +/** ------------------------------------------------------------------ + ** @brief Get number of keypoints. + ** @param f SIFT filter. + ** @return number of keypoints. + **/ + +VL_INLINE int +vl_sift_get_nkeypoints (VlSiftFilt const *f) +{ + return f-> nkeys ; +} + +/** ------------------------------------------------------------------ + ** @brief Get keypoints. + ** @param f SIFT filter. + ** @return pointer to the keypoints list. + **/ + +VL_INLINE VlSiftKeypoint const * +vl_sift_get_keypoints (VlSiftFilt const *f) +{ + return f-> keys ; +} + +/** ------------------------------------------------------------------ + ** @brief Get peaks treashold + ** @param f SIFT filter. + ** @return threshold ; + **/ + +VL_INLINE double +vl_sift_get_peak_thresh (VlSiftFilt const *f) +{ + return f -> peak_thresh ; +} + +/** ------------------------------------------------------------------ + ** @brief Get edges threshold + ** @param f SIFT filter. + ** @return threshold. + **/ + +VL_INLINE double +vl_sift_get_edge_thresh (VlSiftFilt const *f) +{ + return f -> edge_thresh ; +} + +/** ------------------------------------------------------------------ + ** @brief Get norm threshold + ** @param f SIFT filter. + ** @return threshold. + **/ + +VL_INLINE double +vl_sift_get_norm_thresh (VlSiftFilt const *f) +{ + return f -> norm_thresh ; +} + +/** ------------------------------------------------------------------ + ** @brief Get the magnification factor + ** @param f SIFT filter. + ** @return magnification factor. + **/ + +VL_INLINE double +vl_sift_get_magnif (VlSiftFilt const *f) +{ + return f -> magnif ; +} + +/** ------------------------------------------------------------------ + ** @brief Get the Gaussian window size. + ** @param f SIFT filter. + ** @return standard deviation of the Gaussian window (in spatial bin units). + **/ + +VL_INLINE double +vl_sift_get_window_size (VlSiftFilt const *f) +{ + return f -> windowSize ; +} + + + +/** ------------------------------------------------------------------ + ** @brief Set peaks threshold + ** @param f SIFT filter. + ** @param t threshold. + **/ + +VL_INLINE void +vl_sift_set_peak_thresh (VlSiftFilt *f, double t) +{ + f -> peak_thresh = t ; +} + +/** ------------------------------------------------------------------ + ** @brief Set edges threshold + ** @param f SIFT filter. + ** @param t threshold. + **/ + +VL_INLINE void +vl_sift_set_edge_thresh (VlSiftFilt *f, double t) +{ + f -> edge_thresh = t ; +} + +/** ------------------------------------------------------------------ + ** @brief Set norm threshold + ** @param f SIFT filter. + ** @param t threshold. + **/ + +VL_INLINE void +vl_sift_set_norm_thresh (VlSiftFilt *f, double t) +{ + f -> norm_thresh = t ; +} + +/** ------------------------------------------------------------------ + ** @brief Set the magnification factor + ** @param f SIFT filter. + ** @param m magnification factor. + **/ + +VL_INLINE void +vl_sift_set_magnif (VlSiftFilt *f, double m) +{ + f -> magnif = m ; +} + +/** ------------------------------------------------------------------ + ** @brief Set the Gaussian window size + ** @param f SIFT filter. + ** @param x Gaussian window size (in units of spatial bin). + ** + ** This is the parameter @f$ \hat \sigma_{\text{win}} @f$ of + ** the standard SIFT descriptor @ref sift-tech-descriptor-std. + **/ + +VL_INLINE void +vl_sift_set_window_size (VlSiftFilt *f, double x) +{ + f -> windowSize = x ; +} + +/* VL_SIFT_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/slic.h b/colmap/include/colmap/lib/VLFeat/slic.h new file mode 100644 index 0000000000000000000000000000000000000000..1bffcdecf665654d32342c0710d71e22f7d515cf --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/slic.h @@ -0,0 +1,30 @@ +/** @file slic.h + ** @brief SLIC superpixels (@ref slic) + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_SLIC_H +#define VL_SLIC_H + +#include "generic.h" + +VL_EXPORT void +vl_slic_segment (vl_uint32 * segmentation, + float const * image, + vl_size width, + vl_size height, + vl_size numChannels, + vl_size regionSize, + float regularization, + vl_size minRegionSize) ; + +/* VL_SLIC_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/stringop.h b/colmap/include/colmap/lib/VLFeat/stringop.h new file mode 100644 index 0000000000000000000000000000000000000000..f5f6a524ffc9edf7cdc4a7f21a3e140a1741338e --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/stringop.h @@ -0,0 +1,58 @@ +/** @file stringop.h + ** @brief String operations + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_STRINGOP_H +#define VL_STRINGOP_H + +#include "generic.h" + +/** @brief File protocols */ +enum { + VL_PROT_UNKNOWN = -1, /**< unknown protocol */ + VL_PROT_NONE = 0, /**< no protocol */ + VL_PROT_ASCII, /**< ASCII protocol */ + VL_PROT_BINARY /**< Binary protocol */ +} ; + + +VL_EXPORT vl_size vl_string_copy (char *destination, vl_size destinationSize, char const *source) ; +VL_EXPORT vl_size vl_string_copy_sub (char *destination, vl_size destinationSize, + char const *beginning, char const *end) ; +VL_EXPORT char *vl_string_parse_protocol (char const *string, int *protocol) ; +VL_EXPORT char const *vl_string_protocol_name (int prot) ; +VL_EXPORT vl_size vl_string_basename (char *destination, vl_size destinationSize, + char const *source, vl_size maxNumStrippedExtension) ; +VL_EXPORT vl_size vl_string_replace_wildcard (char * destination, vl_size destinationSize, + char const *src, char wildcardChar, char escapeChar, + char const *replacement) ; +VL_EXPORT char *vl_string_find_char_rev (char const *beginning, char const *end, char c) ; +VL_EXPORT vl_size vl_string_length (char const *string) ; +VL_EXPORT int vl_string_casei_cmp (const char *string1, const char *string2) ; + +/** @name String enumerations + ** @{ */ + +/** @brief Member of an enumeration */ +typedef struct _VlEnumerator +{ + char const *name ; /**< enumeration member name. */ + vl_index value ; /**< enumeration member value. */ +} VlEnumerator ; + +VL_EXPORT VlEnumerator *vl_enumeration_get (VlEnumerator const *enumeration, char const *name) ; +VL_EXPORT VlEnumerator *vl_enumeration_get_casei (VlEnumerator const *enumeration, char const *name) ; +VL_EXPORT VlEnumerator *vl_enumeration_get_by_value (VlEnumerator const *enumeration, vl_index value) ; +/** @} */ + +/* VL_STRINGOP_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/svm.h b/colmap/include/colmap/lib/VLFeat/svm.h new file mode 100644 index 0000000000000000000000000000000000000000..1fcc6d4749b800d8d3c626b0751a48493f2c8d3c --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/svm.h @@ -0,0 +1,195 @@ +/** @file svm.h + ** @brief Support Vector Machines (@ref svm) + ** @author Milan Sulc + ** @author Daniele Perrone + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2013 Milan Sulc. +Copyright (C) 2012 Daniele Perrone. +Copyright (C) 2011-13 Andrea Vedaldi. + +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_SVM_H +#define VL_SVM_H + +#include "generic.h" +#include "svmdataset.h" + +/** @typedef VlSvm + ** @brief SVM solver. + ** This object implements VLFeat SVM solvers (see @ref svm.h). + **/ + +#ifndef __DOXYGEN__ +struct VlSvm_ ; +typedef struct VlSvm_ VlSvm ; +#else +typedef OPAQUE VlSvm ; +#endif + +/** @brief Type of SVM solver */ +typedef enum +{ + VlSvmSolverNone = 0, /**< No solver (used to evaluate an SVM). */ + VlSvmSolverSgd = 1, /**< SGD algorithm (@ref svm-sgd). */ + VlSvmSolverSdca /**< SDCA algorithm (@ref svm-sdca). */ +} VlSvmSolverType ; + +/** @brief Type of SVM loss + ** + ** Default SVM loss types. The loss can be set by using ::vl_svm_set_loss. + ** Note that custom losses can be used too by using ::vl_svm_set_loss_function, + ** ::vl_svm_set_loss_derivative_function, etc. + ** + ** @sa svm-loss-functions + **/ +typedef enum +{ + VlSvmLossHinge = 0, /**< Standard hinge loss. */ + VlSvmLossHinge2 = 1, /**< Hinge loss squared. */ + VlSvmLossL1, /**< L1 loss. */ + VlSvmLossL2, /**< L2 loss. */ + VlSvmLossLogistic /**< Logistic loss. */ +} VlSvmLossType ; + +/** @brief Solver status */ +typedef enum +{ + VlSvmStatusTraining = 1, /**< Optimization in progress. */ + VlSvmStatusConverged, /**< Optimization finished because the convergence criterion was met. */ + VlSvmStatusMaxNumIterationsReached /**< Optimization finished without convergence. */ +} VlSvmSolverStatus ; + +/** @brief SVM statistics + ** This structure contains statistics characterising the state of + ** the SVM solver, such as the current value of the objective function. + ** + ** Not all fields are used by all solvers. + **/ +typedef struct VlSvmStatistics_ { + VlSvmSolverStatus status ; /**< Solver status. */ + vl_size iteration ; /**< Solver iteration. */ + vl_size epoch ; /**< Solver epoch (iteration / num samples). */ + double objective ; /**< Objective function value. */ + double regularizer ; /**< Regularizer value. */ + double loss ; /**< Loss value. */ + double dualObjective ; /**< Dual objective value. */ + double dualLoss ; /**< Dual loss value. */ + double dualityGap ; /**< Duality gap = objective - dualObjective. */ + double scoresVariation ; /**< Variance of the score updates. */ + double elapsedTime ; /**< Time elapsed from the start of training. */ +} VlSvmStatistics ; + +/** @name Create and destroy + ** @{ */ +VL_EXPORT VlSvm * vl_svm_new (VlSvmSolverType type, + double const * data, + vl_size dimension, + vl_size numData, + double const * labels, + double lambda) ; + +VL_EXPORT VlSvm * vl_svm_new_with_dataset (VlSvmSolverType type, + VlSvmDataset * dataset, + double const * labels, + double lambda) ; + +VL_EXPORT VlSvm * vl_svm_new_with_abstract_data (VlSvmSolverType type, + void * data, + vl_size dimension, + vl_size numData, + double const * labels, + double lambda) ; + +VL_EXPORT void vl_svm_delete (VlSvm * self) ; +/** @} */ + +/** @name Retrieve parameters and data + ** @{ */ +VL_EXPORT VlSvmStatistics const * vl_svm_get_statistics (VlSvm const *self) ; +VL_EXPORT double const * vl_svm_get_model (VlSvm const *self) ; +VL_EXPORT double vl_svm_get_bias (VlSvm const *self) ; +VL_EXPORT vl_size vl_svm_get_dimension (VlSvm *self) ; +VL_EXPORT vl_size vl_svm_get_num_data (VlSvm *self) ; +VL_EXPORT double vl_svm_get_epsilon (VlSvm const *self) ; +VL_EXPORT double vl_svm_get_bias_learning_rate (VlSvm const *self) ; +VL_EXPORT vl_size vl_svm_get_max_num_iterations (VlSvm const *self) ; +VL_EXPORT vl_size vl_svm_get_diagnostic_frequency (VlSvm const *self) ; +VL_EXPORT VlSvmSolverType vl_svm_get_solver (VlSvm const *self) ; +VL_EXPORT double vl_svm_get_bias_multiplier (VlSvm const *self) ; +VL_EXPORT double vl_svm_get_lambda (VlSvm const *self) ; +VL_EXPORT vl_size vl_svm_get_iteration_number (VlSvm const *self) ; +VL_EXPORT double const * vl_svm_get_scores (VlSvm const *self) ; +VL_EXPORT double const * vl_svm_get_weights (VlSvm const *self) ; +/** @} */ + +/** @name Set parameters + ** @{ */ +VL_EXPORT void vl_svm_set_epsilon (VlSvm *self, double epsilon) ; +VL_EXPORT void vl_svm_set_bias_learning_rate (VlSvm *self, double rate) ; +VL_EXPORT void vl_svm_set_max_num_iterations (VlSvm *self, vl_size maxNumIterations) ; +VL_EXPORT void vl_svm_set_diagnostic_frequency (VlSvm *self, vl_size f) ; +VL_EXPORT void vl_svm_set_bias_multiplier (VlSvm *self, double b) ; +VL_EXPORT void vl_svm_set_model (VlSvm *self, double const *model) ; +VL_EXPORT void vl_svm_set_bias (VlSvm *self, double b) ; +VL_EXPORT void vl_svm_set_iteration_number (VlSvm *self, vl_uindex n) ; +VL_EXPORT void vl_svm_set_weights (VlSvm *self, double const *weights) ; + +VL_EXPORT void vl_svm_set_diagnostic_function (VlSvm *self, VlSvmDiagnosticFunction f, void *data) ; +VL_EXPORT void vl_svm_set_loss_function (VlSvm *self, VlSvmLossFunction f) ; +VL_EXPORT void vl_svm_set_loss_derivative_function (VlSvm *self, VlSvmLossFunction f) ; +VL_EXPORT void vl_svm_set_conjugate_loss_function (VlSvm *self, VlSvmLossFunction f) ; +VL_EXPORT void vl_svm_set_dca_update_function (VlSvm *self, VlSvmDcaUpdateFunction f) ; +VL_EXPORT void vl_svm_set_data_functions (VlSvm *self, VlSvmInnerProductFunction inner, VlSvmAccumulateFunction acc) ; +VL_EXPORT void vl_svm_set_loss (VlSvm *self, VlSvmLossType loss) ; +/** @} */ + +/** @name Process data + ** @{ */ +VL_EXPORT void vl_svm_train (VlSvm * self) ; +/** @} */ + +/** @name Loss functions + ** @sa @ref svm-advanced + ** @{ */ + +/* hinge */ +VL_EXPORT double vl_svm_hinge_loss (double label, double inner) ; +VL_EXPORT double vl_svm_hinge_loss_derivative (double label, double inner) ; +VL_EXPORT double vl_svm_hinge_conjugate_loss (double label, double u) ; +VL_EXPORT double vl_svm_hinge_dca_update (double alpha, double inner, double norm2, double label) ; + +/* square hinge */ +VL_EXPORT double vl_svm_hinge2_loss (double label, double inner) ; +VL_EXPORT double vl_svm_hinge2_loss_derivative (double label, double inner) ; +VL_EXPORT double vl_svm_hinge2_conjugate_loss (double label, double u) ; +VL_EXPORT double vl_svm_hinge2_dca_update (double alpha, double inner, double norm2, double label) ; + +/* l1 */ +VL_EXPORT double vl_svm_l1_loss (double label, double inner) ; +VL_EXPORT double vl_svm_l1_loss_derivative (double label, double inner) ; +VL_EXPORT double vl_svm_l1_conjugate_loss (double label, double u) ; +VL_EXPORT double vl_svm_l1_dca_update (double alpha, double inner, double norm2, double label) ; + +/* l2 */ +VL_EXPORT double vl_svm_l2_loss (double label, double inner) ; +VL_EXPORT double vl_svm_l2_loss_derivative (double label, double inner) ; +VL_EXPORT double vl_svm_l2_conjugate_loss (double label, double u) ; +VL_EXPORT double vl_svm_l2_dca_update (double alpha, double inner, double norm2, double label) ; + +/* logistic */ +VL_EXPORT double vl_svm_logistic_loss (double label, double inner) ; +VL_EXPORT double vl_svm_logistic_loss_derivative (double label, double inner) ; +VL_EXPORT double vl_svm_logistic_conjugate_loss (double label, double u) ; +VL_EXPORT double vl_svm_logistic_dca_update (double alpha, double inner, double norm2, double label) ; +/** } */ + +/* VL_SVM_H */ +#endif diff --git a/colmap/include/colmap/lib/VLFeat/svmdataset.h b/colmap/include/colmap/lib/VLFeat/svmdataset.h new file mode 100644 index 0000000000000000000000000000000000000000..c2e1c62042df68d5fc735acff342bfa32ac6159f --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/svmdataset.h @@ -0,0 +1,82 @@ +/** @file svmdataset.h + ** @brief SVM Dataset + ** @author Daniele Perrone + ** @author Andrea Vedaldi + **/ + +/* +Copyright (C) 2012 Daniele Perrone. +Copyright (C) 2013 Andrea Vedaldi. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_SVMDATASET_H +#define VL_SVMDATASET_H + +#include "generic.h" +#include "homkermap.h" + +struct VlSvm_ ; + +/** @typedef VlSvmDataset + ** @brief SVM dataset object + ** + ** This objects contain a training set to be used in combination with + ** the SVM solver object ::VlSvm. Its main purpose is to implement + ** the two basic operations inner product (::VlSvmInnerProductFunction) + ** and accumulation (::VlSvmAccumulateFunction). + ** + ** See @ref svm and @ref svm-advanced for further information. + **/ + +#ifndef __DOXYGEN__ +struct VlSvmDataset_ ; +typedef struct VlSvmDataset_ VlSvmDataset ; +#else +typedef OPAQUE VlSvmDataset ; +#endif + +/** @name SVM callbacks + ** @{ */ +typedef void (*VlSvmDiagnosticFunction) (struct VlSvm_ *svm, void *data) ; +typedef double (*VlSvmLossFunction) (double inner, double label) ; +typedef double (*VlSvmDcaUpdateFunction) (double alpha, double inner, double norm2, double label) ; +typedef double (*VlSvmInnerProductFunction)(const void *data, vl_uindex element, double *model) ; +typedef void (*VlSvmAccumulateFunction) (const void *data, vl_uindex element, double *model, double multiplier) ; +/* typedef double (*VlSvmSquareNormFunction) (const void *data, vl_uindex element) ; */ +/** @} */ + +/** @name Create and destroy + ** @{ + **/ +VL_EXPORT VlSvmDataset* vl_svmdataset_new (vl_type dataType, void *data, vl_size dimension, vl_size numData) ; +VL_EXPORT void vl_svmdataset_delete (VlSvmDataset * dataset) ; +/** @} */ + +/** @name Set parameters + ** @{ + **/ +VL_EXPORT void vl_svmdataset_set_homogeneous_kernel_map (VlSvmDataset * self, + VlHomogeneousKernelMap * hom) ; +/** @} */ + +/** @name Get data and parameters + ** @{ + **/ +VL_EXPORT void* vl_svmdataset_get_data (VlSvmDataset const *self) ; +VL_EXPORT vl_size vl_svmdataset_get_num_data (VlSvmDataset const *self) ; +VL_EXPORT vl_size vl_svmdataset_get_dimension (VlSvmDataset const *self) ; +VL_EXPORT void* vl_svmdataset_get_map (VlSvmDataset const *self) ; +VL_EXPORT vl_size vl_svmdataset_get_mapDim (VlSvmDataset const *self) ; +VL_EXPORT VlSvmAccumulateFunction vl_svmdataset_get_accumulate_function (VlSvmDataset const *self) ; +VL_EXPORT VlSvmInnerProductFunction vl_svmdataset_get_inner_product_function (VlSvmDataset const * self) ; +VL_EXPORT VlHomogeneousKernelMap * vl_svmdataset_get_homogeneous_kernel_map (VlSvmDataset const * self) ; +/** @} */ + +/* VL_SVMDATASET_H */ +#endif + + diff --git a/colmap/include/colmap/lib/VLFeat/vlad.h b/colmap/include/colmap/lib/VLFeat/vlad.h new file mode 100644 index 0000000000000000000000000000000000000000..72ca6dc930bdb38b6a21df57e2b035c7077e46eb --- /dev/null +++ b/colmap/include/colmap/lib/VLFeat/vlad.h @@ -0,0 +1,53 @@ +/** @file vlad.h + ** @brief VLAD encoding (@ref vlad) + ** @author David Novotny + ** @author Andrea Vedaldi + ** @see @ref vlad + **/ + +/* +Copyright (C) 2013 David Novotny and Andera Vedaldi. +All rights reserved. + +This file is part of the VLFeat library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#ifndef VL_VLAD_H +#define VL_VLAD_H + +#include "generic.h" + +/** @name VLAD options + ** @{ */ +#define VL_VLAD_FLAG_NORMALIZE_COMPONENTS (0x1 << 0) +#define VL_VLAD_FLAG_SQUARE_ROOT (0x1 << 1) +#define VL_VLAD_FLAG_UNNORMALIZED (0x1 << 2) +#define VL_VLAD_FLAG_NORMALIZE_MASS (0x1 << 3) + +/** @def VL_VLAD_FLAG_NORMALIZE_COMPONENTS + ** @brief Normalize each VLAD component individually. + **/ + +/** @def VL_VLAD_FLAG_SQUARE_ROOT + ** @brief Use signed squared-root. + **/ + +/** @def VL_VLAD_FLAG_UNNORMALIZED + ** @brief Do not globally normalize the VLAD descriptor. + **/ + +/** @def VL_VLAD_FLAG_NORMALIZE_MASS + ** @brief Normalize each component by the number of features assigned to it. + **/ +/** @} */ + +VL_EXPORT void vl_vlad_encode + (void * enc, vl_type dataType, + void const * means, vl_size dimension, vl_size numClusters, + void const * data, vl_size numData, + void const * assignments, + int flags) ; + +/* VL_VLAD_H */ +#endif diff --git a/colmap/include/colmap/mvs/consistency_graph.h b/colmap/include/colmap/mvs/consistency_graph.h new file mode 100644 index 0000000000000000000000000000000000000000..c01c824333489a7bbf1d040ddd7f2903b3ad1bd7 --- /dev/null +++ b/colmap/include/colmap/mvs/consistency_graph.h @@ -0,0 +1,79 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_CONSISTENCY_GRAPH_H_ +#define COLMAP_SRC_MVS_CONSISTENCY_GRAPH_H_ + +#include +#include + +#include + +#include "util/types.h" + +namespace colmap { +namespace mvs { + +// List of geometrically consistent images, in the following format: +// +// r_1, c_1, N_1, i_11, i_12, ..., i_1N_1, +// r_2, c_2, N_2, i_21, i_22, ..., i_2N_2, ... +// +// where r, c are the row and column image coordinates of the pixel, +// N is the number of consistent images, followed by the N image indices. +// Note that only pixels are listed which are not filtered and that the +// consistency graph is only filled if filtering is enabled. +class ConsistencyGraph { + public: + ConsistencyGraph(); + ConsistencyGraph(const size_t width, const size_t height, + const std::vector& data); + + size_t GetNumBytes() const; + + void GetImageIdxs(const int row, const int col, int* num_images, + const int** image_idxs) const; + + void Read(const std::string& path); + void Write(const std::string& path) const; + + private: + void InitializeMap(const size_t width, const size_t height); + + const static int kNoConsistentImageIds; + std::vector data_; + Eigen::MatrixXi map_; +}; + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_CONSISTENCY_GRAPH_H_ diff --git a/colmap/include/colmap/mvs/cuda_array_wrapper.h b/colmap/include/colmap/mvs/cuda_array_wrapper.h new file mode 100644 index 0000000000000000000000000000000000000000..e4e48b0e884f5dfd08c518a335df8cbabf4644ed --- /dev/null +++ b/colmap/include/colmap/mvs/cuda_array_wrapper.h @@ -0,0 +1,171 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_CUDA_ARRAY_WRAPPER_H_ +#define COLMAP_SRC_MVS_CUDA_ARRAY_WRAPPER_H_ + +#include + +#include + +#include "mvs/gpu_mat.h" +#include "util/cudacc.h" + +namespace colmap { +namespace mvs { + +template +class CudaArrayWrapper { + public: + CudaArrayWrapper(const size_t width, const size_t height, const size_t depth); + ~CudaArrayWrapper(); + + const cudaArray* GetPtr() const; + cudaArray* GetPtr(); + + size_t GetWidth() const; + size_t GetHeight() const; + size_t GetDepth() const; + + void CopyToDevice(const T* data); + void CopyToHost(const T* data); + void CopyFromGpuMat(const GpuMat& array); + + private: + // Define class as non-copyable and non-movable. + CudaArrayWrapper(CudaArrayWrapper const&) = delete; + void operator=(CudaArrayWrapper const& obj) = delete; + CudaArrayWrapper(CudaArrayWrapper&&) = delete; + + void Allocate(); + void Deallocate(); + + cudaArray* array_; + + size_t width_; + size_t height_; + size_t depth_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +CudaArrayWrapper::CudaArrayWrapper(const size_t width, const size_t height, + const size_t depth) + : width_(width), height_(height), depth_(depth), array_(nullptr) {} + +template +CudaArrayWrapper::~CudaArrayWrapper() { + Deallocate(); +} + +template +const cudaArray* CudaArrayWrapper::GetPtr() const { + return array_; +} + +template +cudaArray* CudaArrayWrapper::GetPtr() { + return array_; +} + +template +size_t CudaArrayWrapper::GetWidth() const { + return width_; +} + +template +size_t CudaArrayWrapper::GetHeight() const { + return height_; +} + +template +size_t CudaArrayWrapper::GetDepth() const { + return depth_; +} + +template +void CudaArrayWrapper::CopyToDevice(const T* data) { + cudaMemcpy3DParms params = {0}; + Allocate(); + params.extent = make_cudaExtent(width_, height_, depth_); + params.kind = cudaMemcpyHostToDevice; + params.dstArray = array_; + params.srcPtr = + make_cudaPitchedPtr((void*)data, width_ * sizeof(T), width_, height_); + CUDA_SAFE_CALL(cudaMemcpy3D(¶ms)); +} + +template +void CudaArrayWrapper::CopyToHost(const T* data) { + cudaMemcpy3DParms params = {0}; + params.extent = make_cudaExtent(width_, height_, depth_); + params.kind = cudaMemcpyDeviceToHost; + params.dstPtr = + make_cudaPitchedPtr((void*)data, width_ * sizeof(T), width_, height_); + params.srcArray = array_; + CUDA_SAFE_CALL(cudaMemcpy3D(¶ms)); +} + +template +void CudaArrayWrapper::CopyFromGpuMat(const GpuMat& array) { + Allocate(); + cudaMemcpy3DParms parameters = {0}; + parameters.extent = make_cudaExtent(width_, height_, depth_); + parameters.kind = cudaMemcpyDeviceToDevice; + parameters.dstArray = array_; + parameters.srcPtr = make_cudaPitchedPtr((void*)array.GetPtr(), + array.GetPitch(), width_, height_); + CUDA_SAFE_CALL(cudaMemcpy3D(¶meters)); +} + +template +void CudaArrayWrapper::Allocate() { + Deallocate(); + struct cudaExtent extent = make_cudaExtent(width_, height_, depth_); + cudaChannelFormatDesc fmt = cudaCreateChannelDesc(); + CUDA_SAFE_CALL(cudaMalloc3DArray(&array_, &fmt, extent, cudaArrayLayered)); +} + +template +void CudaArrayWrapper::Deallocate() { + if (array_ != nullptr) { + CUDA_SAFE_CALL(cudaFreeArray(array_)); + array_ = nullptr; + } +} + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_CUDA_ARRAY_WRAPPER_H_ diff --git a/colmap/include/colmap/mvs/cuda_flip.h b/colmap/include/colmap/mvs/cuda_flip.h new file mode 100644 index 0000000000000000000000000000000000000000..00302097ac9496fd93222017eaae918301ea876b --- /dev/null +++ b/colmap/include/colmap/mvs/cuda_flip.h @@ -0,0 +1,113 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_CUDA_FLIP_H_ +#define COLMAP_SRC_MVS_CUDA_FLIP_H_ + +#include + +namespace colmap { +namespace mvs { + +// Flip the input matrix horizontally. +template +void CudaFlipHorizontal(const T* input, T* output, const int width, + const int height, const int pitch_input, + const int pitch_output); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +#ifdef __CUDACC__ + +// TILE_DIM_FLIP must divide by BLOCK_ROWS. Do not change these values. +#define TILE_DIM_FLIP 32 +#define BLOCK_ROWS_FLIP 8 + +namespace internal { + +template +__global__ void CudaFlipHorizontalKernel(T* output_data, const T* input_data, + const int width, const int height, + const int input_pitch, + const int output_pitch) { + int x_index = blockIdx.x * TILE_DIM_FLIP + threadIdx.x; + const int y_index = blockIdx.y * TILE_DIM_FLIP + threadIdx.y; + + __shared__ T tile[TILE_DIM_FLIP][TILE_DIM_FLIP + 1]; + const int tile_x = min(threadIdx.x, width - 1 - blockIdx.x * TILE_DIM_FLIP); + const int tile_y = min(threadIdx.y, height - 1 - blockIdx.y * TILE_DIM_FLIP); + + for (int i = 0; i < TILE_DIM_FLIP; i += BLOCK_ROWS_FLIP) { + const int x = min(x_index, width - 1); + const int y = min(y_index, height - i - 1); + tile[tile_y + i][tile_x] = + *((T*)((char*)input_data + y * input_pitch + i * input_pitch) + x); + } + + __syncthreads(); + + x_index = width - 1 - (blockIdx.x * TILE_DIM_FLIP + threadIdx.x); + if (x_index < width) { + for (int i = 0; i < TILE_DIM_FLIP; i += BLOCK_ROWS_FLIP) { + if (y_index + i < height) { + *((T*)((char*)output_data + y_index * output_pitch + i * output_pitch) + + x_index) = tile[threadIdx.y + i][threadIdx.x]; + } + } + } +} + +} // namespace internal + +template +void CudaFlipHorizontal(const T* input, T* output, const int width, + const int height, const int pitch_input, + const int pitch_output) { + dim3 block_dim(TILE_DIM_FLIP, BLOCK_ROWS_FLIP, 1); + dim3 grid_dim; + grid_dim.x = (width - 1) / TILE_DIM_FLIP + 1; + grid_dim.y = (height - 1) / TILE_DIM_FLIP + 1; + + internal::CudaFlipHorizontalKernel<<>>( + output, input, width, height, pitch_input, pitch_output); +} + +#undef TILE_DIM_FLIP +#undef BLOCK_ROWS_FLIP + +#endif // __CUDACC__ + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_CUDA_FLIP_H_ diff --git a/colmap/include/colmap/mvs/cuda_rotate.h b/colmap/include/colmap/mvs/cuda_rotate.h new file mode 100644 index 0000000000000000000000000000000000000000..c85198b72612d9c001e42804caa05177cd27f280 --- /dev/null +++ b/colmap/include/colmap/mvs/cuda_rotate.h @@ -0,0 +1,95 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_CUDA_ROTATE_H_ +#define COLMAP_SRC_MVS_CUDA_ROTATE_H_ + +#include + +namespace colmap { +namespace mvs { + +// Rotate the input matrix by 90 degrees in counter-clockwise direction. +template +void CudaRotate(const T* input, T* output, const int width, const int height, + const int pitch_input, const int pitch_output); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +#ifdef __CUDACC__ + +#define TILE_DIM_ROTATE 32 + +namespace internal { + +template +__global__ void CudaRotateKernel(T* output_data, const T* input_data, + const int width, const int height, + const int input_pitch, + const int output_pitch) { + int input_x = blockDim.x * blockIdx.x + threadIdx.x; + int input_y = blockDim.y * blockIdx.y + threadIdx.y; + + if (input_x >= width || input_y >= height) { + return; + } + + int output_x = input_y; + int output_y = width - 1 - input_x; + + *((T*)((char*)output_data + output_y * output_pitch) + output_x) = + *((T*)((char*)input_data + input_y * input_pitch) + input_x); +} + +} // namespace internal + +template +void CudaRotate(const T* input, T* output, const int width, const int height, + const int pitch_input, const int pitch_output) { + dim3 block_dim(TILE_DIM_ROTATE, 1, 1); + dim3 grid_dim; + grid_dim.x = (width - 1) / TILE_DIM_ROTATE + 1; + grid_dim.y = height; + + internal::CudaRotateKernel<<>>( + output, input, width, height, pitch_input, pitch_output); +} + +#undef TILE_DIM_ROTATE + +#endif // __CUDACC__ + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_CUDA_ROTATE_H_ diff --git a/colmap/include/colmap/mvs/cuda_transpose.h b/colmap/include/colmap/mvs/cuda_transpose.h new file mode 100644 index 0000000000000000000000000000000000000000..ab9ad0506a6340c66fa84023266d1e57b6375502 --- /dev/null +++ b/colmap/include/colmap/mvs/cuda_transpose.h @@ -0,0 +1,114 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_CUDA_TRANSPOSE_H_ +#define COLMAP_SRC_MVS_CUDA_TRANSPOSE_H_ + +#include + +namespace colmap { +namespace mvs { + +// Transpose the input matrix. +template +void CudaTranspose(const T* input, T* output, const int width, const int height, + const int pitch_input, const int pitch_output); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +#ifdef __CUDACC__ + +// TILE_DIM_TRANSPOSE must divide by BLOCK_ROWS. Do not change these values. +#define TILE_DIM_TRANSPOSE 32 +#define BLOCK_ROWS_TRANSPOSE 8 + +namespace internal { + +template +__global__ void CudaTransposeKernel(T* output_data, const T* input_data, + const int width, const int height, + const int input_pitch, + const int output_pitch) { + int x_index = blockIdx.x * TILE_DIM_TRANSPOSE + threadIdx.x; + int y_index = blockIdx.y * TILE_DIM_TRANSPOSE + threadIdx.y; + + __shared__ T tile[TILE_DIM_TRANSPOSE][TILE_DIM_TRANSPOSE + 1]; + const int tile_x = + min(threadIdx.x, width - 1 - blockIdx.x * TILE_DIM_TRANSPOSE); + const int tile_y = + min(threadIdx.y, height - 1 - blockIdx.y * TILE_DIM_TRANSPOSE); + + for (int i = 0; i < TILE_DIM_TRANSPOSE; i += BLOCK_ROWS_TRANSPOSE) { + const int x = min(x_index, width - 1); + const int y = min(y_index, height - i - 1); + tile[tile_y + i][tile_x] = + *((T*)((char*)input_data + y * input_pitch + i * input_pitch) + x); + } + + __syncthreads(); + + x_index = blockIdx.y * TILE_DIM_TRANSPOSE + threadIdx.x; + if (x_index < height) { + y_index = blockIdx.x * TILE_DIM_TRANSPOSE + threadIdx.y; + for (int i = 0; i < TILE_DIM_TRANSPOSE; i += BLOCK_ROWS_TRANSPOSE) { + if (y_index + i < width) { + *((T*)((char*)output_data + y_index * output_pitch + i * output_pitch) + + x_index) = tile[threadIdx.x][threadIdx.y + i]; + } + } + } +} + +} // namespace internal + +template +void CudaTranspose(const T* input, T* output, const int width, const int height, + const int pitch_input, const int pitch_output) { + dim3 block_dim(TILE_DIM_TRANSPOSE, BLOCK_ROWS_TRANSPOSE, 1); + dim3 grid_dim; + grid_dim.x = (width - 1) / TILE_DIM_TRANSPOSE + 1; + grid_dim.y = (height - 1) / TILE_DIM_TRANSPOSE + 1; + + internal::CudaTransposeKernel<<>>( + output, input, width, height, pitch_input, pitch_output); +} + +#undef TILE_DIM_TRANSPOSE +#undef BLOCK_ROWS_TRANSPOSE + +#endif // __CUDACC__ + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_CUDA_TRANSPOSE_H_ diff --git a/colmap/include/colmap/mvs/depth_map.h b/colmap/include/colmap/mvs/depth_map.h new file mode 100644 index 0000000000000000000000000000000000000000..fc5b0201090792ace965f989fa9268e16d9b7dab --- /dev/null +++ b/colmap/include/colmap/mvs/depth_map.h @@ -0,0 +1,81 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_DEPTH_MAP_H_ +#define COLMAP_SRC_MVS_DEPTH_MAP_H_ + +#include +#include + +#include "mvs/mat.h" +#include "util/bitmap.h" + +namespace colmap { +namespace mvs { + +class DepthMap : public Mat { + public: + DepthMap(); + DepthMap(const size_t width, const size_t height, const float depth_min, + const float depth_max); + DepthMap(const Mat& mat, const float depth_min, const float depth_max); + + inline float GetDepthMin() const; + inline float GetDepthMax() const; + + inline float Get(const size_t row, const size_t col) const; + + void Rescale(const float factor); + void Downsize(const size_t max_width, const size_t max_height); + + Bitmap ToBitmap(const float min_percentile, const float max_percentile) const; + + private: + float depth_min_ = -1.0f; + float depth_max_ = -1.0f; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +float DepthMap::GetDepthMin() const { return depth_min_; } + +float DepthMap::GetDepthMax() const { return depth_max_; } + +float DepthMap::Get(const size_t row, const size_t col) const { + return data_.at(row * width_ + col); +} + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_DEPTH_MAP_H_ diff --git a/colmap/include/colmap/mvs/fusion.h b/colmap/include/colmap/mvs/fusion.h new file mode 100644 index 0000000000000000000000000000000000000000..1203bea3b828eaf89a4cf732dbd16c972a65a75e --- /dev/null +++ b/colmap/include/colmap/mvs/fusion.h @@ -0,0 +1,190 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_FUSION_H_ +#define COLMAP_SRC_MVS_FUSION_H_ + +#include +#include +#include + +#include + +#include "mvs/depth_map.h" +#include "mvs/image.h" +#include "mvs/mat.h" +#include "mvs/model.h" +#include "mvs/normal_map.h" +#include "mvs/workspace.h" +#include "util/alignment.h" +#include "util/cache.h" +#include "util/math.h" +#include "util/ply.h" +#include "util/threading.h" + +namespace colmap { +namespace mvs { + +struct StereoFusionOptions { + // Path for PNG masks. Same format expected as ImageReaderOptions. + std::string mask_path = ""; + + // The number of threads to use during fusion. + int num_threads = -1; + + // Maximum image size in either dimension. + int max_image_size = -1; + + // Minimum number of fused pixels to produce a point. + int min_num_pixels = 5; + + // Maximum number of pixels to fuse into a single point. + int max_num_pixels = 10000; + + // Maximum depth in consistency graph traversal. + int max_traversal_depth = 100; + + // Maximum relative difference between measured and projected pixel. + double max_reproj_error = 2.0f; + + // Maximum relative difference between measured and projected depth. + double max_depth_error = 0.01f; + + // Maximum angular difference in degrees of normals of pixels to be fused. + double max_normal_error = 10.0f; + + // Number of overlapping images to transitively check for fusing points. + int check_num_images = 50; + + // Flag indicating whether to use LRU cache or pre-load all data + bool use_cache = false; + + // Cache size in gigabytes for fusion. The fusion keeps the bitmaps, depth + // maps, normal maps, and consistency graphs of this number of images in + // memory. A higher value leads to less disk access and faster fusion, while + // a lower value leads to reduced memory usage. Note that a single image can + // consume a lot of memory, if the consistency graph is dense. + double cache_size = 32.0; + + std::pair bounding_box = + std::make_pair(Eigen::Vector3f(-FLT_MAX, -FLT_MAX, -FLT_MAX), + Eigen::Vector3f(FLT_MAX, FLT_MAX, FLT_MAX)); + + // Check the options for validity. + bool Check() const; + + // Print the options to stdout. + void Print() const; +}; + +class StereoFusion : public Thread { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + StereoFusion(const StereoFusionOptions& options, + const std::string& workspace_path, + const std::string& workspace_format, + const std::string& pmvs_option_name, + const std::string& input_type); + + const std::vector& GetFusedPoints() const; + const std::vector>& GetFusedPointsVisibility() const; + + private: + void Run(); + void InitFusedPixelMask(int image_idx, size_t width, size_t height); + void Fuse(const int thread_id, const int image_idx, const int row, + const int col); + + const StereoFusionOptions options_; + const std::string workspace_path_; + const std::string workspace_format_; + const std::string pmvs_option_name_; + const std::string input_type_; + const float max_squared_reproj_error_; + const float min_cos_normal_error_; + + std::unique_ptr workspace_; + std::vector used_images_; + std::vector fused_images_; + std::vector> overlapping_images_; + // Contains image masks of pre-masked and already fused pixels. + // Initialized from image masks if provided in StereoFusionOptions. + std::vector> fused_pixel_masks_; + std::vector> depth_map_sizes_; + std::vector> bitmap_scales_; + std::vector> P_; + std::vector> inv_P_; + std::vector> inv_R_; + + struct FusionData { + int image_idx = kInvalidImageId; + int row = 0; + int col = 0; + int traversal_depth = -1; + FusionData(int image_idx, int row, int col, int traversal_depth) + : image_idx(image_idx), + row(row), + col(col), + traversal_depth(traversal_depth) {} + bool operator()(const FusionData& data1, const FusionData& data2) { + return data1.image_idx > data2.image_idx; + } + }; + + // Already fused points. + std::vector fused_points_; + std::vector> fused_points_visibility_; + + std::vector> task_fused_points_; + std::vector>> task_fused_points_visibility_; +}; + +// Write the visiblity information into a binary file of the following format: +// +// +// +// ... +// +// ... +// ... +// +// Note that an image_idx in the case of the mvs::StereoFuser does not +// correspond to the image_id of a Reconstruction, but the index of the image in +// the mvs::Model, which is the location of the image in the images.bin/.txt. +void WritePointsVisibility( + const std::string& path, + const std::vector>& points_visibility); + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_FUSION_H_ diff --git a/colmap/include/colmap/mvs/gpu_mat.h b/colmap/include/colmap/mvs/gpu_mat.h new file mode 100644 index 0000000000000000000000000000000000000000..7c0edb9086ba146d185e1104b4f08042faa5e123 --- /dev/null +++ b/colmap/include/colmap/mvs/gpu_mat.h @@ -0,0 +1,427 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_GPU_MAT_H_ +#define COLMAP_SRC_MVS_GPU_MAT_H_ + +#include +#include +#include +#include + +#include +#include + +#include "mvs/cuda_flip.h" +#include "mvs/cuda_rotate.h" +#include "mvs/cuda_transpose.h" +#include "mvs/mat.h" +#include "util/cuda.h" +#include "util/cudacc.h" +#include "util/endian.h" + +namespace colmap { +namespace mvs { + +template +class GpuMat { + public: + GpuMat(const size_t width, const size_t height, const size_t depth = 1); + ~GpuMat(); + + __host__ __device__ const T* GetPtr() const; + __host__ __device__ T* GetPtr(); + + __host__ __device__ size_t GetPitch() const; + __host__ __device__ size_t GetWidth() const; + __host__ __device__ size_t GetHeight() const; + __host__ __device__ size_t GetDepth() const; + + __device__ T Get(const size_t row, const size_t col, + const size_t slice = 0) const; + __device__ void GetSlice(const size_t row, const size_t col, T* values) const; + + __device__ T& GetRef(const size_t row, const size_t col); + __device__ T& GetRef(const size_t row, const size_t col, const size_t slice); + + __device__ void Set(const size_t row, const size_t col, const T value); + __device__ void Set(const size_t row, const size_t col, const size_t slice, + const T value); + __device__ void SetSlice(const size_t row, const size_t col, const T* values); + + void FillWithScalar(const T value); + void FillWithVector(const T* values); + void FillWithRandomNumbers(const T min_value, const T max_value, + GpuMat random_state); + + void CopyToDevice(const T* data, const size_t pitch); + void CopyToHost(T* data, const size_t pitch) const; + Mat CopyToMat() const; + + // Transpose array by swapping x and y coordinates. + void Transpose(GpuMat* output); + + // Flip array along vertical axis. + void FlipHorizontal(GpuMat* output); + + // Rotate array in counter-clockwise direction. + void Rotate(GpuMat* output); + + void Read(const std::string& path); + void Write(const std::string& path); + void Write(const std::string& path, const size_t slice); + + protected: + void ComputeCudaConfig(); + + const static size_t kBlockDimX = 32; + const static size_t kBlockDimY = 16; + + std::shared_ptr array_; + T* array_ptr_; + + size_t pitch_; + size_t width_; + size_t height_; + size_t depth_; + + dim3 blockSize_; + dim3 gridSize_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +#ifdef __CUDACC__ + +namespace internal { + +template +__global__ void FillWithScalarKernel(GpuMat output, const T value) { + const size_t row = blockIdx.y * blockDim.y + threadIdx.y; + const size_t col = blockIdx.x * blockDim.x + threadIdx.x; + if (row < output.GetHeight() && col < output.GetWidth()) { + for (size_t slice = 0; slice < output.GetDepth(); ++slice) { + output.Set(row, col, slice, value); + } + } +} + +template +__global__ void FillWithVectorKernel(const T* values, GpuMat output) { + const size_t row = blockIdx.y * blockDim.y + threadIdx.y; + const size_t col = blockIdx.x * blockDim.x + threadIdx.x; + if (row < output.GetHeight() && col < output.GetWidth()) { + for (size_t slice = 0; slice < output.GetDepth(); ++slice) { + output.Set(row, col, slice, values[slice]); + } + } +} + +template +__global__ void FillWithRandomNumbersKernel(GpuMat output, + GpuMat random_state, + const T min_value, + const T max_value) { + const size_t row = blockIdx.y * blockDim.y + threadIdx.y; + const size_t col = blockIdx.x * blockDim.x + threadIdx.x; + if (row < output.GetHeight() && col < output.GetWidth()) { + curandState local_state = random_state.Get(row, col); + for (size_t slice = 0; slice < output.GetDepth(); ++slice) { + const T random_value = + curand_uniform(&local_state) * (max_value - min_value) + min_value; + output.Set(row, col, slice, random_value); + } + random_state.Set(row, col, local_state); + } +} + +} // namespace internal + +template +GpuMat::GpuMat(const size_t width, const size_t height, const size_t depth) + : array_(nullptr), + array_ptr_(nullptr), + width_(width), + height_(height), + depth_(depth) { + CUDA_SAFE_CALL(cudaMallocPitch((void**)&array_ptr_, &pitch_, + width_ * sizeof(T), height_ * depth_)); + + array_ = std::shared_ptr(array_ptr_, cudaFree); + + ComputeCudaConfig(); +} + +template +GpuMat::~GpuMat() { + array_.reset(); + array_ptr_ = nullptr; + pitch_ = 0; + width_ = 0; + height_ = 0; + depth_ = 0; +} + +template +__host__ __device__ const T* GpuMat::GetPtr() const { + return array_ptr_; +} + +template +__host__ __device__ T* GpuMat::GetPtr() { + return array_ptr_; +} + +template +__host__ __device__ size_t GpuMat::GetPitch() const { + return pitch_; +} + +template +__host__ __device__ size_t GpuMat::GetWidth() const { + return width_; +} + +template +__host__ __device__ size_t GpuMat::GetHeight() const { + return height_; +} + +template +__host__ __device__ size_t GpuMat::GetDepth() const { + return depth_; +} + +template +__device__ T GpuMat::Get(const size_t row, const size_t col, + const size_t slice) const { + return *((T*)((char*)array_ptr_ + pitch_ * (slice * height_ + row)) + col); +} + +template +__device__ void GpuMat::GetSlice(const size_t row, const size_t col, + T* values) const { + for (size_t slice = 0; slice < depth_; ++slice) { + values[slice] = Get(row, col, slice); + } +} + +template +__device__ T& GpuMat::GetRef(const size_t row, const size_t col) { + return GetRef(row, col, 0); +} + +template +__device__ T& GpuMat::GetRef(const size_t row, const size_t col, + const size_t slice) { + return *((T*)((char*)array_ptr_ + pitch_ * (slice * height_ + row)) + col); +} + +template +__device__ void GpuMat::Set(const size_t row, const size_t col, + const T value) { + Set(row, col, 0, value); +} + +template +__device__ void GpuMat::Set(const size_t row, const size_t col, + const size_t slice, const T value) { + *((T*)((char*)array_ptr_ + pitch_ * (slice * height_ + row)) + col) = value; +} + +template +__device__ void GpuMat::SetSlice(const size_t row, const size_t col, + const T* values) { + for (size_t slice = 0; slice < depth_; ++slice) { + Set(row, col, slice, values[slice]); + } +} + +template +void GpuMat::FillWithScalar(const T value) { + internal::FillWithScalarKernel<<>>(*this, value); + CUDA_SYNC_AND_CHECK(); +} + +template +void GpuMat::FillWithVector(const T* values) { + T* values_device; + CUDA_SAFE_CALL(cudaMalloc((void**)&values_device, depth_ * sizeof(T))); + CUDA_SAFE_CALL(cudaMemcpy(values_device, values, depth_ * sizeof(T), + cudaMemcpyHostToDevice)); + internal::FillWithVectorKernel + <<>>(values_device, *this); + CUDA_SYNC_AND_CHECK(); + CUDA_SAFE_CALL(cudaFree(values_device)); +} + +template +void GpuMat::FillWithRandomNumbers(const T min_value, const T max_value, + const GpuMat random_state) { + internal::FillWithRandomNumbersKernel + <<>>(*this, random_state, min_value, max_value); + CUDA_SYNC_AND_CHECK(); +} + +template +void GpuMat::CopyToDevice(const T* data, const size_t pitch) { + CUDA_SAFE_CALL(cudaMemcpy2D((void*)array_ptr_, (size_t)pitch_, (void*)data, + pitch, width_ * sizeof(T), height_ * depth_, + cudaMemcpyHostToDevice)); +} + +template +void GpuMat::CopyToHost(T* data, const size_t pitch) const { + CUDA_SAFE_CALL(cudaMemcpy2D((void*)data, pitch, (void*)array_ptr_, + (size_t)pitch_, width_ * sizeof(T), + height_ * depth_, cudaMemcpyDeviceToHost)); +} + +template +Mat GpuMat::CopyToMat() const { + Mat mat(width_, height_, depth_); + CopyToHost(mat.GetPtr(), mat.GetWidth() * sizeof(T)); + return mat; +} + +template +void GpuMat::Transpose(GpuMat* output) { + for (size_t slice = 0; slice < depth_; ++slice) { + CudaTranspose(array_ptr_ + slice * pitch_ / sizeof(T) * GetHeight(), + output->GetPtr() + + slice * output->pitch_ / sizeof(T) * output->GetHeight(), + width_, height_, pitch_, output->pitch_); + } + CUDA_SYNC_AND_CHECK(); +} + +template +void GpuMat::FlipHorizontal(GpuMat* output) { + for (size_t slice = 0; slice < depth_; ++slice) { + CudaFlipHorizontal(array_ptr_ + slice * pitch_ / sizeof(T) * GetHeight(), + output->GetPtr() + slice * output->pitch_ / sizeof(T) * + output->GetHeight(), + width_, height_, pitch_, output->pitch_); + } + CUDA_SYNC_AND_CHECK(); +} + +template +void GpuMat::Rotate(GpuMat* output) { + for (size_t slice = 0; slice < depth_; ++slice) { + CudaRotate((T*)((char*)array_ptr_ + slice * pitch_ * GetHeight()), + (T*)((char*)output->GetPtr() + + slice * output->pitch_ * output->GetHeight()), + width_, height_, pitch_, output->pitch_); + } + CUDA_SYNC_AND_CHECK(); + // This is equivalent to the following code: + // GpuMat flipped_array(width_, height_, GetDepth()); + // FlipHorizontal(&flipped_array); + // flipped_array.Transpose(output); +} + +template +void GpuMat::Read(const std::string& path) { + std::fstream text_file(path, std::ios::in | std::ios::binary); + CHECK(text_file.is_open()) << path; + + size_t width; + size_t height; + size_t depth; + char unused_char; + text_file >> width >> unused_char >> height >> unused_char >> depth >> + unused_char; + std::streampos pos = text_file.tellg(); + text_file.close(); + + std::fstream binary_file(path, std::ios::in | std::ios::binary); + binary_file.seekg(pos); + + std::vector source(width_ * height_ * depth_); + ReadBinaryLittleEndian(&binary_file, &source); + binary_file.close(); + + CopyToDevice(source.data(), width_ * sizeof(T)); +} + +template +void GpuMat::Write(const std::string& path) { + std::vector dest(width_ * height_ * depth_); + CopyToHost(dest.data(), width_ * sizeof(T)); + + std::fstream text_file(path, std::ios::out); + text_file << width_ << "&" << height_ << "&" << depth_ << "&"; + text_file.close(); + + std::fstream binary_file(path, + std::ios::out | std::ios::binary | std::ios::app); + WriteBinaryLittleEndian(&binary_file, dest); + binary_file.close(); +} + +template +void GpuMat::Write(const std::string& path, const size_t slice) { + std::vector dest(width_ * height_); + CUDA_SAFE_CALL(cudaMemcpy2D( + (void*)dest.data(), width_ * sizeof(T), + (void*)(array_ptr_ + slice * height_ * pitch_ / sizeof(T)), pitch_, + width_ * sizeof(T), height_, cudaMemcpyDeviceToHost)); + + std::fstream text_file(path, std::ios::out); + text_file << width_ << "&" << height_ << "&" << 1 << "&"; + text_file.close(); + + std::fstream binary_file(path, + std::ios::out | std::ios::binary | std::ios::app); + WriteBinaryLittleEndian(&binary_file, dest); + binary_file.close(); +} + +template +void GpuMat::ComputeCudaConfig() { + blockSize_.x = kBlockDimX; + blockSize_.y = kBlockDimY; + blockSize_.z = 1; + + gridSize_.x = (width_ - 1) / kBlockDimX + 1; + gridSize_.y = (height_ - 1) / kBlockDimY + 1; + gridSize_.z = 1; +} + +#endif // __CUDACC__ + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_GPU_MAT_H_ diff --git a/colmap/include/colmap/mvs/gpu_mat_prng.h b/colmap/include/colmap/mvs/gpu_mat_prng.h new file mode 100644 index 0000000000000000000000000000000000000000..d117823bfd9142667c4dc4a6c821baa5c1c61eb8 --- /dev/null +++ b/colmap/include/colmap/mvs/gpu_mat_prng.h @@ -0,0 +1,51 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_GPU_MAT_PRNG_H_ +#define COLMAP_SRC_MVS_GPU_MAT_PRNG_H_ + +#include "mvs/gpu_mat.h" + +namespace colmap { +namespace mvs { + +class GpuMatPRNG : public GpuMat { + public: + GpuMatPRNG(const int width, const int height); + + private: + void InitRandomState(); +}; + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_GPU_MAT_PRNG_H_ diff --git a/colmap/include/colmap/mvs/gpu_mat_ref_image.h b/colmap/include/colmap/mvs/gpu_mat_ref_image.h new file mode 100644 index 0000000000000000000000000000000000000000..1e04e5f436134c1ca25d05f1a368160b8175b7fd --- /dev/null +++ b/colmap/include/colmap/mvs/gpu_mat_ref_image.h @@ -0,0 +1,95 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_GPU_MAT_REF_IMAGE_H_ +#define COLMAP_SRC_MVS_GPU_MAT_REF_IMAGE_H_ + +#include + +#include "mvs/cuda_array_wrapper.h" +#include "mvs/gpu_mat.h" + +namespace colmap { +namespace mvs { + +class GpuMatRefImage { + public: + GpuMatRefImage(const size_t width, const size_t height); + + // Filter image using sum convolution kernel to compute local sum of + // intensities. The filtered images can then be used for repeated, efficient + // NCC computation. + void Filter(const uint8_t* image_data, const size_t window_radius, + const size_t window_step, const float sigma_spatial, + const float sigma_color); + + // Image intensities. + std::unique_ptr> image; + + // Local sum of image intensities. + std::unique_ptr> sum_image; + + // Local sum of squared image intensities. + std::unique_ptr> squared_sum_image; + + private: + const static size_t kBlockDimX = 16; + const static size_t kBlockDimY = 12; + + size_t width_; + size_t height_; +}; + +struct BilateralWeightComputer { + __device__ BilateralWeightComputer(const float sigma_spatial, + const float sigma_color) + : spatial_normalization_(1.0f / (2.0f * sigma_spatial * sigma_spatial)), + color_normalization_(1.0f / (2.0f * sigma_color * sigma_color)) {} + + __device__ inline float Compute(const float row_diff, const float col_diff, + const float color1, + const float color2) const { + const float spatial_dist_squared = + row_diff * row_diff + col_diff * col_diff; + const float color_dist = color1 - color2; + return exp(-spatial_dist_squared * spatial_normalization_ - + color_dist * color_dist * color_normalization_); + } + + private: + const float spatial_normalization_; + const float color_normalization_; +}; + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_GPU_MAT_REF_IMAGE_H_ diff --git a/colmap/include/colmap/mvs/image.h b/colmap/include/colmap/mvs/image.h new file mode 100644 index 0000000000000000000000000000000000000000..c1a6f29e6a6fa230da1fba2eb4a9bd2b060a34a1 --- /dev/null +++ b/colmap/include/colmap/mvs/image.h @@ -0,0 +1,124 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_IMAGE_H_ +#define COLMAP_SRC_MVS_IMAGE_H_ + +#include +#include +#include +#include +#include +#include + +#include "util/bitmap.h" + +namespace colmap { +namespace mvs { + +class Image { + public: + Image(); + Image(const std::string& path, const size_t width, const size_t height, + const float* K, const float* R, const float* T); + + inline size_t GetWidth() const; + inline size_t GetHeight() const; + + void SetBitmap(const Bitmap& bitmap); + inline const Bitmap& GetBitmap() const; + + inline const std::string& GetPath() const; + inline const float* GetR() const; + inline const float* GetT() const; + inline const float* GetK() const; + inline const float* GetP() const; + inline const float* GetInvP() const; + inline const float* GetViewingDirection() const; + + void Rescale(const float factor); + void Rescale(const float factor_x, const float factor_y); + void Downsize(const size_t max_width, const size_t max_height); + + private: + std::string path_; + size_t width_; + size_t height_; + float K_[9]; + float R_[9]; + float T_[3]; + float P_[12]; + float inv_P_[12]; + Bitmap bitmap_; +}; + +void ComputeRelativePose(const float R1[9], const float T1[3], + const float R2[9], const float T2[3], float R[9], + float T[3]); + +void ComposeProjectionMatrix(const float K[9], const float R[9], + const float T[3], float P[12]); + +void ComposeInverseProjectionMatrix(const float K[9], const float R[9], + const float T[3], float inv_P[12]); + +void ComputeProjectionCenter(const float R[9], const float T[3], float C[3]); + +void RotatePose(const float RR[9], float R[9], float T[3]); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +size_t Image::GetWidth() const { return width_; } + +size_t Image::GetHeight() const { return height_; } + +const Bitmap& Image::GetBitmap() const { return bitmap_; } + +const std::string& Image::GetPath() const { return path_; } + +const float* Image::GetR() const { return R_; } + +const float* Image::GetT() const { return T_; } + +const float* Image::GetK() const { return K_; } + +const float* Image::GetP() const { return P_; } + +const float* Image::GetInvP() const { return inv_P_; } + +const float* Image::GetViewingDirection() const { return &R_[6]; } + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_IMAGE_H_ diff --git a/colmap/include/colmap/mvs/mat.h b/colmap/include/colmap/mvs/mat.h new file mode 100644 index 0000000000000000000000000000000000000000..c50115a15e40fa20f48309d7f53e5f7561bd89ab --- /dev/null +++ b/colmap/include/colmap/mvs/mat.h @@ -0,0 +1,196 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_MAT_H_ +#define COLMAP_SRC_MVS_MAT_H_ + +#include +#include +#include + +#include "util/endian.h" +#include "util/logging.h" + +namespace colmap { +namespace mvs { + +template +class Mat { + public: + Mat(); + Mat(const size_t width, const size_t height, const size_t depth); + + size_t GetWidth() const; + size_t GetHeight() const; + size_t GetDepth() const; + + size_t GetNumBytes() const; + + T Get(const size_t row, const size_t col, const size_t slice = 0) const; + void GetSlice(const size_t row, const size_t col, T* values) const; + T* GetPtr(); + const T* GetPtr() const; + + const std::vector& GetData() const; + + void Set(const size_t row, const size_t col, const T value); + void Set(const size_t row, const size_t col, const size_t slice, + const T value); + + void Fill(const T value); + + void Read(const std::string& path); + void Write(const std::string& path) const; + + protected: + size_t width_ = 0; + size_t height_ = 0; + size_t depth_ = 0; + std::vector data_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +Mat::Mat() : Mat(0, 0, 0) {} + +template +Mat::Mat(const size_t width, const size_t height, const size_t depth) + : width_(width), height_(height), depth_(depth) { + data_.resize(width_ * height_ * depth_, 0); +} + +template +size_t Mat::GetWidth() const { + return width_; +} + +template +size_t Mat::GetHeight() const { + return height_; +} + +template +size_t Mat::GetDepth() const { + return depth_; +} + +template +size_t Mat::GetNumBytes() const { + return data_.size() * sizeof(T); +} + +template +T Mat::Get(const size_t row, const size_t col, const size_t slice) const { + return data_.at(slice * width_ * height_ + row * width_ + col); +} + +template +void Mat::GetSlice(const size_t row, const size_t col, T* values) const { + for (size_t slice = 0; slice < depth_; ++slice) { + values[slice] = Get(row, col, slice); + } +} + +template +T* Mat::GetPtr() { + return data_.data(); +} + +template +const T* Mat::GetPtr() const { + return data_.data(); +} + +template +const std::vector& Mat::GetData() const { + return data_; +} + +template +void Mat::Set(const size_t row, const size_t col, const T value) { + Set(row, col, 0, value); +} + +template +void Mat::Set(const size_t row, const size_t col, const size_t slice, + const T value) { + data_.at(slice * width_ * height_ + row * width_ + col) = value; +} + +template +void Mat::Fill(const T value) { + std::fill(data_.begin(), data_.end(), value); +} + +template +void Mat::Read(const std::string& path) { + std::fstream text_file(path, std::ios::in | std::ios::binary); + CHECK(text_file.is_open()) << path; + + char unused_char; + text_file >> width_ >> unused_char >> height_ >> unused_char >> depth_ >> + unused_char; + std::streampos pos = text_file.tellg(); + text_file.close(); + + CHECK_GT(width_, 0); + CHECK_GT(height_, 0); + CHECK_GT(depth_, 0); + data_.resize(width_ * height_ * depth_); + + std::fstream binary_file(path, std::ios::in | std::ios::binary); + CHECK(binary_file.is_open()) << path; + binary_file.seekg(pos); + ReadBinaryLittleEndian(&binary_file, &data_); + binary_file.close(); +} + +template +void Mat::Write(const std::string& path) const { + std::fstream text_file(path, std::ios::out); + CHECK(text_file.is_open()) << path; + text_file << width_ << "&" << height_ << "&" << depth_ << "&"; + text_file.close(); + + std::fstream binary_file(path, + std::ios::out | std::ios::binary | std::ios::app); + CHECK(binary_file.is_open()) << path; + WriteBinaryLittleEndian(&binary_file, data_); + binary_file.close(); +} + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_MAT_H_ diff --git a/colmap/include/colmap/mvs/meshing.h b/colmap/include/colmap/mvs/meshing.h new file mode 100644 index 0000000000000000000000000000000000000000..fcdf10b46006b0cb44f0a12440b8bcff727f3ab1 --- /dev/null +++ b/colmap/include/colmap/mvs/meshing.h @@ -0,0 +1,134 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_MESHING_H_ +#define COLMAP_SRC_MVS_MESHING_H_ + +#include + +namespace colmap { +namespace mvs { + +struct PoissonMeshingOptions { + // This floating point value specifies the importance that interpolation of + // the point samples is given in the formulation of the screened Poisson + // equation. The results of the original (unscreened) Poisson Reconstruction + // can be obtained by setting this value to 0. + double point_weight = 1.0; + + // This integer is the maximum depth of the tree that will be used for surface + // reconstruction. Running at depth d corresponds to solving on a voxel grid + // whose resolution is no larger than 2^d x 2^d x 2^d. Note that since the + // reconstructor adapts the octree to the sampling density, the specified + // reconstruction depth is only an upper bound. + int depth = 13; + + // If specified, the reconstruction code assumes that the input is equipped + // with colors and will extrapolate the color values to the vertices of the + // reconstructed mesh. The floating point value specifies the relative + // importance of finer color estimates over lower ones. + double color = 32.0; + + // This floating point values specifies the value for mesh trimming. The + // subset of the mesh with signal value less than the trim value is discarded. + double trim = 10.0; + + // The number of threads used for the Poisson reconstruction. + int num_threads = -1; + + bool Check() const; +}; + +struct DelaunayMeshingOptions { + // Unify input points into one cell in the Delaunay triangulation that fall + // within a reprojected radius of the given pixels. + double max_proj_dist = 20.0; + + // Maximum relative depth difference between input point and a vertex of an + // existing cell in the Delaunay triangulation, otherwise a new vertex is + // created in the triangulation. + double max_depth_dist = 0.05; + + // The standard deviation of wrt. the number of images seen by each point. + // Increasing this value decreases the influence of points seen in few images. + double visibility_sigma = 3.0; + + // The factor that is applied to the computed distance sigma, which is + // automatically computed as the 25th percentile of edge lengths. A higher + // value will increase the smoothness of the surface. + double distance_sigma_factor = 1.0; + + // A higher quality regularization leads to a smoother surface. + double quality_regularization = 1.0; + + // Filtering thresholds for outlier surface mesh faces. If the longest side of + // a mesh face (longest out of 3) exceeds the side lengths of all faces at a + // certain percentile by the given factor, then it is considered an outlier + // mesh face and discarded. + double max_side_length_factor = 25.0; + double max_side_length_percentile = 95.0; + + // The number of threads to use for reconstruction. Default is all threads. + int num_threads = -1; + + bool Check() const; +}; + +// Perform Poisson surface reconstruction and return true if successful. +bool PoissonMeshing(const PoissonMeshingOptions& options, + const std::string& input_path, + const std::string& output_path); + +#ifdef CGAL_ENABLED + +// Delaunay meshing of sparse and dense COLMAP reconstructions. This is an +// implementation of the approach described in: +// +// P. Labatut, J‐P. Pons, and R. Keriven. "Robust and efficient surface +// reconstruction from range data". Computer graphics forum, 2009. +// +// In case of sparse input, the path should point to a sparse COLMAP +// reconstruction. In case of dense input, the path should point to a dense +// COLMAP workspace folder, which has been fully processed by the stereo and +// fusion pipeline. +void SparseDelaunayMeshing(const DelaunayMeshingOptions& options, + const std::string& input_path, + const std::string& output_path); +void DenseDelaunayMeshing(const DelaunayMeshingOptions& options, + const std::string& input_path, + const std::string& output_path); + +#endif // CGAL_ENABLED + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_MESHING_H_ diff --git a/colmap/include/colmap/mvs/model.h b/colmap/include/colmap/mvs/model.h new file mode 100644 index 0000000000000000000000000000000000000000..15761a9497ef15fe31442eccc649ca58f191127d --- /dev/null +++ b/colmap/include/colmap/mvs/model.h @@ -0,0 +1,110 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_MODEL_H_ +#define COLMAP_SRC_MVS_MODEL_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include "mvs/depth_map.h" +#include "mvs/image.h" +#include "mvs/normal_map.h" + +namespace colmap { +namespace mvs { + +// Simple sparse model class. +struct Model { + struct Point { + float x = 0; + float y = 0; + float z = 0; + std::vector track; + }; + + // Read the model from different data formats. + void Read(const std::string& path, const std::string& format); + void ReadFromCOLMAP(const std::string& path, + const std::string& sparse_path = "sparse", + const std::string& images_path = "images"); + void ReadFromPMVS(const std::string& path); + + // Get the image index for the given image name. + int GetImageIdx(const std::string& name) const; + std::string GetImageName(const int image_idx) const; + + // For each image, determine the maximally overlapping images, sorted based on + // the number of shared points subject to a minimum robust average + // triangulation angle of the points. + std::vector> GetMaxOverlappingImages( + const size_t num_images, const double min_triangulation_angle) const; + + // Get the overlapping images defined in the vis.dat file. + const std::vector>& GetMaxOverlappingImagesFromPMVS() const; + + // Compute the robust minimum and maximum depths from the sparse point cloud. + std::vector> ComputeDepthRanges() const; + + // Compute the number of shared points between all overlapping images. + std::vector> ComputeSharedPoints() const; + + // Compute the median triangulation angles between all overlapping images. + std::vector> ComputeTriangulationAngles( + const float percentile = 50) const; + + // Note that in case the data is read from a COLMAP reconstruction, the index + // of an image or point does not correspond to its original identifier in the + // reconstruction, but it corresponds to the position in the + // images.bin/points3D.bin files. This is mainly done for more efficient + // access to the data, which is required during the stereo fusion stage. + std::vector images; + std::vector points; + + private: + bool ReadFromBundlerPMVS(const std::string& path); + bool ReadFromRawPMVS(const std::string& path); + + std::vector image_names_; + std::unordered_map image_name_to_idx_; + + std::vector> pmvs_vis_dat_; +}; + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_MODEL_H_ diff --git a/colmap/include/colmap/mvs/normal_map.h b/colmap/include/colmap/mvs/normal_map.h new file mode 100644 index 0000000000000000000000000000000000000000..7661402f6cc93ad0c48862c37e37b91fa89717ca --- /dev/null +++ b/colmap/include/colmap/mvs/normal_map.h @@ -0,0 +1,60 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_NORMAL_MAP_H_ +#define COLMAP_SRC_MVS_NORMAL_MAP_H_ + +#include +#include + +#include "mvs/mat.h" +#include "util/bitmap.h" + +namespace colmap { +namespace mvs { + +// Normal map class that stores per-pixel normals as a MxNx3 image. +class NormalMap : public Mat { + public: + NormalMap(); + NormalMap(const size_t width, const size_t height); + explicit NormalMap(const Mat& mat); + + void Rescale(const float factor); + void Downsize(const size_t max_width, const size_t max_height); + + Bitmap ToBitmap() const; +}; + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_NORMAL_MAP_H_ diff --git a/colmap/include/colmap/mvs/patch_match.h b/colmap/include/colmap/mvs/patch_match.h new file mode 100644 index 0000000000000000000000000000000000000000..a00a448f2f61a1d7419d2a69adefae44eb37bf0f --- /dev/null +++ b/colmap/include/colmap/mvs/patch_match.h @@ -0,0 +1,286 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_PATCH_MATCH_H_ +#define COLMAP_SRC_MVS_PATCH_MATCH_H_ + +#include +#include +#include + +#include "mvs/depth_map.h" +#include "mvs/image.h" +#include "mvs/model.h" +#include "mvs/normal_map.h" +#ifndef __CUDACC__ +#include "util/threading.h" +#endif + +namespace colmap { +namespace mvs { + +// Maximum possible window radius for the photometric consistency cost. This +// value is equal to THREADS_PER_BLOCK in patch_match_cuda.cu and the limit +// arises from the shared memory implementation. +const static size_t kMaxPatchMatchWindowRadius = 32; + +class ConsistencyGraph; +class PatchMatchCuda; +class Workspace; + +struct PatchMatchOptions { + // Maximum image size in either dimension. + int max_image_size = -1; + + // Index of the GPU used for patch match. For multi-GPU usage, + // you should separate multiple GPU indices by comma, e.g., "0,1,2,3". + std::string gpu_index = "-1"; + + // Depth range in which to randomly sample depth hypotheses. + double depth_min = -1.0f; + double depth_max = -1.0f; + + // Half window size to compute NCC photo-consistency cost. + int window_radius = 5; + + // Number of pixels to skip when computing NCC. For a value of 1, every + // pixel is used to compute the NCC. For larger values, only every n-th row + // and column is used and the computation speed thereby increases roughly by + // a factor of window_step^2. Note that not all combinations of window sizes + // and steps produce nice results, especially if the step is greather than 2. + int window_step = 1; + + // Parameters for bilaterally weighted NCC. + double sigma_spatial = -1; + double sigma_color = 0.2f; + + // Number of random samples to draw in Monte Carlo sampling. + int num_samples = 15; + + // Spread of the NCC likelihood function. + double ncc_sigma = 0.6f; + + // Minimum triangulation angle in degrees. + double min_triangulation_angle = 1.0f; + + // Spread of the incident angle likelihood function. + double incident_angle_sigma = 0.9f; + + // Number of coordinate descent iterations. Each iteration consists + // of four sweeps from left to right, top to bottom, and vice versa. + int num_iterations = 5; + + // Whether to add a regularized geometric consistency term to the cost + // function. If true, the `depth_maps` and `normal_maps` must not be null. + bool geom_consistency = true; + + // The relative weight of the geometric consistency term w.r.t. to + // the photo-consistency term. + double geom_consistency_regularizer = 0.3f; + + // Maximum geometric consistency cost in terms of the forward-backward + // reprojection error in pixels. + double geom_consistency_max_cost = 3.0f; + + // Whether to enable filtering. + bool filter = true; + + // Minimum NCC coefficient for pixel to be photo-consistent. + double filter_min_ncc = 0.1f; + + // Minimum triangulation angle to be stable. + double filter_min_triangulation_angle = 3.0f; + + // Minimum number of source images have to be consistent + // for pixel not to be filtered. + int filter_min_num_consistent = 2; + + // Maximum forward-backward reprojection error for pixel + // to be geometrically consistent. + double filter_geom_consistency_max_cost = 1.0f; + + // Cache size in gigabytes for patch match, which keeps the bitmaps, depth + // maps, and normal maps of this number of images in memory. A higher value + // leads to less disk access and faster computation, while a lower value + // leads to reduced memory usage. Note that a single image can consume a lot + // of memory, if the consistency graph is dense. + double cache_size = 32.0; + + // Whether to tolerate missing images/maps in the problem setup + bool allow_missing_files = false; + + // Whether to write the consistency graph. + bool write_consistency_graph = false; + + void Print() const; + bool Check() const { + if (depth_min != -1.0f || depth_max != -1.0f) { + CHECK_OPTION_LE(depth_min, depth_max); + CHECK_OPTION_GE(depth_min, 0.0f); + } + CHECK_OPTION_LE(window_radius, + static_cast(kMaxPatchMatchWindowRadius)); + CHECK_OPTION_GT(sigma_color, 0.0f); + CHECK_OPTION_GT(window_radius, 0); + CHECK_OPTION_GT(window_step, 0); + CHECK_OPTION_LE(window_step, 2); + CHECK_OPTION_GT(num_samples, 0); + CHECK_OPTION_GT(ncc_sigma, 0.0f); + CHECK_OPTION_GE(min_triangulation_angle, 0.0f); + CHECK_OPTION_LT(min_triangulation_angle, 180.0f); + CHECK_OPTION_GT(incident_angle_sigma, 0.0f); + CHECK_OPTION_GT(num_iterations, 0); + CHECK_OPTION_GE(geom_consistency_regularizer, 0.0f); + CHECK_OPTION_GE(geom_consistency_max_cost, 0.0f); + CHECK_OPTION_GE(filter_min_ncc, -1.0f); + CHECK_OPTION_LE(filter_min_ncc, 1.0f); + CHECK_OPTION_GE(filter_min_triangulation_angle, 0.0f); + CHECK_OPTION_LE(filter_min_triangulation_angle, 180.0f); + CHECK_OPTION_GE(filter_min_num_consistent, 0); + CHECK_OPTION_GE(filter_geom_consistency_max_cost, 0.0f); + CHECK_OPTION_GT(cache_size, 0); + return true; + } +}; + +// This is a wrapper class around the actual PatchMatchCuda implementation. This +// class is necessary to hide Cuda code from any boost or Eigen code, since +// NVCC/MSVC cannot compile complex C++ code. +class PatchMatch { + public: + struct Problem { + // Index of the reference image. + int ref_image_idx = -1; + + // Indices of the source images. + std::vector src_image_idxs; + + // Input images for the photometric consistency term. + std::vector* images = nullptr; + + // Input depth maps for the geometric consistency term. + std::vector* depth_maps = nullptr; + + // Input normal maps for the geometric consistency term. + std::vector* normal_maps = nullptr; + + // Print the configuration to stdout. + void Print() const; + }; + + PatchMatch(const PatchMatchOptions& options, const Problem& problem); + ~PatchMatch(); + + // Check the options and the problem for validity. + void Check() const; + + // Run the patch match algorithm. + void Run(); + + // Get the computed values after running the algorithm. + DepthMap GetDepthMap() const; + NormalMap GetNormalMap() const; + ConsistencyGraph GetConsistencyGraph() const; + Mat GetSelProbMap() const; + + private: + const PatchMatchOptions options_; + const Problem problem_; + std::unique_ptr patch_match_cuda_; +}; + +// This thread processes all problems in a workspace. A workspace has the +// following file structure, if the workspace format is "COLMAP": +// +// images/* +// sparse/{cameras.txt, images.txt, points3D.txt} +// stereo/ +// depth_maps/* +// normal_maps/* +// consistency_graphs/* +// patch-match.cfg +// +// The `patch-match.cfg` file specifies the images to be processed as: +// +// image_name1.jpg +// __all__ +// image_name2.jpg +// __auto__, 20 +// image_name3.jpg +// image_name1.jpg, image_name2.jpg +// +// Two consecutive lines specify the images used to compute one patch match +// problem. The first line specifies the reference image and the second line the +// source images. Image names are relative to the `images` directory. In this +// example, the first reference image uses all other images as source images, +// the second reference image uses the 20 most connected images as source +// images, and the third reference image uses the first and second as source +// images. Note that all specified images must be reconstructed in the COLMAP +// reconstruction provided in the `sparse` folder. + +#ifndef __CUDACC__ + +class PatchMatchController : public Thread { + public: + PatchMatchController(const PatchMatchOptions& options, + const std::string& workspace_path, + const std::string& workspace_format, + const std::string& pmvs_option_name, + const std::string& config_path = ""); + + private: + void Run(); + void ReadWorkspace(); + void ReadProblems(); + void ReadGpuIndices(); + void ProcessProblem(const PatchMatchOptions& options, + const size_t problem_idx); + + const PatchMatchOptions options_; + const std::string workspace_path_; + const std::string workspace_format_; + const std::string pmvs_option_name_; + const std::string config_path_; + + std::unique_ptr thread_pool_; + std::mutex workspace_mutex_; + std::unique_ptr workspace_; + std::vector problems_; + std::vector gpu_indices_; + std::vector> depth_ranges_; +}; + +#endif + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_PATCH_MATCH_H_ diff --git a/colmap/include/colmap/mvs/patch_match_cuda.h b/colmap/include/colmap/mvs/patch_match_cuda.h new file mode 100644 index 0000000000000000000000000000000000000000..adbecdbd92e2bb59df25aaa476822321c7af3992 --- /dev/null +++ b/colmap/include/colmap/mvs/patch_match_cuda.h @@ -0,0 +1,142 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_PATCH_MATCH_CUDA_H_ +#define COLMAP_SRC_MVS_PATCH_MATCH_CUDA_H_ + +#include +#include +#include + +#include + +#include "mvs/cuda_array_wrapper.h" +#include "mvs/depth_map.h" +#include "mvs/gpu_mat.h" +#include "mvs/gpu_mat_prng.h" +#include "mvs/gpu_mat_ref_image.h" +#include "mvs/image.h" +#include "mvs/normal_map.h" +#include "mvs/patch_match.h" + +namespace colmap { +namespace mvs { + +class PatchMatchCuda { + public: + PatchMatchCuda(const PatchMatchOptions& options, + const PatchMatch::Problem& problem); + ~PatchMatchCuda(); + + void Run(); + + DepthMap GetDepthMap() const; + NormalMap GetNormalMap() const; + Mat GetSelProbMap() const; + std::vector GetConsistentImageIdxs() const; + + private: + template + void RunWithWindowSizeAndStep(); + + void ComputeCudaConfig(); + + void InitRefImage(); + void InitSourceImages(); + void InitTransforms(); + void InitWorkspaceMemory(); + + // Rotate reference image by 90 degrees in counter-clockwise direction. + void Rotate(); + + const PatchMatchOptions options_; + const PatchMatch::Problem problem_; + + // Dimensions for sweeping from top to bottom, i.e. one thread per column. + dim3 sweep_block_size_; + dim3 sweep_grid_size_; + // Dimensions for element-wise operations, i.e. one thread per pixel. + dim3 elem_wise_block_size_; + dim3 elem_wise_grid_size_; + + // Original (not rotated) dimension of reference image. + size_t ref_width_; + size_t ref_height_; + + // Rotation of reference image in pi/2. This is equivalent to the number of + // calls to `rotate` mod 4. + int rotation_in_half_pi_; + + // Reference and source image input data. + std::unique_ptr> ref_image_device_; + std::unique_ptr> src_images_device_; + std::unique_ptr> src_depth_maps_device_; + + // Relative poses from rotated versions of reference image to source images + // corresponding to _rotationInHalfPi: + // + // [S(1), S(2), S(3), ..., S(n)] + // + // where n is the number of source images and: + // + // S(i) = [K_i(0, 0), K_i(0, 2), K_i(1, 1), K_i(1, 2), R_i(:), T_i(:) + // C_i(:), P(:), P^-1(:)] + // + // where i denotes the index of the source image and K is its calibration. + // R, T, C, P, P^-1 denote the relative rotation, translation, camera + // center, projection, and inverse projection from there reference to the + // i-th source image. + std::unique_ptr> poses_device_[4]; + + // Calibration matrix for rotated versions of reference image + // as {K[0, 0], K[0, 2], K[1, 1], K[1, 2]} corresponding to _rotationInHalfPi. + float ref_K_host_[4][4]; + float ref_inv_K_host_[4][4]; + + // Data for reference image. + std::unique_ptr ref_image_; + std::unique_ptr> depth_map_; + std::unique_ptr> normal_map_; + std::unique_ptr> sel_prob_map_; + std::unique_ptr> prev_sel_prob_map_; + std::unique_ptr> cost_map_; + std::unique_ptr rand_state_map_; + std::unique_ptr> consistency_mask_; + + // Shared memory is too small to hold local state for each thread, + // so this is workspace memory in global memory. + std::unique_ptr> global_workspace_; +}; + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_PATCH_MATCH_CUDA_H_ diff --git a/colmap/include/colmap/mvs/workspace.h b/colmap/include/colmap/mvs/workspace.h new file mode 100644 index 0000000000000000000000000000000000000000..14bc518d82a95a688c99013ebd3ee99174f3eeed --- /dev/null +++ b/colmap/include/colmap/mvs/workspace.h @@ -0,0 +1,145 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_MVS_WORKSPACE_H_ +#define COLMAP_SRC_MVS_WORKSPACE_H_ + +#include "mvs/consistency_graph.h" +#include "mvs/depth_map.h" +#include "mvs/model.h" +#include "mvs/normal_map.h" +#include "util/bitmap.h" +#include "util/cache.h" +#include "util/misc.h" + +namespace colmap { +namespace mvs { + +class Workspace { + public: + struct Options { + // The maximum cache size in gigabytes. + double cache_size = 32.0; + + // The number of threads to use when pre-loading workspace. + int num_threads = -1; + + // Maximum image size in either dimension. + int max_image_size = -1; + + // Whether to read image as RGB or gray scale. + bool image_as_rgb = true; + + // Location and type of workspace. + std::string workspace_path; + std::string workspace_format; + std::string input_type; + std::string stereo_folder = "stereo"; + }; + + Workspace(const Options& options); + virtual ~Workspace() = default; + + // Do nothing when we use a cache. Data is loaded as needed. + virtual void Load(const std::vector& image_names); + + inline const Options& GetOptions() const { return options_; } + + inline const Model& GetModel() const { return model_; } + + virtual const Bitmap& GetBitmap(const int image_idx); + virtual const DepthMap& GetDepthMap(const int image_idx); + virtual const NormalMap& GetNormalMap(const int image_idx); + + // Get paths to bitmap, depth map, normal map and consistency graph. + std::string GetBitmapPath(const int image_idx) const; + std::string GetDepthMapPath(const int image_idx) const; + std::string GetNormalMapPath(const int image_idx) const; + + // Return whether bitmap, depth map, normal map, and consistency graph exist. + bool HasBitmap(const int image_idx) const; + bool HasDepthMap(const int image_idx) const; + bool HasNormalMap(const int image_idx) const; + + protected: + std::string GetFileName(const int image_idx) const; + + Options options_; + Model model_; + + private: + std::string depth_map_path_; + std::string normal_map_path_; + std::vector> bitmaps_; + std::vector> depth_maps_; + std::vector> normal_maps_; +}; + +class CachedWorkspace : public Workspace { + public: + CachedWorkspace(const Options& options); + + void Load(const std::vector& image_names) override {} + + inline void ClearCache() { cache_.Clear(); } + + const Bitmap& GetBitmap(const int image_idx) override; + const DepthMap& GetDepthMap(const int image_idx) override; + const NormalMap& GetNormalMap(const int image_idx) override; + + private: + class CachedImage { + public: + CachedImage() {} + CachedImage(CachedImage&& other); + CachedImage& operator=(CachedImage&& other); + inline size_t NumBytes() const { return num_bytes; } + size_t num_bytes = 0; + std::unique_ptr bitmap; + std::unique_ptr depth_map; + std::unique_ptr normal_map; + + private: + NON_COPYABLE(CachedImage) + }; + + MemoryConstrainedLRUCache cache_; +}; + +// Import a PMVS workspace into the COLMAP workspace format. Only images in the +// provided option file name will be imported and used for reconstruction. +void ImportPMVSWorkspace(const Workspace& workspace, + const std::string& option_name); + +} // namespace mvs +} // namespace colmap + +#endif // COLMAP_SRC_MVS_WORKSPACE_H_ diff --git a/colmap/include/colmap/optim/bundle_adjustment.h b/colmap/include/colmap/optim/bundle_adjustment.h new file mode 100644 index 0000000000000000000000000000000000000000..8d6282ea765a6a6067a1601913ef91b0edee6be8 --- /dev/null +++ b/colmap/include/colmap/optim/bundle_adjustment.h @@ -0,0 +1,333 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_OPTIM_BUNDLE_ADJUSTMENT_H_ +#define COLMAP_SRC_OPTIM_BUNDLE_ADJUSTMENT_H_ + +#include +#include + +#include + +#include + +#include "PBA/pba.h" +#include "base/camera_rig.h" +#include "base/reconstruction.h" +#include "util/alignment.h" + +namespace colmap { + +struct BundleAdjustmentOptions { + // Loss function types: Trivial (non-robust) and Cauchy (robust) loss. + enum class LossFunctionType { TRIVIAL, SOFT_L1, CAUCHY }; + LossFunctionType loss_function_type = LossFunctionType::TRIVIAL; + + // Scaling factor determines residual at which robustification takes place. + double loss_function_scale = 1.0; + + // Whether to refine the focal length parameter group. + bool refine_focal_length = true; + + // Whether to refine the principal point parameter group. + bool refine_principal_point = false; + + // Whether to refine the extra parameter group. + bool refine_extra_params = true; + + // Whether to refine the extrinsic parameter group. + bool refine_extrinsics = true; + + // Whether to print a final summary. + bool print_summary = true; + + // Minimum number of residuals to enable multi-threading. Note that + // single-threaded is typically better for small bundle adjustment problems + // due to the overhead of threading. + int min_num_residuals_for_multi_threading = 50000; + + // Ceres-Solver options. + ceres::Solver::Options solver_options; + + BundleAdjustmentOptions() { + solver_options.function_tolerance = 0.0; + solver_options.gradient_tolerance = 0.0; + solver_options.parameter_tolerance = 0.0; + solver_options.minimizer_progress_to_stdout = false; + solver_options.max_num_iterations = 100; + solver_options.max_linear_solver_iterations = 200; + solver_options.max_num_consecutive_invalid_steps = 10; + solver_options.max_consecutive_nonmonotonic_steps = 10; + solver_options.num_threads = -1; +#if CERES_VERSION_MAJOR < 2 + solver_options.num_linear_solver_threads = -1; +#endif // CERES_VERSION_MAJOR + } + + // Create a new loss function based on the specified options. The caller + // takes ownership of the loss function. + ceres::LossFunction* CreateLossFunction() const; + + bool Check() const; +}; + +// Configuration container to setup bundle adjustment problems. +class BundleAdjustmentConfig { + public: + BundleAdjustmentConfig(); + + size_t NumImages() const; + size_t NumPoints() const; + size_t NumConstantCameras() const; + size_t NumConstantPoses() const; + size_t NumConstantTvecs() const; + size_t NumVariablePoints() const; + size_t NumConstantPoints() const; + + // Determine the number of residuals for the given reconstruction. The number + // of residuals equals the number of observations times two. + size_t NumResiduals(const Reconstruction& reconstruction) const; + + // Add / remove images from the configuration. + void AddImage(const image_t image_id); + bool HasImage(const image_t image_id) const; + void RemoveImage(const image_t image_id); + + // Set cameras of added images as constant or variable. By default all + // cameras of added images are variable. Note that the corresponding images + // have to be added prior to calling these methods. + void SetConstantCamera(const camera_t camera_id); + void SetVariableCamera(const camera_t camera_id); + bool IsConstantCamera(const camera_t camera_id) const; + + // Set the pose of added images as constant. The pose is defined as the + // rotational and translational part of the projection matrix. + void SetConstantPose(const image_t image_id); + void SetVariablePose(const image_t image_id); + bool HasConstantPose(const image_t image_id) const; + + // Set the translational part of the pose, hence the constant pose + // indices may be in [0, 1, 2] and must be unique. Note that the + // corresponding images have to be added prior to calling these methods. + void SetConstantTvec(const image_t image_id, const std::vector& idxs); + void RemoveConstantTvec(const image_t image_id); + bool HasConstantTvec(const image_t image_id) const; + + // Add / remove points from the configuration. Note that points can either + // be variable or constant but not both at the same time. + void AddVariablePoint(const point3D_t point3D_id); + void AddConstantPoint(const point3D_t point3D_id); + bool HasPoint(const point3D_t point3D_id) const; + bool HasVariablePoint(const point3D_t point3D_id) const; + bool HasConstantPoint(const point3D_t point3D_id) const; + void RemoveVariablePoint(const point3D_t point3D_id); + void RemoveConstantPoint(const point3D_t point3D_id); + + // Access configuration data. + const std::unordered_set& Images() const; + const std::unordered_set& VariablePoints() const; + const std::unordered_set& ConstantPoints() const; + const std::vector& ConstantTvec(const image_t image_id) const; + + private: + std::unordered_set constant_camera_ids_; + std::unordered_set image_ids_; + std::unordered_set variable_point3D_ids_; + std::unordered_set constant_point3D_ids_; + std::unordered_set constant_poses_; + std::unordered_map> constant_tvecs_; +}; + +// Bundle adjustment based on Ceres-Solver. Enables most flexible configurations +// and provides best solution quality. +class BundleAdjuster { + public: + BundleAdjuster(const BundleAdjustmentOptions& options, + const BundleAdjustmentConfig& config); + + bool Solve(Reconstruction* reconstruction); + + // Get the Ceres solver summary for the last call to `Solve`. + const ceres::Solver::Summary& Summary() const; + + private: + void SetUp(Reconstruction* reconstruction, + ceres::LossFunction* loss_function); + void TearDown(Reconstruction* reconstruction); + + void AddImageToProblem(const image_t image_id, Reconstruction* reconstruction, + ceres::LossFunction* loss_function); + + void AddPointToProblem(const point3D_t point3D_id, + Reconstruction* reconstruction, + ceres::LossFunction* loss_function); + + protected: + void ParameterizeCameras(Reconstruction* reconstruction); + void ParameterizePoints(Reconstruction* reconstruction); + + const BundleAdjustmentOptions options_; + BundleAdjustmentConfig config_; + std::unique_ptr problem_; + ceres::Solver::Summary summary_; + std::unordered_set camera_ids_; + std::unordered_map point3D_num_observations_; +}; + +// Bundle adjustment using PBA (GPU or CPU). Less flexible and accurate than +// Ceres-Solver bundle adjustment but much faster. Only supports SimpleRadial +// camera model. +class ParallelBundleAdjuster { + public: + struct Options { + // Whether to print a final summary. + bool print_summary = true; + + // Maximum number of iterations. + int max_num_iterations = 50; + + // Index of the GPU used for bundle adjustment. + int gpu_index = -1; + + // Number of threads for CPU based bundle adjustment. + int num_threads = -1; + + // Minimum number of residuals to enable multi-threading. Note that + // single-threaded is typically better for small bundle adjustment problems + // due to the overhead of threading. + int min_num_residuals_for_multi_threading = 50000; + + bool Check() const; + }; + + ParallelBundleAdjuster(const Options& options, + const BundleAdjustmentOptions& ba_options, + const BundleAdjustmentConfig& config); + + bool Solve(Reconstruction* reconstruction); + + // Get the Ceres solver summary for the last call to `Solve`. + const ceres::Solver::Summary& Summary() const; + + // Check whether PBA is supported for the given reconstruction. If the + // reconstruction is not supported, the PBA solver will exit ungracefully. + static bool IsSupported(const BundleAdjustmentOptions& options, + const Reconstruction& reconstruction); + + private: + void SetUp(Reconstruction* reconstruction); + void TearDown(Reconstruction* reconstruction); + + void AddImagesToProblem(Reconstruction* reconstruction); + void AddPointsToProblem(Reconstruction* reconstruction); + + const Options options_; + const BundleAdjustmentOptions ba_options_; + BundleAdjustmentConfig config_; + ceres::Solver::Summary summary_; + + size_t num_measurements_; + std::vector cameras_; + std::vector points3D_; + std::vector measurements_; + std::unordered_set camera_ids_; + std::unordered_set point3D_ids_; + std::vector camera_idxs_; + std::vector point3D_idxs_; + std::vector ordered_image_ids_; + std::vector ordered_point3D_ids_; + std::unordered_map image_id_to_camera_idx_; +}; + +class RigBundleAdjuster : public BundleAdjuster { + public: + struct Options { + // Whether to optimize the relative poses of the camera rigs. + bool refine_relative_poses = true; + + // The maximum allowed reprojection error for an observation to be + // considered in the bundle adjustment. Some observations might have large + // reprojection errors due to the concatenation of the absolute and relative + // rig poses, which might be different from the absolute pose of the image + // in the reconstruction. + double max_reproj_error = 1000.0; + }; + + RigBundleAdjuster(const BundleAdjustmentOptions& options, + const Options& rig_options, + const BundleAdjustmentConfig& config); + + bool Solve(Reconstruction* reconstruction, + std::vector* camera_rigs); + + private: + void SetUp(Reconstruction* reconstruction, + std::vector* camera_rigs, + ceres::LossFunction* loss_function); + void TearDown(Reconstruction* reconstruction, + const std::vector& camera_rigs); + + void AddImageToProblem(const image_t image_id, Reconstruction* reconstruction, + std::vector* camera_rigs, + ceres::LossFunction* loss_function); + + void AddPointToProblem(const point3D_t point3D_id, + Reconstruction* reconstruction, + ceres::LossFunction* loss_function); + + void ComputeCameraRigPoses(const Reconstruction& reconstruction, + const std::vector& camera_rigs); + + void ParameterizeCameraRigs(Reconstruction* reconstruction); + + const Options rig_options_; + + // Mapping from images to camera rigs. + std::unordered_map image_id_to_camera_rig_; + + // Mapping from images to the absolute camera rig poses. + std::unordered_map image_id_to_rig_qvec_; + std::unordered_map image_id_to_rig_tvec_; + + // For each camera rig, the absolute camera rig poses. + std::vector> camera_rig_qvecs_; + std::vector> camera_rig_tvecs_; + + // The Quaternions added to the problem, used to set the local + // parameterization once after setting up the problem. + std::unordered_set parameterized_qvec_data_; +}; + +void PrintSolverSummary(const ceres::Solver::Summary& summary); + +} // namespace colmap + +#endif // COLMAP_SRC_OPTIM_BUNDLE_ADJUSTMENT_H_ diff --git a/colmap/include/colmap/optim/combination_sampler.h b/colmap/include/colmap/optim/combination_sampler.h new file mode 100644 index 0000000000000000000000000000000000000000..027e6aca744125fce80642090f69c598d4de1f9f --- /dev/null +++ b/colmap/include/colmap/optim/combination_sampler.h @@ -0,0 +1,60 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_OPTIM_COMBINATION_SAMPLER_H_ +#define COLMAP_SRC_OPTIM_COMBINATION_SAMPLER_H_ + +#include "optim/sampler.h" + +namespace colmap { + +// Random sampler for RANSAC-based methods that generates unique samples. +// +// Note that a separate sampler should be instantiated per thread and it assumes +// that the input data is shuffled in advance. +class CombinationSampler : public Sampler { + public: + explicit CombinationSampler(const size_t num_samples); + + void Initialize(const size_t total_num_samples) override; + + size_t MaxNumSamples() override; + + std::vector Sample() override; + + private: + const size_t num_samples_; + std::vector total_sample_idxs_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_OPTIM_COMBINATION_SAMPLER_H_ diff --git a/colmap/include/colmap/optim/least_absolute_deviations.h b/colmap/include/colmap/optim/least_absolute_deviations.h new file mode 100644 index 0000000000000000000000000000000000000000..a9e593cabc89421d76eec001c886f9afe7a6af6a --- /dev/null +++ b/colmap/include/colmap/optim/least_absolute_deviations.h @@ -0,0 +1,72 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_OPTIM_LEAST_ABSOLUTE_DEVIATIONS_H_ +#define COLMAP_SRC_OPTIM_LEAST_ABSOLUTE_DEVIATIONS_H_ + +#include +#include + +#include "util/logging.h" + +namespace colmap { + +struct LeastAbsoluteDeviationsOptions { + // Augmented Lagrangian parameter. + double rho = 1.0; + + // Over-relaxation parameter, typical values are between 1.0 and 1.8. + double alpha = 1.0; + + // Maximum solver iterations. + int max_num_iterations = 1000; + + // Absolute and relative solution thresholds, as suggested by Boyd et al. + double absolute_tolerance = 1e-4; + double relative_tolerance = 1e-2; +}; + +// Least absolute deviations (LAD) fitting via ADMM by solving the problem: +// +// min || A x - b ||_1 +// +// The solution is returned in the vector x and the iterative solver is +// initialized with the given value. This implementation is based on the paper +// "Distributed Optimization and Statistical Learning via the Alternating +// Direction Method of Multipliers" by Boyd et al. and the Matlab implementation +// at https://web.stanford.edu/~boyd/papers/admm/least_abs_deviations/lad.html +bool SolveLeastAbsoluteDeviations(const LeastAbsoluteDeviationsOptions& options, + const Eigen::SparseMatrix& A, + const Eigen::VectorXd& b, Eigen::VectorXd* x); + +} // namespace colmap + +#endif // COLMAP_SRC_OPTIM_LEAST_ABSOLUTE_DEVIATIONS_H_ diff --git a/colmap/include/colmap/optim/loransac.h b/colmap/include/colmap/optim/loransac.h new file mode 100644 index 0000000000000000000000000000000000000000..5f0b95f455e29b61c34221e23cca68c0c2b0877d --- /dev/null +++ b/colmap/include/colmap/optim/loransac.h @@ -0,0 +1,253 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_OPTIM_LORANSAC_H_ +#define COLMAP_SRC_OPTIM_LORANSAC_H_ + +#include +#include +#include +#include + +#include "optim/random_sampler.h" +#include "optim/ransac.h" +#include "optim/support_measurement.h" +#include "util/alignment.h" +#include "util/logging.h" + +namespace colmap { + +// Implementation of LO-RANSAC (Locally Optimized RANSAC). +// +// "Locally Optimized RANSAC" Ondrej Chum, Jiri Matas, Josef Kittler, DAGM 2003. +template +class LORANSAC : public RANSAC { + public: + using typename RANSAC::Report; + + explicit LORANSAC(const RANSACOptions& options); + + // Robustly estimate model with RANSAC (RANdom SAmple Consensus). + // + // @param X Independent variables. + // @param Y Dependent variables. + // + // @return The report with the results of the estimation. + Report Estimate(const std::vector& X, + const std::vector& Y); + + // Objects used in RANSAC procedure. + using RANSAC::estimator; + LocalEstimator local_estimator; + using RANSAC::sampler; + using RANSAC::support_measurer; + + private: + using RANSAC::options_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +LORANSAC::LORANSAC( + const RANSACOptions& options) + : RANSAC(options) {} + +template +typename LORANSAC::Report +LORANSAC::Estimate( + const std::vector& X, + const std::vector& Y) { + CHECK_EQ(X.size(), Y.size()); + + const size_t num_samples = X.size(); + + typename RANSAC::Report report; + report.success = false; + report.num_trials = 0; + + if (num_samples < Estimator::kMinNumSamples) { + return report; + } + + typename SupportMeasurer::Support best_support; + typename Estimator::M_t best_model; + bool best_model_is_local = false; + + bool abort = false; + + const double max_residual = options_.max_error * options_.max_error; + + std::vector residuals; + std::vector best_local_residuals; + + std::vector X_inlier; + std::vector Y_inlier; + + std::vector X_rand(Estimator::kMinNumSamples); + std::vector Y_rand(Estimator::kMinNumSamples); + + sampler.Initialize(num_samples); + + size_t max_num_trials = options_.max_num_trials; + max_num_trials = std::min(max_num_trials, sampler.MaxNumSamples()); + size_t dyn_max_num_trials = max_num_trials; + + for (report.num_trials = 0; report.num_trials < max_num_trials; + ++report.num_trials) { + if (abort) { + report.num_trials += 1; + break; + } + + sampler.SampleXY(X, Y, &X_rand, &Y_rand); + + // Estimate model for current subset. + const std::vector sample_models = + estimator.Estimate(X_rand, Y_rand); + + // Iterate through all estimated models + for (const auto& sample_model : sample_models) { + estimator.Residuals(X, Y, sample_model, &residuals); + CHECK_EQ(residuals.size(), num_samples); + + const auto support = support_measurer.Evaluate(residuals, max_residual); + + // Do local optimization if better than all previous subsets. + if (support_measurer.Compare(support, best_support)) { + best_support = support; + best_model = sample_model; + best_model_is_local = false; + + // Estimate locally optimized model from inliers. + if (support.num_inliers > Estimator::kMinNumSamples && + support.num_inliers >= LocalEstimator::kMinNumSamples) { + // Recursive local optimization to expand inlier set. + const size_t kMaxNumLocalTrials = 10; + for (size_t local_num_trials = 0; + local_num_trials < kMaxNumLocalTrials; ++local_num_trials) { + X_inlier.clear(); + Y_inlier.clear(); + X_inlier.reserve(num_samples); + Y_inlier.reserve(num_samples); + for (size_t i = 0; i < residuals.size(); ++i) { + if (residuals[i] <= max_residual) { + X_inlier.push_back(X[i]); + Y_inlier.push_back(Y[i]); + } + } + + const std::vector local_models = + local_estimator.Estimate(X_inlier, Y_inlier); + + const size_t prev_best_num_inliers = best_support.num_inliers; + + for (const auto& local_model : local_models) { + local_estimator.Residuals(X, Y, local_model, &residuals); + CHECK_EQ(residuals.size(), num_samples); + + const auto local_support = + support_measurer.Evaluate(residuals, max_residual); + + // Check if locally optimized model is better. + if (support_measurer.Compare(local_support, best_support)) { + best_support = local_support; + best_model = local_model; + best_model_is_local = true; + std::swap(residuals, best_local_residuals); + } + } + + // Only continue recursive local optimization, if the inlier set + // size increased and we thus have a chance to further improve. + if (best_support.num_inliers <= prev_best_num_inliers) { + break; + } + + // Swap back the residuals, so we can extract the best inlier + // set in the next recursion of local optimization. + std::swap(residuals, best_local_residuals); + } + } + + dyn_max_num_trials = + RANSAC::ComputeNumTrials( + best_support.num_inliers, num_samples, options_.confidence, + options_.dyn_num_trials_multiplier); + } + + if (report.num_trials >= dyn_max_num_trials && + report.num_trials >= options_.min_num_trials) { + abort = true; + break; + } + } + } + + report.support = best_support; + report.model = best_model; + + // No valid model was found + if (report.support.num_inliers < estimator.kMinNumSamples) { + return report; + } + + report.success = true; + + // Determine inlier mask. Note that this calculates the residuals for the + // best model twice, but saves to copy and fill the inlier mask for each + // evaluated model. Some benchmarking revealed that this approach is faster. + + if (best_model_is_local) { + local_estimator.Residuals(X, Y, report.model, &residuals); + } else { + estimator.Residuals(X, Y, report.model, &residuals); + } + + CHECK_EQ(residuals.size(), num_samples); + + report.inlier_mask.resize(num_samples); + for (size_t i = 0; i < residuals.size(); ++i) { + report.inlier_mask[i] = residuals[i] <= max_residual; + } + + return report; +} + +} // namespace colmap + +#endif // COLMAP_SRC_OPTIM_LORANSAC_H_ diff --git a/colmap/include/colmap/optim/progressive_sampler.h b/colmap/include/colmap/optim/progressive_sampler.h new file mode 100644 index 0000000000000000000000000000000000000000..c178b9ce15c912e09a49e07d26fdf9bd82b70f52 --- /dev/null +++ b/colmap/include/colmap/optim/progressive_sampler.h @@ -0,0 +1,73 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_OPTIM_PROGRESSIVE_SAMPLER_H_ +#define COLMAP_SRC_OPTIM_PROGRESSIVE_SAMPLER_H_ + +#include "optim/sampler.h" + +namespace colmap { + +// Random sampler for PROSAC (Progressive Sample Consensus), as described in: +// +// "Matching with PROSAC - Progressive Sample Consensus". +// Ondrej Chum and Matas, CVPR 2005. +// +// Note that a separate sampler should be instantiated per thread and that the +// data to be sampled from is assumed to be sorted according to the quality +// function in descending order, i.e., higher quality data is closer to the +// front of the list. +class ProgressiveSampler : public Sampler { + public: + explicit ProgressiveSampler(const size_t num_samples); + + void Initialize(const size_t total_num_samples) override; + + size_t MaxNumSamples() override; + + std::vector Sample() override; + + private: + const size_t num_samples_; + size_t total_num_samples_; + + // The number of generated samples, i.e. the number of calls to `Sample`. + size_t t_; + size_t n_; + + // Variables defined in equation 3. + double T_n_; + double T_n_p_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_OPTIM_PROGRESSIVE_SAMPLER_H_ diff --git a/colmap/include/colmap/optim/random_sampler.h b/colmap/include/colmap/optim/random_sampler.h new file mode 100644 index 0000000000000000000000000000000000000000..1efa88436fecc9c119faaf0696010ea12efbd8ee --- /dev/null +++ b/colmap/include/colmap/optim/random_sampler.h @@ -0,0 +1,59 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_OPTIM_RANDOM_SAMPLER_H_ +#define COLMAP_SRC_OPTIM_RANDOM_SAMPLER_H_ + +#include "optim/sampler.h" + +namespace colmap { + +// Random sampler for RANSAC-based methods. +// +// Note that a separate sampler should be instantiated per thread. +class RandomSampler : public Sampler { + public: + explicit RandomSampler(const size_t num_samples); + + void Initialize(const size_t total_num_samples) override; + + size_t MaxNumSamples() override; + + std::vector Sample() override; + + private: + const size_t num_samples_; + std::vector sample_idxs_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_OPTIM_RANDOM_SAMPLER_H_ diff --git a/colmap/include/colmap/optim/ransac.h b/colmap/include/colmap/optim/ransac.h new file mode 100644 index 0000000000000000000000000000000000000000..4d30385953fc1353764ddf41057cb73fce938d7d --- /dev/null +++ b/colmap/include/colmap/optim/ransac.h @@ -0,0 +1,282 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_OPTIM_RANSAC_H_ +#define COLMAP_SRC_OPTIM_RANSAC_H_ + +#include +#include +#include +#include + +#include "optim/random_sampler.h" +#include "optim/support_measurement.h" +#include "util/alignment.h" +#include "util/logging.h" + +namespace colmap { + +struct RANSACOptions { + // Maximum error for a sample to be considered as an inlier. Note that + // the residual of an estimator corresponds to a squared error. + double max_error = 0.0; + + // A priori assumed minimum inlier ratio, which determines the maximum number + // of iterations. Only applies if smaller than `max_num_trials`. + double min_inlier_ratio = 0.1; + + // Abort the iteration if minimum probability that one sample is free from + // outliers is reached. + double confidence = 0.99; + + // The num_trials_multiplier to the dynamically computed maximum number of + // iterations based on the specified confidence value. + double dyn_num_trials_multiplier = 3.0; + + // Number of random trials to estimate model from random subset. + size_t min_num_trials = 0; + size_t max_num_trials = std::numeric_limits::max(); + + void Check() const { + CHECK_GT(max_error, 0); + CHECK_GE(min_inlier_ratio, 0); + CHECK_LE(min_inlier_ratio, 1); + CHECK_GE(confidence, 0); + CHECK_LE(confidence, 1); + CHECK_LE(min_num_trials, max_num_trials); + } +}; + +template +class RANSAC { + public: + struct Report { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + // Whether the estimation was successful. + bool success = false; + + // The number of RANSAC trials / iterations. + size_t num_trials = 0; + + // The support of the estimated model. + typename SupportMeasurer::Support support; + + // Boolean mask which is true if a sample is an inlier. + std::vector inlier_mask; + + // The estimated model. + typename Estimator::M_t model; + }; + + explicit RANSAC(const RANSACOptions& options); + + // Determine the maximum number of trials required to sample at least one + // outlier-free random set of samples with the specified confidence, + // given the inlier ratio. + // + // @param num_inliers The number of inliers. + // @param num_samples The total number of samples. + // @param confidence Confidence that one sample is + // outlier-free. + // @param num_trials_multiplier Multiplication factor to the computed + // number of trials. + // + // @return The required number of iterations. + static size_t ComputeNumTrials(const size_t num_inliers, + const size_t num_samples, + const double confidence, + const double num_trials_multiplier); + + // Robustly estimate model with RANSAC (RANdom SAmple Consensus). + // + // @param X Independent variables. + // @param Y Dependent variables. + // + // @return The report with the results of the estimation. + Report Estimate(const std::vector& X, + const std::vector& Y); + + // Objects used in RANSAC procedure. Access useful to define custom behavior + // through options or e.g. to compute residuals. + Estimator estimator; + Sampler sampler; + SupportMeasurer support_measurer; + + protected: + RANSACOptions options_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +RANSAC::RANSAC( + const RANSACOptions& options) + : sampler(Sampler(Estimator::kMinNumSamples)), options_(options) { + options.Check(); + + // Determine max_num_trials based on assumed `min_inlier_ratio`. + const size_t kNumSamples = 100000; + const size_t dyn_max_num_trials = ComputeNumTrials( + static_cast(options_.min_inlier_ratio * kNumSamples), kNumSamples, + options_.confidence, options_.dyn_num_trials_multiplier); + options_.max_num_trials = + std::min(options_.max_num_trials, dyn_max_num_trials); +} + +template +size_t RANSAC::ComputeNumTrials( + const size_t num_inliers, const size_t num_samples, const double confidence, + const double num_trials_multiplier) { + const double inlier_ratio = num_inliers / static_cast(num_samples); + + const double nom = 1 - confidence; + if (nom <= 0) { + return std::numeric_limits::max(); + } + + const double denom = 1 - std::pow(inlier_ratio, Estimator::kMinNumSamples); + if (denom <= 0) { + return 1; + } + // Prevent divide by zero below. + if (denom == 1.0) { + return std::numeric_limits::max(); + } + + return static_cast( + std::ceil(std::log(nom) / std::log(denom) * num_trials_multiplier)); +} + +template +typename RANSAC::Report +RANSAC::Estimate( + const std::vector& X, + const std::vector& Y) { + CHECK_EQ(X.size(), Y.size()); + + const size_t num_samples = X.size(); + + Report report; + report.success = false; + report.num_trials = 0; + + if (num_samples < Estimator::kMinNumSamples) { + return report; + } + + typename SupportMeasurer::Support best_support; + typename Estimator::M_t best_model; + + bool abort = false; + + const double max_residual = options_.max_error * options_.max_error; + + std::vector residuals(num_samples); + + std::vector X_rand(Estimator::kMinNumSamples); + std::vector Y_rand(Estimator::kMinNumSamples); + + sampler.Initialize(num_samples); + + size_t max_num_trials = options_.max_num_trials; + max_num_trials = std::min(max_num_trials, sampler.MaxNumSamples()); + size_t dyn_max_num_trials = max_num_trials; + + for (report.num_trials = 0; report.num_trials < max_num_trials; + ++report.num_trials) { + if (abort) { + report.num_trials += 1; + break; + } + + sampler.SampleXY(X, Y, &X_rand, &Y_rand); + + // Estimate model for current subset. + const std::vector sample_models = + estimator.Estimate(X_rand, Y_rand); + + // Iterate through all estimated models. + for (const auto& sample_model : sample_models) { + estimator.Residuals(X, Y, sample_model, &residuals); + CHECK_EQ(residuals.size(), num_samples); + + const auto support = support_measurer.Evaluate(residuals, max_residual); + + // Save as best subset if better than all previous subsets. + if (support_measurer.Compare(support, best_support)) { + best_support = support; + best_model = sample_model; + + dyn_max_num_trials = ComputeNumTrials( + best_support.num_inliers, num_samples, options_.confidence, + options_.dyn_num_trials_multiplier); + } + + if (report.num_trials >= dyn_max_num_trials && + report.num_trials >= options_.min_num_trials) { + abort = true; + break; + } + } + } + + report.support = best_support; + report.model = best_model; + + // No valid model was found. + if (report.support.num_inliers < estimator.kMinNumSamples) { + return report; + } + + report.success = true; + + // Determine inlier mask. Note that this calculates the residuals for the + // best model twice, but saves to copy and fill the inlier mask for each + // evaluated model. Some benchmarking revealed that this approach is faster. + + estimator.Residuals(X, Y, report.model, &residuals); + CHECK_EQ(residuals.size(), num_samples); + + report.inlier_mask.resize(num_samples); + for (size_t i = 0; i < residuals.size(); ++i) { + report.inlier_mask[i] = residuals[i] <= max_residual; + } + + return report; +} + +} // namespace colmap + +#endif // COLMAP_SRC_OPTIM_RANSAC_H_ diff --git a/colmap/include/colmap/optim/sampler.h b/colmap/include/colmap/optim/sampler.h new file mode 100644 index 0000000000000000000000000000000000000000..f1cb45c92a2ac738dd212a38d8e3b2473e8683d0 --- /dev/null +++ b/colmap/include/colmap/optim/sampler.h @@ -0,0 +1,97 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_OPTIM_SAMPLER_H_ +#define COLMAP_SRC_OPTIM_SAMPLER_H_ + +#include +#include + +#include "util/logging.h" + +namespace colmap { + +// Abstract base class for sampling methods. +class Sampler { + public: + Sampler(){}; + explicit Sampler(const size_t num_samples); + + // Initialize the sampler, before calling the `Sample` method. + virtual void Initialize(const size_t total_num_samples) = 0; + + // Maximum number of unique samples that can be generated. + virtual size_t MaxNumSamples() = 0; + + // Sample `num_samples` elements from all samples. + virtual std::vector Sample() = 0; + + // Sample elements from `X` into `X_rand`. + // + // Note that `X.size()` should equal `num_total_samples` and `X_rand.size()` + // should equal `num_samples`. + template + void SampleX(const X_t& X, X_t* X_rand); + + // Sample elements from `X` and `Y` into `X_rand` and `Y_rand`. + // + // Note that `X.size()` should equal `num_total_samples` and `X_rand.size()` + // should equal `num_samples`. The same applies for `Y` and `Y_rand`. + template + void SampleXY(const X_t& X, const Y_t& Y, X_t* X_rand, Y_t* Y_rand); +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +void Sampler::SampleX(const X_t& X, X_t* X_rand) { + const auto sample_idxs = Sample(); + for (size_t i = 0; i < X_rand->size(); ++i) { + (*X_rand)[i] = X[sample_idxs[i]]; + } +} + +template +void Sampler::SampleXY(const X_t& X, const Y_t& Y, X_t* X_rand, Y_t* Y_rand) { + CHECK_EQ(X.size(), Y.size()); + CHECK_EQ(X_rand->size(), Y_rand->size()); + const auto sample_idxs = Sample(); + for (size_t i = 0; i < X_rand->size(); ++i) { + (*X_rand)[i] = X[sample_idxs[i]]; + (*Y_rand)[i] = Y[sample_idxs[i]]; + } +} + +} // namespace colmap + +#endif // COLMAP_SRC_OPTIM_SAMPLER_H_ diff --git a/colmap/include/colmap/optim/sprt.h b/colmap/include/colmap/optim/sprt.h new file mode 100644 index 0000000000000000000000000000000000000000..7a72fe4e76bcec2fdb4fd2037e6932a75ac9757c --- /dev/null +++ b/colmap/include/colmap/optim/sprt.h @@ -0,0 +1,83 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_OPTIM_SPRT_H_ +#define COLMAP_SRC_OPTIM_SPRT_H_ + +#include +#include +#include + +namespace colmap { + +// Sequential Probability Ratio Test as proposed in +// +// "Randomized RANSAC with Sequential Probability Ratio Test", +// Matas et al., 2005 +class SPRT { + public: + struct Options { + // Probability of rejecting a good model. + double delta = 0.01; + + // A priori assumed minimum inlier ratio + double epsilon = 0.1; + + // The ratio of the time it takes to estimate a model from a random sample + // over the time it takes to decide whether one data sample is an + // inlier or not. Matas et al. propose 200 for the 7-point algorithm. + double eval_time_ratio = 200; + + // Number of models per random sample, that have to be verified. E.g. 1-3 + // for the 7-point fundamental matrix algorithm, or 1-10 for the 5-point + // essential matrix algorithm. + int num_models_per_sample = 1; + }; + + explicit SPRT(const Options& options); + + void Update(const Options& options); + + bool Evaluate(const std::vector& residuals, const double max_residual, + size_t* num_inliers, size_t* num_eval_samples); + + private: + void UpdateDecisionThreshold(); + + Options options_; + double delta_epsilon_; + double delta_1_epsilon_1_; + double decision_threshold_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_OPTIM_SPRT_H_ diff --git a/colmap/include/colmap/optim/support_measurement.h b/colmap/include/colmap/optim/support_measurement.h new file mode 100644 index 0000000000000000000000000000000000000000..45a1d31168385441fea186a06f59571892d970c9 --- /dev/null +++ b/colmap/include/colmap/optim/support_measurement.h @@ -0,0 +1,82 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_OPTIM_SUPPORT_MEASUREMENT_H_ +#define COLMAP_SRC_OPTIM_SUPPORT_MEASUREMENT_H_ + +#include +#include +#include + +namespace colmap { + +// Measure the support of a model by counting the number of inliers and +// summing all inlier residuals. The support is better if it has more inliers +// and a smaller residual sum. +struct InlierSupportMeasurer { + struct Support { + // The number of inliers. + size_t num_inliers = 0; + + // The sum of all inlier residuals. + double residual_sum = std::numeric_limits::max(); + }; + + // Compute the support of the residuals. + Support Evaluate(const std::vector& residuals, + const double max_residual); + + // Compare the two supports and return the better support. + bool Compare(const Support& support1, const Support& support2); +}; + +// Measure the support of a model by its fitness to the data as used in MSAC. +// A support is better if it has a smaller MSAC score. +struct MEstimatorSupportMeasurer { + struct Support { + // The number of inliers. + size_t num_inliers = 0; + + // The MSAC score, defined as the truncated sum of residuals. + double score = std::numeric_limits::max(); + }; + + // Compute the support of the residuals. + Support Evaluate(const std::vector& residuals, + const double max_residual); + + // Compare the two supports and return the better support. + bool Compare(const Support& support1, const Support& support2); +}; + +} // namespace colmap + +#endif // COLMAP_SRC_OPTIM_SUPPORT_MEASUREMENT_H_ diff --git a/colmap/include/colmap/retrieval/geometry.h b/colmap/include/colmap/retrieval/geometry.h new file mode 100644 index 0000000000000000000000000000000000000000..44bea78bd0cb379d19633382a2ec61180c458b17 --- /dev/null +++ b/colmap/include/colmap/retrieval/geometry.h @@ -0,0 +1,78 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_RETRIEVAL_GEOMETRY_H_ +#define COLMAP_SRC_RETRIEVAL_GEOMETRY_H_ + +#include + +#include + +namespace colmap { +namespace retrieval { + +struct FeatureGeometryTransform { + float scale = 0.0f; + float angle = 0.0f; + float tx = 0.0f; + float ty = 0.0f; +}; + +struct FeatureGeometry { + // Compute the similarity that transforms the shape of feature 1 to feature 2. + static FeatureGeometryTransform TransformFromMatch( + const FeatureGeometry& feature1, const FeatureGeometry& feature2); + static Eigen::Matrix TransformMatrixFromMatch( + const FeatureGeometry& feature1, const FeatureGeometry& feature2); + + // Get the approximate area occupied by the feature. + float GetArea() const; + + // Get the approximate area occupied by the feature after applying an affine + // transformation to the feature geometry. + float GetAreaUnderTransform(const Eigen::Matrix2f& A) const; + + float x = 0.0f; + float y = 0.0f; + float scale = 0.0f; + float orientation = 0.0f; +}; + +// 1-to-M feature geometry match. +struct FeatureGeometryMatch { + FeatureGeometry geometry1; + std::vector geometries2; +}; + +} // namespace retrieval +} // namespace colmap + +#endif // COLMAP_SRC_RETRIEVAL_GEOMETRY_H_ diff --git a/colmap/include/colmap/retrieval/inverted_file.h b/colmap/include/colmap/retrieval/inverted_file.h new file mode 100644 index 0000000000000000000000000000000000000000..1469c8b58ca31d3559f88101f18c554184fd62d5 --- /dev/null +++ b/colmap/include/colmap/retrieval/inverted_file.h @@ -0,0 +1,417 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_RETRIEVAL_INVERTED_FILE_H_ +#define COLMAP_SRC_RETRIEVAL_INVERTED_FILE_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "retrieval/geometry.h" +#include "retrieval/inverted_file_entry.h" +#include "retrieval/utils.h" +#include "util/alignment.h" +#include "util/logging.h" +#include "util/math.h" + +namespace colmap { +namespace retrieval { + +// Implements an inverted file, including the ability to compute image scores +// and matches. The template parameter is the length of the binary vectors +// in the Hamming Embedding. +// This class is based on an original implementation by Torsten Sattler. +template +class InvertedFile { + public: + typedef Eigen::VectorXf DescType; + typedef FeatureGeometry GeomType; + typedef InvertedFileEntry EntryType; + + enum Status { + UNUSABLE = 0x00, + HAS_EMBEDDING = 0x01, + ENTRIES_SORTED = 0x02, + USABLE = 0x03, + }; + + InvertedFile(); + + // The number of added entries. + size_t NumEntries() const; + + // Return all entries in the file. + const std::vector& GetEntries() const; + + // Whether the Hamming embedding was computed for this file. + bool HasHammingEmbedding() const; + + // Whether the entries in this file are sorted. + bool EntriesSorted() const; + + // Whether this file is usable for scoring, i.e. the entries are sorted and + // the Hamming embedding has been computed. + bool IsUsable() const; + + // Adds an inverted file entry given a projected descriptor and its image + // information stored in an inverted file entry. In particular, this function + // generates the binary descriptor for the inverted file entry and then stores + // the entry in the inverted file. + void AddEntry(const int image_id, typename DescType::Index feature_idx, + const DescType& descriptor, const GeomType& geometry); + + // Sorts the inverted file entries in ascending order of image ids. This is + // required for efficient scoring and must be called before ScoreFeature. + void SortEntries(); + + // Clear all entries in this file. + void ClearEntries(); + + // Reset all computed weights/thresholds and clear all entries. + void Reset(); + + // Given a projected descriptor, returns the corresponding binary string. + void ConvertToBinaryDescriptor( + const DescType& descriptor, + std::bitset* binary_descriptor) const; + + // Compute the idf-weight for this inverted file. + void ComputeIDFWeight(const int num_total_images); + + // Return the idf-weight of this inverted file. + float IDFWeight() const; + + // Given a set of descriptors, learns the thresholds required for the Hamming + // embedding. Each row in descriptors represents a single descriptor projected + // into the kEmbeddingDim dimensional space used for Hamming embedding. + void ComputeHammingEmbedding( + const Eigen::Matrix& descriptors); + + // Given a query feature, performs inverted file scoring. + void ScoreFeature(const DescType& descriptor, + std::vector* image_scores) const; + + // Get the identifiers of all indexed images in this file. + void GetImageIds(std::unordered_set* ids) const; + + // For each image in the inverted file, computes the self-similarity of each + // image in the file (the part caused by this word) and adds the weight to the + // entry corresponding to that image. This function is useful to determine the + // normalization factor for each image that is used during retrieval. + void ComputeImageSelfSimilarities( + std::unordered_map* self_similarities) const; + + // Read/write the inverted file from/to a binary file. + void Read(std::ifstream* ifs); + void Write(std::ofstream* ofs) const; + + private: + // Whether the inverted file is initialized. + uint8_t status_; + + // The inverse document frequency weight of this inverted file. + float idf_weight_; + + // The entries of the inverted file system. + std::vector entries_; + + // The thresholds used for Hamming embedding. + DescType thresholds_; + + // The functor to derive a voting weight from a Hamming distance. + static const HammingDistWeightFunctor + hamming_dist_weight_functor_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +const HammingDistWeightFunctor + InvertedFile::hamming_dist_weight_functor_; + +template +InvertedFile::InvertedFile() + : status_(UNUSABLE), idf_weight_(0.0f) { + static_assert(kEmbeddingDim % 8 == 0, + "Dimensionality of projected space needs to" + " be a multiple of 8."); + static_assert(kEmbeddingDim > 0, + "Dimensionality of projected space needs to be > 0."); + + thresholds_.resize(kEmbeddingDim); + thresholds_.setZero(); +} + +template +size_t InvertedFile::NumEntries() const { + return entries_.size(); +} + +template +const std::vector::EntryType>& +InvertedFile::GetEntries() const { + return entries_; +} + +template +bool InvertedFile::HasHammingEmbedding() const { + return status_ & HAS_EMBEDDING; +} + +template +bool InvertedFile::EntriesSorted() const { + return status_ & ENTRIES_SORTED; +} + +template +bool InvertedFile::IsUsable() const { + return status_ & USABLE; +} + +template +void InvertedFile::AddEntry(const int image_id, + typename DescType::Index feature_idx, + const DescType& descriptor, + const GeomType& geometry) { + CHECK_GE(image_id, 0); + CHECK_EQ(descriptor.size(), kEmbeddingDim); + EntryType entry; + entry.image_id = image_id; + entry.feature_idx = feature_idx; + entry.geometry = geometry; + ConvertToBinaryDescriptor(descriptor, &entry.descriptor); + entries_.push_back(entry); + status_ &= ~ENTRIES_SORTED; +} + +template +void InvertedFile::SortEntries() { + std::sort(entries_.begin(), entries_.end(), + [](const EntryType& entry1, const EntryType& entry2) { + return entry1.image_id < entry2.image_id; + }); + status_ |= ENTRIES_SORTED; +} + +template +void InvertedFile::ClearEntries() { + entries_.clear(); + status_ &= ~ENTRIES_SORTED; +} + +template +void InvertedFile::Reset() { + status_ = UNUSABLE; + idf_weight_ = 0.0f; + entries_.clear(); + thresholds_.setZero(); +} + +template +void InvertedFile::ConvertToBinaryDescriptor( + const DescType& descriptor, + std::bitset* binary_descriptor) const { + CHECK_EQ(descriptor.size(), kEmbeddingDim); + for (int i = 0; i < kEmbeddingDim; ++i) { + (*binary_descriptor)[i] = descriptor[i] > thresholds_[i]; + } +} + +template +void InvertedFile::ComputeIDFWeight(const int num_total_images) { + if (entries_.empty()) { + return; + } + + std::unordered_set image_ids; + GetImageIds(&image_ids); + + idf_weight_ = std::log(static_cast(num_total_images) / + static_cast(image_ids.size())); +} + +template +float InvertedFile::IDFWeight() const { + return idf_weight_; +} + +template +void InvertedFile::ComputeHammingEmbedding( + const Eigen::Matrix& descriptors) { + const int num_descriptors = static_cast(descriptors.rows()); + if (num_descriptors < 2) { + return; + } + + std::vector elements(num_descriptors); + for (int n = 0; n < kEmbeddingDim; ++n) { + for (int i = 0; i < num_descriptors; ++i) { + elements[i] = descriptors(i, n); + } + thresholds_[n] = Median(elements); + } + + status_ |= HAS_EMBEDDING; +} + +template +void InvertedFile::ScoreFeature( + const DescType& descriptor, std::vector* image_scores) const { + CHECK_EQ(descriptor.size(), kEmbeddingDim); + + image_scores->clear(); + + if (!IsUsable()) { + return; + } + + if (entries_.size() == 0) { + return; + } + + const float squared_idf_weight = idf_weight_ * idf_weight_; + + std::bitset bin_descriptor; + ConvertToBinaryDescriptor(descriptor, &bin_descriptor); + + ImageScore image_score; + image_score.image_id = entries_.front().image_id; + image_score.score = 0.0f; + int num_image_votes = 0; + + // Note that this assumes that the entries are sorted using SortEntries + // according to their image identifiers. + for (const auto& entry : entries_) { + if (image_score.image_id < entry.image_id) { + if (num_image_votes > 0) { + // Finalizes the voting since we now know how many features from + // the database image match the current image feature. This is + // required to perform burstiness normalization (cf. Eqn. 2 in + // Arandjelovic, Zisserman: Scalable descriptor + // distinctiveness for location recognition. ACCV 2014). + // Notice that the weight from the descriptor matching is already + // accumulated in image_score.score, i.e., we only need + // to apply the burstiness weighting. + image_score.score /= std::sqrt(static_cast(num_image_votes)); + image_score.score *= squared_idf_weight; + image_scores->push_back(image_score); + } + + image_score.image_id = entry.image_id; + image_score.score = 0.0f; + num_image_votes = 0; + } + + const size_t hamming_dist = (bin_descriptor ^ entry.descriptor).count(); + + if (hamming_dist <= hamming_dist_weight_functor_.kMaxHammingDistance) { + image_score.score += hamming_dist_weight_functor_(hamming_dist); + num_image_votes += 1; + } + } + + // Add the voting for the largest image_id in the entries. + if (num_image_votes > 0) { + image_score.score /= std::sqrt(static_cast(num_image_votes)); + image_score.score *= squared_idf_weight; + image_scores->push_back(image_score); + } +} + +template +void InvertedFile::GetImageIds( + std::unordered_set* ids) const { + for (const EntryType& entry : entries_) { + ids->insert(entry.image_id); + } +} + +template +void InvertedFile::ComputeImageSelfSimilarities( + std::unordered_map* self_similarities) const { + const double squared_idf_weight = idf_weight_ * idf_weight_; + for (const auto& entry : entries_) { + (*self_similarities)[entry.image_id] += squared_idf_weight; + } +} + +template +void InvertedFile::Read(std::ifstream* ifs) { + CHECK(ifs->is_open()); + + ifs->read(reinterpret_cast(&status_), sizeof(uint8_t)); + ifs->read(reinterpret_cast(&idf_weight_), sizeof(float)); + + for (int i = 0; i < kEmbeddingDim; ++i) { + ifs->read(reinterpret_cast(&thresholds_[i]), sizeof(float)); + } + + uint32_t num_entries = 0; + ifs->read(reinterpret_cast(&num_entries), sizeof(uint32_t)); + entries_.resize(num_entries); + + for (uint32_t i = 0; i < num_entries; ++i) { + entries_[i].Read(ifs); + } +} + +template +void InvertedFile::Write(std::ofstream* ofs) const { + CHECK(ofs->is_open()); + + ofs->write(reinterpret_cast(&status_), sizeof(uint8_t)); + ofs->write(reinterpret_cast(&idf_weight_), sizeof(float)); + + for (int i = 0; i < kEmbeddingDim; ++i) { + ofs->write(reinterpret_cast(&thresholds_[i]), sizeof(float)); + } + + const uint32_t num_entries = static_cast(entries_.size()); + ofs->write(reinterpret_cast(&num_entries), sizeof(uint32_t)); + + for (uint32_t i = 0; i < num_entries; ++i) { + entries_[i].Write(ofs); + } +} + +} // namespace retrieval +} // namespace colmap + +#endif // COLMAP_SRC_RETRIEVAL_INVERTED_FILE_H_ diff --git a/colmap/include/colmap/retrieval/inverted_file_entry.h b/colmap/include/colmap/retrieval/inverted_file_entry.h new file mode 100644 index 0000000000000000000000000000000000000000..a1df71bf9717819d5177c1fa06ef2b70450fe507 --- /dev/null +++ b/colmap/include/colmap/retrieval/inverted_file_entry.h @@ -0,0 +1,113 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_RETRIEVAL_INVERTED_FILE_ENTRY_H_ +#define COLMAP_SRC_RETRIEVAL_INVERTED_FILE_ENTRY_H_ + +#include +#include + +#include "retrieval/geometry.h" + +namespace colmap { +namespace retrieval { + +// An inverted file entry. The template defines the dimensionality of the binary +// string used to approximate the descriptor in the Hamming space. +// This class is based on an original implementation by Torsten Sattler. +template +struct InvertedFileEntry { + void Read(std::istream* ifs); + void Write(std::ostream* ofs) const; + + // The identifier of the image this entry is associated with. + int image_id = -1; + + // The index of the feature within the image's keypoints list. + int feature_idx = -1; + + // The geometry of the feature, used for spatial verification. + FeatureGeometry geometry; + + // The binary signature in the Hamming embedding. + std::bitset descriptor; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +void InvertedFileEntry::Read(std::istream* ifs) { + static_assert(N <= 64, "Dimensionality too large"); + static_assert(sizeof(unsigned long long) >= 8, + "Expected unsigned long to be at least 8 byte"); + static_assert(sizeof(FeatureGeometry) == 16, "Geometry type size mismatch"); + + int32_t image_id_data = 0; + ifs->read(reinterpret_cast(&image_id_data), sizeof(int32_t)); + image_id = static_cast(image_id_data); + + int32_t feature_idx_data = 0; + ifs->read(reinterpret_cast(&feature_idx_data), sizeof(int32_t)); + feature_idx = static_cast(feature_idx_data); + + ifs->read(reinterpret_cast(&geometry), sizeof(FeatureGeometry)); + + uint64_t descriptor_data = 0; + ifs->read(reinterpret_cast(&descriptor_data), sizeof(uint64_t)); + descriptor = std::bitset(descriptor_data); +} + +template +void InvertedFileEntry::Write(std::ostream* ofs) const { + static_assert(N <= 64, "Dimensionality too large"); + static_assert(sizeof(unsigned long long) >= 8, + "Expected unsigned long to be at least 8 byte"); + static_assert(sizeof(FeatureGeometry) == 16, "Geometry type size mismatch"); + + const int32_t image_id_data = image_id; + ofs->write(reinterpret_cast(&image_id_data), sizeof(int32_t)); + + const int32_t feature_idx_data = feature_idx; + ofs->write(reinterpret_cast(&feature_idx_data), sizeof(int32_t)); + + ofs->write(reinterpret_cast(&geometry), sizeof(FeatureGeometry)); + + const uint64_t descriptor_data = + static_cast(descriptor.to_ullong()); + ofs->write(reinterpret_cast(&descriptor_data), sizeof(uint64_t)); +} + +} // namespace retrieval +} // namespace colmap + +#endif // COLMAP_SRC_RETRIEVAL_INVERTED_FILE_ENTRY_H_ diff --git a/colmap/include/colmap/retrieval/inverted_index.h b/colmap/include/colmap/retrieval/inverted_index.h new file mode 100644 index 0000000000000000000000000000000000000000..12c9a489223763d88eba899f58552b84003ca8af --- /dev/null +++ b/colmap/include/colmap/retrieval/inverted_index.h @@ -0,0 +1,446 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_RETRIEVAL_INVERTED_INDEX_H_ +#define COLMAP_SRC_RETRIEVAL_INVERTED_INDEX_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "retrieval/inverted_file.h" +#include "util/alignment.h" +#include "util/random.h" + +namespace colmap { +namespace retrieval { + +// Implements an inverted index system. The template parameter is the length of +// the binary vectors in the Hamming Embedding. +// This class is based on an original implementation by Torsten Sattler. +template +class InvertedIndex { + public: + const static int kInvalidWordId; + typedef Eigen::Matrix + DescType; + typedef typename InvertedFile::EntryType EntryType; + typedef typename InvertedFile::GeomType GeomType; + typedef Eigen::Matrix ProjMatrixType; + typedef Eigen::VectorXf ProjDescType; + + InvertedIndex(); + + // The number of visual words in the index. + int NumVisualWords() const; + + // Initializes the inverted index with num_words empty inverted files. + void Initialize(const int num_words); + + // Finalizes the inverted index by sorting each inverted file such that all + // entries are in ascending order of image ids. + void Finalize(); + + // Generate projection matrix for Hamming embedding. + void GenerateHammingEmbeddingProjection(); + + // Compute Hamming embedding thresholds from a set of descriptors with + // given visual word identifies. + void ComputeHammingEmbedding(const DescType& descriptors, + const Eigen::VectorXi& word_ids); + + // Add single entry to the index. + void AddEntry(const int image_id, const int word_id, + typename DescType::Index feature_idx, + const DescType& descriptor, const GeomType& geometry); + + // Clear all index entries. + void ClearEntries(); + + // Query the inverted file and return a list of sorted images. + void Query(const DescType& descriptors, const Eigen::MatrixXi& word_ids, + std::vector* image_scores) const; + + void ConvertToBinaryDescriptor( + const int word_id, const DescType& descriptor, + std::bitset* binary_descriptor) const; + + float GetIDFWeight(const int word_id) const; + + void FindMatches(const int word_id, const std::unordered_set& image_ids, + std::vector* matches) const; + + // Compute the self-similarity for the image. + float ComputeSelfSimilarity(const Eigen::MatrixXi& word_ids) const; + + // Get the identifiers of all indexed images. + void GetImageIds(std::unordered_set* image_ids) const; + + // Read/write the inverted index from/to a binary file. + void Read(std::ifstream* ifs); + void Write(std::ofstream* ofs) const; + + private: + void ComputeWeightsAndNormalizationConstants(); + + // The individual inverted indices. + std::vector, + Eigen::aligned_allocator>> + inverted_files_; + + // For each image in the database, a normalization factor to be used to + // normalize the votes. + std::unordered_map normalization_constants_; + + // The projection matrix used to project SIFT descriptors. + ProjMatrixType proj_matrix_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +const int InvertedIndex::kInvalidWordId = + std::numeric_limits::max(); + +template +InvertedIndex::InvertedIndex() { + proj_matrix_.resize(kEmbeddingDim, kDescDim); + proj_matrix_.setIdentity(); +} + +template +int InvertedIndex::NumVisualWords() const { + return static_cast(inverted_files_.size()); +} + +template +void InvertedIndex::Initialize( + const int num_words) { + CHECK_GT(num_words, 0); + inverted_files_.resize(num_words); + for (auto& inverted_file : inverted_files_) { + inverted_file.Reset(); + } +} + +template +void InvertedIndex::Finalize() { + CHECK_GT(NumVisualWords(), 0); + + for (auto& inverted_file : inverted_files_) { + inverted_file.SortEntries(); + } + + ComputeWeightsAndNormalizationConstants(); +} + +template +void InvertedIndex::GenerateHammingEmbeddingProjection() { + Eigen::MatrixXf random_matrix(kDescDim, kDescDim); + for (Eigen::MatrixXf::Index i = 0; i < random_matrix.size(); ++i) { + random_matrix(i) = RandomGaussian(0.0f, 1.0f); + } + const Eigen::MatrixXf Q = random_matrix.colPivHouseholderQr().matrixQ(); + proj_matrix_ = Q.topRows(); +} + +template +void InvertedIndex::ComputeHammingEmbedding( + const DescType& descriptors, const Eigen::VectorXi& word_ids) { + CHECK_EQ(descriptors.rows(), word_ids.rows()); + CHECK_EQ(descriptors.cols(), kDescDim); + + // Skip every inverted file with less than kMinEntries entries. + const size_t kMinEntries = 5; + + // Determines for each word the corresponding descriptors. + std::vector> indices_per_word(NumVisualWords()); + for (Eigen::MatrixXi::Index i = 0; i < word_ids.rows(); ++i) { + indices_per_word.at(word_ids(i)).push_back(i); + } + + // For each word, learn the Hamming embedding threshold and the local + // descriptor space densities. + for (int i = 0; i < NumVisualWords(); ++i) { + const auto& indices = indices_per_word[i]; + if (indices.size() < kMinEntries) { + continue; + } + + Eigen::Matrix proj_desc( + indices.size(), kEmbeddingDim); + for (size_t j = 0; j < indices.size(); ++j) { + proj_desc.row(j) = + proj_matrix_ * + descriptors.row(indices[j]).transpose().template cast(); + } + + inverted_files_[i].ComputeHammingEmbedding(proj_desc); + } +} + +template +void InvertedIndex::AddEntry( + const int image_id, const int word_id, typename DescType::Index feature_idx, + const DescType& descriptor, const GeomType& geometry) { + CHECK_EQ(descriptor.size(), kDescDim); + const ProjDescType proj_desc = + proj_matrix_ * descriptor.transpose().template cast(); + inverted_files_.at(word_id).AddEntry(image_id, feature_idx, proj_desc, + geometry); +} + +template +void InvertedIndex::ClearEntries() { + for (auto& inverted_file : inverted_files_) { + inverted_file.ClearEntries(); + } +} + +template +void InvertedIndex::Query( + const DescType& descriptors, const Eigen::MatrixXi& word_ids, + std::vector* image_scores) const { + CHECK_EQ(descriptors.cols(), kDescDim); + + image_scores->clear(); + + // Computes the self-similarity score for the query image. + const float self_similarity = ComputeSelfSimilarity(word_ids); + float normalization_weight = 1.0f; + if (self_similarity > 0.0f) { + normalization_weight = 1.0f / std::sqrt(self_similarity); + } + + std::unordered_map score_map; + std::vector inverted_file_scores; + + for (typename DescType::Index i = 0; i < descriptors.rows(); ++i) { + const ProjDescType proj_descriptor = + proj_matrix_ * descriptors.row(i).transpose().template cast(); + for (Eigen::MatrixXi::Index n = 0; n < word_ids.cols(); ++n) { + const int word_id = word_ids(i, n); + if (word_id == kInvalidWordId) { + continue; + } + + inverted_files_.at(word_id).ScoreFeature(proj_descriptor, + &inverted_file_scores); + + for (const ImageScore& score : inverted_file_scores) { + const auto score_map_it = score_map.find(score.image_id); + if (score_map_it == score_map.end()) { + // Image not found in another inverted file. + score_map.emplace(score.image_id, + static_cast(image_scores->size())); + image_scores->push_back(score); + } else { + // Image already found in another inverted file, so accumulate. + (*image_scores).at(score_map_it->second).score += score.score; + } + } + } + } + + // Normalization. + for (ImageScore& score : *image_scores) { + score.score *= + normalization_weight * normalization_constants_.at(score.image_id); + } +} + +template +void InvertedIndex:: + ConvertToBinaryDescriptor( + const int word_id, const DescType& descriptor, + std::bitset* binary_descriptor) const { + const ProjDescType proj_desc = + proj_matrix_ * descriptor.transpose().template cast(); + inverted_files_.at(word_id).ConvertToBinaryDescriptor(proj_desc, + binary_descriptor); +} + +template +float InvertedIndex::GetIDFWeight( + const int word_id) const { + return inverted_files_.at(word_id).IDFWeight(); +} + +template +void InvertedIndex::FindMatches( + const int word_id, const std::unordered_set& image_ids, + std::vector* matches) const { + matches->clear(); + const auto& entries = inverted_files_.at(word_id).GetEntries(); + for (const auto& entry : entries) { + if (image_ids.count(entry.image_id)) { + matches->emplace_back(&entry); + } + } +} + +template +float InvertedIndex::ComputeSelfSimilarity( + const Eigen::MatrixXi& word_ids) const { + double self_similarity = 0.0; + for (Eigen::MatrixXi::Index i = 0; i < word_ids.size(); ++i) { + const int word_id = word_ids(i); + if (word_id != kInvalidWordId) { + const auto& inverted_file = inverted_files_.at(word_id); + self_similarity += inverted_file.IDFWeight() * inverted_file.IDFWeight(); + } + } + return static_cast(self_similarity); +} + +template +void InvertedIndex::GetImageIds( + std::unordered_set* image_ids) const { + for (const auto& inverted_file : inverted_files_) { + inverted_file.GetImageIds(image_ids); + } +} + +template +void InvertedIndex::Read( + std::ifstream* ifs) { + CHECK(ifs->is_open()); + + int32_t num_words = 0; + ifs->read(reinterpret_cast(&num_words), sizeof(int32_t)); + CHECK_GT(num_words, 0); + + Initialize(num_words); + + int32_t N_t = 0; + ifs->read(reinterpret_cast(&N_t), sizeof(int32_t)); + CHECK_EQ(N_t, kEmbeddingDim) + << "The length of the binary strings should be " << kEmbeddingDim + << " but is " << N_t << ". The indices are not compatible!"; + + for (int i = 0; i < kEmbeddingDim; ++i) { + for (int j = 0; j < kDescDim; ++j) { + ifs->read(reinterpret_cast(&proj_matrix_(i, j)), sizeof(float)); + } + } + + for (auto& inverted_file : inverted_files_) { + inverted_file.Read(ifs); + } + + int32_t num_images = 0; + ifs->read(reinterpret_cast(&num_images), sizeof(int32_t)); + CHECK_GE(num_images, 0); + + normalization_constants_.clear(); + normalization_constants_.reserve(num_images); + for (int32_t i = 0; i < num_images; ++i) { + int image_id; + float value; + ifs->read(reinterpret_cast(&image_id), sizeof(int)); + ifs->read(reinterpret_cast(&value), sizeof(float)); + normalization_constants_[image_id] = value; + } +} + +template +void InvertedIndex::Write( + std::ofstream* ofs) const { + CHECK(ofs->is_open()); + + int32_t num_words = static_cast(NumVisualWords()); + ofs->write(reinterpret_cast(&num_words), sizeof(int32_t)); + CHECK_GT(num_words, 0); + + const int32_t N_t = static_cast(kEmbeddingDim); + ofs->write(reinterpret_cast(&N_t), sizeof(int32_t)); + + for (int i = 0; i < kEmbeddingDim; ++i) { + for (int j = 0; j < kDescDim; ++j) { + ofs->write(reinterpret_cast(&proj_matrix_(i, j)), + sizeof(float)); + } + } + + for (const auto& inverted_file : inverted_files_) { + inverted_file.Write(ofs); + } + + const int32_t num_images = normalization_constants_.size(); + ofs->write(reinterpret_cast(&num_images), sizeof(int32_t)); + + for (const auto& constant : normalization_constants_) { + ofs->write(reinterpret_cast(&constant.first), sizeof(int)); + ofs->write(reinterpret_cast(&constant.second), sizeof(float)); + } +} + +template +void InvertedIndex::ComputeWeightsAndNormalizationConstants() { + std::unordered_set image_ids; + GetImageIds(&image_ids); + + for (auto& inverted_file : inverted_files_) { + inverted_file.ComputeIDFWeight(image_ids.size()); + } + + std::unordered_map self_similarities(image_ids.size()); + for (const auto& inverted_file : inverted_files_) { + inverted_file.ComputeImageSelfSimilarities(&self_similarities); + } + + normalization_constants_.clear(); + normalization_constants_.reserve(image_ids.size()); + for (const auto& self_similarity : self_similarities) { + if (self_similarity.second > 0.0) { + normalization_constants_[self_similarity.first] = + static_cast(1.0 / std::sqrt(self_similarity.second)); + } else { + normalization_constants_[self_similarity.first] = 0.0f; + } + } +} + +} // namespace retrieval +} // namespace colmap + +#endif // COLMAP_SRC_RETRIEVAL_INVERTED_INDEX_H_ diff --git a/colmap/include/colmap/retrieval/utils.h b/colmap/include/colmap/retrieval/utils.h new file mode 100644 index 0000000000000000000000000000000000000000..e09b38a24e55f0cd90c66130b4ab899cdc91f37a --- /dev/null +++ b/colmap/include/colmap/retrieval/utils.h @@ -0,0 +1,87 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_RETRIEVAL_UTILS_H_ +#define COLMAP_SRC_RETRIEVAL_UTILS_H_ + +#include +#include + +namespace colmap { +namespace retrieval { + +struct ImageScore { + int image_id = -1; + float score = 0.0f; +}; + +// Implements the weighting function used to derive a voting weight from the +// Hamming distance of two binary signatures. See Eqn. 4 in +// Arandjelovic, Zisserman. DisLocation: Scalable descriptor distinctiveness for +// location recognition. ACCV 2014. +// The template is the length of the Hamming embedding vectors. +// This class is based on an original implementation by Torsten Sattler. +template +class HammingDistWeightFunctor { + public: + static const size_t kMaxHammingDistance = static_cast(1.5f * kSigma); + + HammingDistWeightFunctor() { + // Fills the look-up table. + const float sigma_squared = kSigma * kSigma; + for (int n = 0; n <= N; ++n) { + const float hamming_dist = static_cast(n); + if (hamming_dist <= kMaxHammingDistance) { + look_up_table_.at(n) = + std::exp(-hamming_dist * hamming_dist / sigma_squared); + } else { + look_up_table_.at(n) = 0.0f; + } + } + } + + // Returns the weight for Hamming distance h and standard deviation sigma. + // Does not perform a range check when performing the look-up. + inline float operator()(const size_t hamming_dist) const { + return look_up_table_.at(hamming_dist); + } + + private: + // In order to avoid wasting computations, we once compute a look-up table + // storing all function values for all possible values of the standard + // deviation \sigma. This is implemented as a (N + 1) vector. + std::array look_up_table_; +}; + +} // namespace retrieval +} // namespace colmap + +#endif // COLMAP_SRC_RETRIEVAL_UTILS_H_ diff --git a/colmap/include/colmap/retrieval/visual_index.h b/colmap/include/colmap/retrieval/visual_index.h new file mode 100644 index 0000000000000000000000000000000000000000..79faf81c092b347ed079555e08e4273e02e2a16f --- /dev/null +++ b/colmap/include/colmap/retrieval/visual_index.h @@ -0,0 +1,744 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_RETRIEVAL_VISUAL_INDEX_H_ +#define COLMAP_SRC_RETRIEVAL_VISUAL_INDEX_H_ + +#include +#include + +#include "flann/flann.hpp" +#include "feature/types.h" +#include "retrieval/inverted_file.h" +#include "retrieval/inverted_index.h" +#include "retrieval/vote_and_verify.h" +#include "util/alignment.h" +#include "util/endian.h" +#include "util/logging.h" +#include "util/math.h" + +namespace colmap { +namespace retrieval { + +// Visual index for image retrieval using a vocabulary tree with Hamming +// embedding, based on the papers: +// +// Schönberger, Price, Sattler, Pollefeys, Frahm. "A Vote-and-Verify Strategy +// for Fast Spatial Verification in Image Retrieval". ACCV 2016. +// +// Arandjelovic, Zisserman: Scalable descriptor +// distinctiveness for location recognition. ACCV 2014. +template +class VisualIndex { + public: + static const int kMaxNumThreads = -1; + typedef InvertedIndex InvertedIndexType; + typedef FeatureKeypoints GeomType; + typedef typename InvertedIndexType::DescType DescType; + typedef typename InvertedIndexType::EntryType EntryType; + + struct IndexOptions { + // The number of nearest neighbor visual words that each feature descriptor + // is assigned to. + int num_neighbors = 1; + + // The number of checks in the nearest neighbor search. + int num_checks = 256; + + // The number of threads used in the index. + int num_threads = kMaxNumThreads; + }; + + struct QueryOptions { + // The maximum number of most similar images to retrieve. + int max_num_images = -1; + + // The number of nearest neighbor visual words that each feature descriptor + // is assigned to. + int num_neighbors = 5; + + // The number of checks in the nearest neighbor search. + int num_checks = 256; + + // Whether to perform spatial verification after image retrieval. + int num_images_after_verification = 0; + + // The number of threads used in the index. + int num_threads = kMaxNumThreads; + }; + + struct BuildOptions { + // The desired number of visual words, i.e. the number of leaf node + // clusters. Note that the actual number of visual words might be less. + int num_visual_words = 256 * 256; + + // The branching factor of the hierarchical k-means tree. + int branching = 256; + + // The number of iterations for the clustering. + int num_iterations = 11; + + // The target precision of the visual word search index. + double target_precision = 0.95; + + // The number of checks in the nearest neighbor search. + int num_checks = 256; + + // The number of threads used in the index. + int num_threads = kMaxNumThreads; + }; + + VisualIndex(); + ~VisualIndex(); + + size_t NumVisualWords() const; + + // Add image to the visual index. + void Add(const IndexOptions& options, const int image_id, + const GeomType& geometries, const DescType& descriptors); + + // Check if an image has been indexed. + bool ImageIndexed(const int image_id) const; + + // Query for most similar images in the visual index. + void Query(const QueryOptions& options, const DescType& descriptors, + std::vector* image_scores) const; + + // Query for most similar images in the visual index. + void Query(const QueryOptions& options, const GeomType& geometries, + const DescType& descriptors, + std::vector* image_scores) const; + + // Prepare the index after adding images and before querying. + void Prepare(); + + // Build a visual index from a set of training descriptors by quantizing the + // descriptor space into visual words and compute their Hamming embedding. + void Build(const BuildOptions& options, const DescType& descriptors); + + // Read and write the visual index. This can be done for an index with and + // without indexed images. + void Read(const std::string& path); + void Write(const std::string& path); + + private: + // Quantize the descriptor space into visual words. + void Quantize(const BuildOptions& options, const DescType& descriptors); + + // Query for nearest neighbor images and return nearest neighbor visual word + // identifiers for each descriptor. + void QueryAndFindWordIds(const QueryOptions& options, + const DescType& descriptors, + std::vector* image_scores, + Eigen::MatrixXi* word_ids) const; + + // Find the nearest neighbor visual words for the given descriptors. + Eigen::MatrixXi FindWordIds(const DescType& descriptors, + const int num_neighbors, const int num_checks, + const int num_threads) const; + + // The search structure on the quantized descriptor space. + flann::AutotunedIndex> visual_word_index_; + + // The centroids of the visual words. + flann::Matrix visual_words_; + + // The inverted index of the database. + InvertedIndexType inverted_index_; + + // Identifiers of all indexed images. + std::unordered_set image_ids_; + + // Whether the index is prepared. + bool prepared_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +VisualIndex::VisualIndex() + : prepared_(false) {} + +template +VisualIndex::~VisualIndex() { + if (visual_words_.ptr() != nullptr) { + delete[] visual_words_.ptr(); + } +} + +template +size_t VisualIndex::NumVisualWords() const { + return visual_words_.rows; +} + +template +void VisualIndex::Add( + const IndexOptions& options, const int image_id, const GeomType& geometries, + const DescType& descriptors) { + CHECK_EQ(geometries.size(), descriptors.rows()); + + // If the image is already indexed, do nothing. + if (ImageIndexed(image_id)) { + return; + } + + image_ids_.insert(image_id); + + prepared_ = false; + + if (descriptors.rows() == 0) { + return; + } + + const Eigen::MatrixXi word_ids = + FindWordIds(descriptors, options.num_neighbors, options.num_checks, + options.num_threads); + + for (typename DescType::Index i = 0; i < descriptors.rows(); ++i) { + const auto& descriptor = descriptors.row(i); + + typename InvertedIndexType::GeomType geometry; + geometry.x = geometries[i].x; + geometry.y = geometries[i].y; + geometry.scale = geometries[i].ComputeScale(); + geometry.orientation = geometries[i].ComputeOrientation(); + + for (int n = 0; n < options.num_neighbors; ++n) { + const int word_id = word_ids(i, n); + if (word_id != InvertedIndexType::kInvalidWordId) { + inverted_index_.AddEntry(image_id, word_id, i, descriptor, geometry); + } + } + } +} + +template +bool VisualIndex::ImageIndexed( + const int image_id) const { + return image_ids_.count(image_id) != 0; +} + +template +void VisualIndex::Query( + const QueryOptions& options, const DescType& descriptors, + std::vector* image_scores) const { + const GeomType geometries; + Query(options, geometries, descriptors, image_scores); +} + +template +void VisualIndex::Query( + const QueryOptions& options, const GeomType& geometries, + const DescType& descriptors, std::vector* image_scores) const { + Eigen::MatrixXi word_ids; + QueryAndFindWordIds(options, descriptors, image_scores, &word_ids); + + if (options.num_images_after_verification <= 0) { + return; + } + + CHECK_EQ(descriptors.rows(), geometries.size()); + + // Extract top-ranked images to verify. + std::unordered_set image_ids; + for (const auto& image_score : *image_scores) { + image_ids.insert(image_score.image_id); + } + + // Find matches for top-ranked images + typedef std::vector< + std::pair>> + OrderedMatchListType; + + // Reference our matches (with their lowest distance) for both + // {query feature => db feature} and vice versa. + std::unordered_map> + query_to_db_matches; + std::unordered_map> + db_to_query_matches; + + std::vector word_matches; + + std::vector query_entries; // Convert query features, too. + query_entries.reserve(descriptors.rows()); + + // NOTE: Currently, we are redundantly computing the feature weighting. + const HammingDistWeightFunctor hamming_dist_weight_functor; + + for (typename DescType::Index i = 0; i < descriptors.rows(); ++i) { + const auto& descriptor = descriptors.row(i); + + EntryType query_entry; + query_entry.feature_idx = i; + query_entry.geometry.x = geometries[i].x; + query_entry.geometry.y = geometries[i].y; + query_entry.geometry.scale = geometries[i].ComputeScale(); + query_entry.geometry.orientation = geometries[i].ComputeOrientation(); + query_entries.push_back(query_entry); + + // For each db feature, keep track of the lowest distance (if db features + // are mapped to more than one visual word). + std::unordered_map< + int, std::unordered_map>> + image_matches; + + for (int j = 0; j < word_ids.cols(); ++j) { + const int word_id = word_ids(i, j); + + if (word_id != InvertedIndexType::kInvalidWordId) { + inverted_index_.ConvertToBinaryDescriptor(word_id, descriptor, + &query_entries[i].descriptor); + + const auto idf_weight = inverted_index_.GetIDFWeight(word_id); + const auto squared_idf_weight = idf_weight * idf_weight; + + inverted_index_.FindMatches(word_id, image_ids, &word_matches); + + for (const auto& match : word_matches) { + const size_t hamming_dist = + (query_entries[i].descriptor ^ match->descriptor).count(); + + if (hamming_dist <= hamming_dist_weight_functor.kMaxHammingDistance) { + const float dist = + hamming_dist_weight_functor(hamming_dist) * squared_idf_weight; + + auto& feature_matches = image_matches[match->image_id]; + const auto feature_match = feature_matches.find(match->feature_idx); + + if (feature_match == feature_matches.end() || + feature_match->first < dist) { + feature_matches[match->feature_idx] = std::make_pair(dist, match); + } + } + } + } + } + + // Finally, cross-reference the query and db feature matches. + for (const auto& feature_matches : image_matches) { + const auto image_id = feature_matches.first; + + for (const auto& feature_match : feature_matches.second) { + const auto feature_idx = feature_match.first; + const auto dist = feature_match.second.first; + const auto db_match = feature_match.second.second; + + const auto entry_pair = std::make_pair(&query_entries[i], db_match); + + query_to_db_matches[image_id][i].emplace_back(dist, entry_pair); + db_to_query_matches[image_id][feature_idx].emplace_back(dist, + entry_pair); + } + } + } + + // Verify top-ranked images using the found matches. + for (auto& image_score : *image_scores) { + auto& query_matches = query_to_db_matches[image_score.image_id]; + auto& db_matches = db_to_query_matches[image_score.image_id]; + + // No matches found. + if (query_matches.empty()) { + continue; + } + + // Enforce 1-to-1 matching: Build Fibonacci heaps for the query and database + // features, ordered by the minimum number of matches per feature. We'll + // select these matches one at a time. For convenience, we'll also pre-sort + // the matched feature lists by matching score. + + typedef boost::heap::fibonacci_heap> FibonacciHeapType; + FibonacciHeapType query_heap; + FibonacciHeapType db_heap; + std::unordered_map + query_heap_handles; + std::unordered_map + db_heap_handles; + + for (auto& match_data : query_matches) { + std::sort(match_data.second.begin(), match_data.second.end(), + std::greater>>()); + + query_heap_handles[match_data.first] = query_heap.push(std::make_pair( + -static_cast(match_data.second.size()), match_data.first)); + } + + for (auto& match_data : db_matches) { + std::sort(match_data.second.begin(), match_data.second.end(), + std::greater>>()); + + db_heap_handles[match_data.first] = db_heap.push(std::make_pair( + -static_cast(match_data.second.size()), match_data.first)); + } + + // Keep tabs on what features have been already matched. + std::vector matches; + + auto db_top = db_heap.top(); // (-num_available_matches, feature_idx) + auto query_top = query_heap.top(); + + while (!db_heap.empty() && !query_heap.empty()) { + // Take the query or database feature with the smallest number of + // available matches. + const bool use_query = + (query_top.first >= db_top.first) && !query_heap.empty(); + + // Find the best matching feature that hasn't already been matched. + auto& heap1 = (use_query) ? query_heap : db_heap; + auto& heap2 = (use_query) ? db_heap : query_heap; + auto& handles1 = (use_query) ? query_heap_handles : db_heap_handles; + auto& handles2 = (use_query) ? db_heap_handles : query_heap_handles; + auto& matches1 = (use_query) ? query_matches : db_matches; + auto& matches2 = (use_query) ? db_matches : query_matches; + + const auto idx1 = heap1.top().second; + heap1.pop(); + + // Entries that have been matched (or processed and subsequently ignored) + // get their handles removed. + if (handles1.count(idx1) > 0) { + handles1.erase(idx1); + + bool match_found = false; + + // The matches have been ordered by Hamming distance, already -- + // select the lowest available match. + for (auto& entry2 : matches1[idx1]) { + const auto idx2 = (use_query) ? entry2.second.second->feature_idx + : entry2.second.first->feature_idx; + + if (handles2.count(idx2) > 0) { + if (!match_found) { + match_found = true; + FeatureGeometryMatch match; + match.geometry1 = entry2.second.first->geometry; + match.geometries2.push_back(entry2.second.second->geometry); + matches.push_back(match); + + handles2.erase(idx2); + + // Remove this feature from consideration for all other features + // that matched to it. + for (auto& entry1 : matches2[idx2]) { + const auto other_idx1 = (use_query) + ? entry1.second.first->feature_idx + : entry1.second.second->feature_idx; + if (handles1.count(other_idx1) > 0) { + (*handles1[other_idx1]).first += 1; + heap1.increase(handles1[other_idx1]); + } + } + } else { + (*handles2[idx2]).first += 1; + heap2.increase(handles2[idx2]); + } + } + } + } + + if (!query_heap.empty()) { + query_top = query_heap.top(); + } + + if (!db_heap.empty()) { + db_top = db_heap.top(); + } + } + + // Finally, run verification for the current image. + VoteAndVerifyOptions vote_and_verify_options; + image_score.score += VoteAndVerify(vote_and_verify_options, matches); + } + + // Re-rank the images using the spatial verification scores. + + const size_t num_images = std::min( + image_scores->size(), options.num_images_after_verification); + + auto SortFunc = [](const ImageScore& score1, const ImageScore& score2) { + return score1.score > score2.score; + }; + + if (num_images == image_scores->size()) { + std::sort(image_scores->begin(), image_scores->end(), SortFunc); + } else { + std::partial_sort(image_scores->begin(), image_scores->begin() + num_images, + image_scores->end(), SortFunc); + image_scores->resize(num_images); + } +} + +template +void VisualIndex::Prepare() { + inverted_index_.Finalize(); + prepared_ = true; +} + +template +void VisualIndex::Build( + const BuildOptions& options, const DescType& descriptors) { + // Quantize the descriptor space into visual words. + Quantize(options, descriptors); + + // Build the search index on the visual words. + flann::AutotunedIndexParams index_params; + index_params["target_precision"] = + static_cast(options.target_precision); + visual_word_index_ = + flann::AutotunedIndex>(index_params); + visual_word_index_.buildIndex(visual_words_); + + // Initialize a new inverted index. + inverted_index_ = InvertedIndexType(); + inverted_index_.Initialize(NumVisualWords()); + + // Generate descriptor projection matrix. + inverted_index_.GenerateHammingEmbeddingProjection(); + + // Learn the Hamming embedding. + const int kNumNeighbors = 1; + const Eigen::MatrixXi word_ids = FindWordIds( + descriptors, kNumNeighbors, options.num_checks, options.num_threads); + inverted_index_.ComputeHammingEmbedding(descriptors, word_ids); +} + +template +void VisualIndex::Read( + const std::string& path) { + long int file_offset = 0; + + // Read the visual words. + + { + if (visual_words_.ptr() != nullptr) { + delete[] visual_words_.ptr(); + } + + std::ifstream file(path, std::ios::binary); + CHECK(file.is_open()) << path; + const uint64_t rows = ReadBinaryLittleEndian(&file); + const uint64_t cols = ReadBinaryLittleEndian(&file); + kDescType* visual_words_data = new kDescType[rows * cols]; + for (size_t i = 0; i < rows * cols; ++i) { + visual_words_data[i] = ReadBinaryLittleEndian(&file); + } + visual_words_ = flann::Matrix(visual_words_data, rows, cols); + file_offset = file.tellg(); + } + + // Read the visual words search index. + + visual_word_index_ = + flann::AutotunedIndex>(visual_words_); + + { + FILE* fin = fopen(path.c_str(), "rb"); + CHECK_NOTNULL(fin); + fseek(fin, file_offset, SEEK_SET); + visual_word_index_.loadIndex(fin); + file_offset = ftell(fin); + fclose(fin); + } + + // Read the inverted index. + + { + std::ifstream file(path, std::ios::binary); + CHECK(file.is_open()) << path; + file.seekg(file_offset, std::ios::beg); + inverted_index_.Read(&file); + } + + image_ids_.clear(); + inverted_index_.GetImageIds(&image_ids_); +} + +template +void VisualIndex::Write( + const std::string& path) { + // Write the visual words. + + { + CHECK_NOTNULL(visual_words_.ptr()); + std::ofstream file(path, std::ios::binary); + CHECK(file.is_open()) << path; + WriteBinaryLittleEndian(&file, visual_words_.rows); + WriteBinaryLittleEndian(&file, visual_words_.cols); + for (size_t i = 0; i < visual_words_.rows * visual_words_.cols; ++i) { + WriteBinaryLittleEndian(&file, visual_words_.ptr()[i]); + } + } + + // Write the visual words search index. + + { + FILE* fout = fopen(path.c_str(), "ab"); + CHECK_NOTNULL(fout); + visual_word_index_.saveIndex(fout); + fclose(fout); + } + + // Write the inverted index. + + { + std::ofstream file(path, std::ios::binary | std::ios::app); + CHECK(file.is_open()) << path; + inverted_index_.Write(&file); + } +} + +template +void VisualIndex::Quantize( + const BuildOptions& options, const DescType& descriptors) { + static_assert(DescType::IsRowMajor, "Descriptors must be row-major."); + + CHECK_GE(options.num_visual_words, options.branching); + CHECK_GE(descriptors.rows(), options.num_visual_words); + + const flann::Matrix descriptor_matrix( + const_cast(descriptors.data()), descriptors.rows(), + descriptors.cols()); + + std::vector::ResultType> centers_data( + options.num_visual_words * descriptors.cols()); + flann::Matrix::ResultType> centers( + centers_data.data(), options.num_visual_words, descriptors.cols()); + + flann::KMeansIndexParams index_params; + index_params["branching"] = options.branching; + index_params["iterations"] = options.num_iterations; + index_params["centers_init"] = flann::FLANN_CENTERS_KMEANSPP; + const int num_centers = flann::hierarchicalClustering>( + descriptor_matrix, centers, index_params); + + CHECK_LE(num_centers, options.num_visual_words); + + const size_t visual_word_data_size = num_centers * descriptors.cols(); + kDescType* visual_words_data = new kDescType[visual_word_data_size]; + for (size_t i = 0; i < visual_word_data_size; ++i) { + if (std::is_integral::value) { + visual_words_data[i] = std::round(centers_data[i]); + } else { + visual_words_data[i] = centers_data[i]; + } + } + + if (visual_words_.ptr() != nullptr) { + delete[] visual_words_.ptr(); + } + + visual_words_ = flann::Matrix(visual_words_data, num_centers, + descriptors.cols()); +} + +template +void VisualIndex::QueryAndFindWordIds( + const QueryOptions& options, const DescType& descriptors, + std::vector* image_scores, Eigen::MatrixXi* word_ids) const { + CHECK(prepared_); + + if (descriptors.rows() == 0) { + image_scores->clear(); + return; + } + + *word_ids = FindWordIds(descriptors, options.num_neighbors, + options.num_checks, options.num_threads); + inverted_index_.Query(descriptors, *word_ids, image_scores); + + auto SortFunc = [](const ImageScore& score1, const ImageScore& score2) { + return score1.score > score2.score; + }; + + size_t num_images = image_scores->size(); + if (options.max_num_images >= 0) { + num_images = std::min(image_scores->size(), options.max_num_images); + } + + if (num_images == image_scores->size()) { + std::sort(image_scores->begin(), image_scores->end(), SortFunc); + } else { + std::partial_sort(image_scores->begin(), image_scores->begin() + num_images, + image_scores->end(), SortFunc); + image_scores->resize(num_images); + } +} + +template +Eigen::MatrixXi VisualIndex::FindWordIds( + const DescType& descriptors, const int num_neighbors, const int num_checks, + const int num_threads) const { + static_assert(DescType::IsRowMajor, "Descriptors must be row-major"); + + CHECK_GT(descriptors.rows(), 0); + CHECK_GT(num_neighbors, 0); + + Eigen::Matrix + word_ids(descriptors.rows(), num_neighbors); + word_ids.setConstant(InvertedIndexType::kInvalidWordId); + flann::Matrix indices(word_ids.data(), descriptors.rows(), + num_neighbors); + + Eigen::Matrix::ResultType, Eigen::Dynamic, + Eigen::Dynamic, Eigen::RowMajor> + distance_matrix(descriptors.rows(), num_neighbors); + flann::Matrix::ResultType> distances( + distance_matrix.data(), descriptors.rows(), num_neighbors); + + const flann::Matrix query( + const_cast(descriptors.data()), descriptors.rows(), + descriptors.cols()); + + flann::SearchParams search_params(num_checks); + if (num_threads < 0) { + search_params.cores = std::thread::hardware_concurrency(); + } else { + search_params.cores = num_threads; + } + if (search_params.cores <= 0) { + search_params.cores = 1; + } + + visual_word_index_.knnSearch(query, indices, distances, num_neighbors, + search_params); + + return word_ids.cast(); +} + +} // namespace retrieval +} // namespace colmap + +#endif // COLMAP_SRC_RETRIEVAL_VISUAL_INDEX_H_ diff --git a/colmap/include/colmap/retrieval/vote_and_verify.h b/colmap/include/colmap/retrieval/vote_and_verify.h new file mode 100644 index 0000000000000000000000000000000000000000..4a03855f4f0609115c1dacd4ecef849a7aa70664 --- /dev/null +++ b/colmap/include/colmap/retrieval/vote_and_verify.h @@ -0,0 +1,78 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_RETRIEVAL_VOTE_AND_VERIFY_H_ +#define COLMAP_SRC_RETRIEVAL_VOTE_AND_VERIFY_H_ + +#include "retrieval/geometry.h" + +namespace colmap { +namespace retrieval { + +struct VoteAndVerifyOptions { + // Number of top transformations to generate. + int num_transformations = 30; + + // Number of voting bins in the translation dimension. + int num_trans_bins = 64; + + // Number of voting bins in the scale dimension. + int num_scale_bins = 32; + + // Number of voting bins in the orientation dimension. + int num_angle_bins = 8; + + // Maximum image dimension that bounds the range of the translation bins. + int max_image_size = 4096; + + // Minimum number of votes for a transformation to be considered. + int min_num_votes = 1; + + // RANSAC confidence level used to abort the iteration. + double confidence = 0.99; + + // Thresholds for considering a match an inlier. + double max_transfer_error = 100.0 * 100.0; + double max_scale_error = 2.0; +}; + +// Compute effective inlier count using Vote-and-Verify by estimating an affine +// transformation from 2D-2D image matches. The method is described in: +// "A Vote­-and­-Verify Strategy for +// Fast Spatial Verification in Image Retrieval", +// Schönberger et al., ACCV 2016. +int VoteAndVerify(const VoteAndVerifyOptions& options, + const std::vector& matches); + +} // namespace retrieval +} // namespace colmap + +#endif // COLMAP_SRC_RETRIEVAL_VOTE_AND_VERIFY_H_ diff --git a/colmap/include/colmap/sfm/incremental_mapper.h b/colmap/include/colmap/sfm/incremental_mapper.h new file mode 100644 index 0000000000000000000000000000000000000000..859194f145103069d2e165d337b7e6a7f2a40741 --- /dev/null +++ b/colmap/include/colmap/sfm/incremental_mapper.h @@ -0,0 +1,313 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_SFM_INCREMENTAL_MAPPER_H_ +#define COLMAP_SRC_SFM_INCREMENTAL_MAPPER_H_ + +#include "base/database.h" +#include "base/database_cache.h" +#include "base/reconstruction.h" +#include "optim/bundle_adjustment.h" +#include "sfm/incremental_triangulator.h" +#include "util/alignment.h" + +namespace colmap { + +// Class that provides all functionality for the incremental reconstruction +// procedure. Example usage: +// +// IncrementalMapper mapper(&database_cache); +// mapper.BeginReconstruction(&reconstruction); +// CHECK(mapper.FindInitialImagePair(options, image_id1, image_id2)); +// CHECK(mapper.RegisterInitialImagePair(options, image_id1, image_id2)); +// while (...) { +// const auto next_image_ids = mapper.FindNextImages(options); +// for (const auto image_id : next_image_ids) { +// CHECK(mapper.RegisterNextImage(options, image_id)); +// if (...) { +// mapper.AdjustLocalBundle(...); +// } else { +// mapper.AdjustGlobalBundle(...); +// } +// } +// } +// mapper.EndReconstruction(false); +// +class IncrementalMapper { + public: + struct Options { + // Minimum number of inliers for initial image pair. + int init_min_num_inliers = 100; + + // Maximum error in pixels for two-view geometry estimation for initial + // image pair. + double init_max_error = 4.0; + + // Maximum forward motion for initial image pair. + double init_max_forward_motion = 0.95; + + // Minimum triangulation angle for initial image pair. + double init_min_tri_angle = 16.0; + + // Maximum number of trials to use an image for initialization. + int init_max_reg_trials = 2; + + // Maximum reprojection error in absolute pose estimation. + double abs_pose_max_error = 12.0; + + // Minimum number of inliers in absolute pose estimation. + int abs_pose_min_num_inliers = 30; + + // Minimum inlier ratio in absolute pose estimation. + double abs_pose_min_inlier_ratio = 0.25; + + // Whether to estimate the focal length in absolute pose estimation. + bool abs_pose_refine_focal_length = true; + + // Whether to estimate the extra parameters in absolute pose estimation. + bool abs_pose_refine_extra_params = true; + + // Number of images to optimize in local bundle adjustment. + int local_ba_num_images = 6; + + // Minimum triangulation for images to be chosen in local bundle adjustment. + double local_ba_min_tri_angle = 6; + + // Thresholds for bogus camera parameters. Images with bogus camera + // parameters are filtered and ignored in triangulation. + double min_focal_length_ratio = 0.1; // Opening angle of ~130deg + double max_focal_length_ratio = 10; // Opening angle of ~5deg + double max_extra_param = 1; + + // Maximum reprojection error in pixels for observations. + double filter_max_reproj_error = 4.0; + + // Minimum triangulation angle in degrees for stable 3D points. + double filter_min_tri_angle = 1.5; + + // Maximum number of trials to register an image. + int max_reg_trials = 3; + + // If reconstruction is provided as input, fix the existing image poses. + bool fix_existing_images = false; + + // Number of threads. + int num_threads = -1; + + // Method to find and select next best image to register. + enum class ImageSelectionMethod { + MAX_VISIBLE_POINTS_NUM, + MAX_VISIBLE_POINTS_RATIO, + MIN_UNCERTAINTY, + }; + ImageSelectionMethod image_selection_method = + ImageSelectionMethod::MIN_UNCERTAINTY; + + bool Check() const; + }; + + struct LocalBundleAdjustmentReport { + size_t num_merged_observations = 0; + size_t num_completed_observations = 0; + size_t num_filtered_observations = 0; + size_t num_adjusted_observations = 0; + }; + + // Create incremental mapper. The database cache must live for the entire + // life-time of the incremental mapper. + explicit IncrementalMapper(const DatabaseCache* database_cache); + + // Prepare the mapper for a new reconstruction, which might have existing + // registered images (in which case `RegisterNextImage` must be called) or + // which is empty (in which case `RegisterInitialImagePair` must be called). + void BeginReconstruction(Reconstruction* reconstruction); + + // Cleanup the mapper after the current reconstruction is done. If the + // model is discarded, the number of total and shared registered images will + // be updated accordingly. + void EndReconstruction(const bool discard); + + // Find initial image pair to seed the incremental reconstruction. The image + // pairs should be passed to `RegisterInitialImagePair`. This function + // automatically ignores image pairs that failed to register previously. + bool FindInitialImagePair(const Options& options, image_t* image_id1, + image_t* image_id2); + + // Find best next image to register in the incremental reconstruction. The + // images should be passed to `RegisterNextImage`. This function automatically + // ignores images that failed to registered for `max_reg_trials`. + std::vector FindNextImages(const Options& options); + + // Attempt to seed the reconstruction from an image pair. + bool RegisterInitialImagePair(const Options& options, const image_t image_id1, + const image_t image_id2); + + // Attempt to register image to the existing model. This requires that + // a previous call to `RegisterInitialImagePair` was successful. + bool RegisterNextImage(const Options& options, const image_t image_id); + + // Triangulate observations of image. + size_t TriangulateImage(const IncrementalTriangulator::Options& tri_options, + const image_t image_id); + + // Retriangulate image pairs that should have common observations according to + // the scene graph but don't due to drift, etc. To handle drift, the employed + // reprojection error thresholds should be relatively large. If the thresholds + // are too large, non-robust bundle adjustment will break down; if the + // thresholds are too small, we cannot fix drift effectively. + size_t Retriangulate(const IncrementalTriangulator::Options& tri_options); + + // Complete tracks by transitively following the scene graph correspondences. + // This is especially effective after bundle adjustment, since many cameras + // and point locations might have improved. Completion of tracks enables + // better subsequent registration of new images. + size_t CompleteTracks(const IncrementalTriangulator::Options& tri_options); + + // Merge tracks by using scene graph correspondences. Similar to + // `CompleteTracks`, this is effective after bundle adjustment and improves + // the redundancy in subsequent bundle adjustments. + size_t MergeTracks(const IncrementalTriangulator::Options& tri_options); + + // Adjust locally connected images and points of a reference image. In + // addition, refine the provided 3D points. Only images connected to the + // reference image are optimized. If the provided 3D points are not locally + // connected to the reference image, their observing images are set as + // constant in the adjustment. + LocalBundleAdjustmentReport AdjustLocalBundle( + const Options& options, const BundleAdjustmentOptions& ba_options, + const IncrementalTriangulator::Options& tri_options, + const image_t image_id, const std::unordered_set& point3D_ids); + + // Global bundle adjustment using Ceres Solver or PBA. + bool AdjustGlobalBundle(const Options& options, + const BundleAdjustmentOptions& ba_options); + bool AdjustParallelGlobalBundle( + const BundleAdjustmentOptions& ba_options, + const ParallelBundleAdjuster::Options& parallel_ba_options); + + // Filter images and point observations. + size_t FilterImages(const Options& options); + size_t FilterPoints(const Options& options); + + const Reconstruction& GetReconstruction() const; + + // Number of images that are registered in at least on reconstruction. + size_t NumTotalRegImages() const; + + // Number of shared images between current reconstruction and all other + // previous reconstructions. + size_t NumSharedRegImages() const; + + // Get changed 3D points, since the last call to `ClearModifiedPoints3D`. + const std::unordered_set& GetModifiedPoints3D(); + + // Clear the collection of changed 3D points. + void ClearModifiedPoints3D(); + + private: + // Find seed images for incremental reconstruction. Suitable seed images have + // a large number of correspondences and have camera calibration priors. The + // returned list is ordered such that most suitable images are in the front. + std::vector FindFirstInitialImage(const Options& options) const; + + // For a given first seed image, find other images that are connected to the + // first image. Suitable second images have a large number of correspondences + // to the first image and have camera calibration priors. The returned list is + // ordered such that most suitable images are in the front. + std::vector FindSecondInitialImage(const Options& options, + const image_t image_id1) const; + + // Find local bundle for given image in the reconstruction. The local bundle + // is defined as the images that are most connected, i.e. maximum number of + // shared 3D points, to the given image. + std::vector FindLocalBundle(const Options& options, + const image_t image_id) const; + + // Register / De-register image in current reconstruction and update + // the number of shared images between all reconstructions. + void RegisterImageEvent(const image_t image_id); + void DeRegisterImageEvent(const image_t image_id); + + bool EstimateInitialTwoViewGeometry(const Options& options, + const image_t image_id1, + const image_t image_id2); + + // Class that holds all necessary data from database in memory. + const DatabaseCache* database_cache_; + + // Class that holds data of the reconstruction. + Reconstruction* reconstruction_; + + // Class that is responsible for incremental triangulation. + std::unique_ptr triangulator_; + + // Number of images that are registered in at least on reconstruction. + size_t num_total_reg_images_; + + // Number of shared images between current reconstruction and all other + // previous reconstructions. + size_t num_shared_reg_images_; + + // Estimated two-view geometry of last call to `FindFirstInitialImage`, + // used as a cache for a subsequent call to `RegisterInitialImagePair`. + image_pair_t prev_init_image_pair_id_; + TwoViewGeometry prev_init_two_view_geometry_; + + // Images and image pairs that have been used for initialization. Each image + // and image pair is only tried once for initialization. + std::unordered_map init_num_reg_trials_; + std::unordered_set init_image_pairs_; + + // The number of registered images per camera. This information is used + // to avoid duplicate refinement of camera parameters and degradation of + // already refined camera parameters in local bundle adjustment when multiple + // images share intrinsics. + std::unordered_map num_reg_images_per_camera_; + + // The number of reconstructions in which images are registered. + std::unordered_map num_registrations_; + + // Images that have been filtered in current reconstruction. + std::unordered_set filtered_images_; + + // Number of trials to register image in current reconstruction. Used to set + // an upper bound to the number of trials to register an image. + std::unordered_map num_reg_trials_; + + // Images that were registered before beginning the reconstruction. + // This image list will be non-empty, if the reconstruction is continued from + // an existing reconstruction. + std::unordered_set existing_image_ids_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_SFM_INCREMENTAL_MAPPER_H_ diff --git a/colmap/include/colmap/sfm/incremental_triangulator.h b/colmap/include/colmap/sfm/incremental_triangulator.h new file mode 100644 index 0000000000000000000000000000000000000000..570d0cfb68aabef0675c55ea015c94e9a106a8b4 --- /dev/null +++ b/colmap/include/colmap/sfm/incremental_triangulator.h @@ -0,0 +1,214 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_SFM_INCREMENTAL_TRIANGULATOR_H_ +#define COLMAP_SRC_SFM_INCREMENTAL_TRIANGULATOR_H_ + +#include "base/database_cache.h" +#include "base/reconstruction.h" +#include "util/alignment.h" + +namespace colmap { + +// Class that triangulates points during the incremental reconstruction. +// It holds the state and provides all functionality for triangulation. +class IncrementalTriangulator { + public: + struct Options { + // Maximum transitivity to search for correspondences. + int max_transitivity = 1; + + // Maximum angular error to create new triangulations. + double create_max_angle_error = 2.0; + + // Maximum angular error to continue existing triangulations. + double continue_max_angle_error = 2.0; + + // Maximum reprojection error in pixels to merge triangulations. + double merge_max_reproj_error = 4.0; + + // Maximum reprojection error to complete an existing triangulation. + double complete_max_reproj_error = 4.0; + + // Maximum transitivity for track completion. + int complete_max_transitivity = 5; + + // Maximum angular error to re-triangulate under-reconstructed image pairs. + double re_max_angle_error = 5.0; + + // Minimum ratio of common triangulations between an image pair over the + // number of correspondences between that image pair to be considered + // as under-reconstructed. + double re_min_ratio = 0.2; + + // Maximum number of trials to re-triangulate an image pair. + int re_max_trials = 1; + + // Minimum pairwise triangulation angle for a stable triangulation. + double min_angle = 1.5; + + // Whether to ignore two-view tracks. + bool ignore_two_view_tracks = true; + + // Thresholds for bogus camera parameters. Images with bogus camera + // parameters are ignored in triangulation. + double min_focal_length_ratio = 0.1; + double max_focal_length_ratio = 10.0; + double max_extra_param = 1.0; + + bool Check() const; + }; + + // Create new incremental triangulator. Note that both the correspondence + // graph and the reconstruction objects must live as long as the triangulator. + IncrementalTriangulator(const CorrespondenceGraph* correspondence_graph, + Reconstruction* reconstruction); + + // Triangulate observations of image. + // + // Triangulation includes creation of new points, continuation of existing + // points, and merging of separate points if given image bridges tracks. + // + // Note that the given image must be registered and its pose must be set + // in the associated reconstruction. + size_t TriangulateImage(const Options& options, const image_t image_id); + + // Complete triangulations for image. Tries to create new tracks for not + // yet triangulated observations and tries to complete existing tracks. + // Returns the number of completed observations. + size_t CompleteImage(const Options& options, const image_t image_id); + + // Complete tracks for specific 3D points. + // + // Completion tries to recursively add observations to a track that might + // have failed to triangulate before due to inaccurate poses, etc. + // Returns the number of completed observations. + size_t CompleteTracks(const Options& options, + const std::unordered_set& point3D_ids); + + // Complete tracks of all 3D points. + // Returns the number of completed observations. + size_t CompleteAllTracks(const Options& options); + + // Merge tracks of for specific 3D points. + // Returns the number of merged observations. + size_t MergeTracks(const Options& options, + const std::unordered_set& point3D_ids); + + // Merge tracks of all 3D points. + // Returns the number of merged observations. + size_t MergeAllTracks(const Options& options); + + // Perform retriangulation for under-reconstructed image pairs. Under- + // reconstruction usually occurs in the case of a drifting reconstruction. + // + // Image pairs are under-reconstructed if less than `Options::tri_re_min_ratio + // > tri_ratio`, where `tri_ratio` is the number of triangulated matches over + // inlier matches between the image pair. + size_t Retriangulate(const Options& options); + + // Indicate that a 3D point has been modified. + void AddModifiedPoint3D(const point3D_t point3D_id); + + // Get changed 3D points, since the last call to `ClearModifiedPoints3D`. + const std::unordered_set& GetModifiedPoints3D(); + + // Clear the collection of changed 3D points. + void ClearModifiedPoints3D(); + + // Data for a correspondence / element of a track, used to store all + // relevant data for triangulation, in order to avoid duplicate lookup + // in the underlying unordered_map's in the Reconstruction + struct CorrData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + image_t image_id; + point2D_t point2D_idx; + const Image* image; + const Camera* camera; + const Point2D* point2D; + }; + + private: + // Clear cache of bogus camera parameters and merge trials. + void ClearCaches(); + + // Find (transitive) correspondences to other images. + size_t Find(const Options& options, const image_t image_id, + const point2D_t point2D_idx, const size_t transitivity, + std::vector* corrs_data); + + // Try to create a new 3D point from the given correspondences. + size_t Create(const Options& options, + const std::vector& corrs_data); + + // Try to continue the 3D point with the given correspondences. + size_t Continue(const Options& options, const CorrData& ref_corr_data, + const std::vector& corrs_data); + + // Try to merge 3D point with any of its corresponding 3D points. + size_t Merge(const Options& options, const point3D_t point3D_id); + + // Try to transitively complete the track of a 3D point. + size_t Complete(const Options& options, const point3D_t point3D_id); + + // Check if camera has bogus parameters and cache the result. + bool HasCameraBogusParams(const Options& options, const Camera& camera); + + // Database cache for the reconstruction. Used to retrieve correspondence + // information for triangulation. + const CorrespondenceGraph* correspondence_graph_; + + // Reconstruction of the model. Modified when triangulating new points. + Reconstruction* reconstruction_; + + // Cache for cameras with bogus parameters. + std::unordered_map camera_has_bogus_params_; + + // Cache for tried track merges to avoid duplicate merge trials. + std::unordered_map> merge_trials_; + + // Cache for found correspondences in the graph. + std::vector found_corrs_; + + // Number of trials to retriangulate image pair. + std::unordered_map re_num_trials_; + + // Changed 3D points, i.e. if a 3D point is modified (created, continued, + // deleted, merged, etc.). Cleared once `ModifiedPoints3D` is called. + std::unordered_set modified_point3D_ids_; +}; + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM( + colmap::IncrementalTriangulator::CorrData) + +#endif // COLMAP_SRC_SFM_INCREMENTAL_TRIANGULATOR_H_ diff --git a/colmap/include/colmap/ui/automatic_reconstruction_widget.h b/colmap/include/colmap/ui/automatic_reconstruction_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..d1b5b0154ac4e4ef16d6a042708f6a3eb240ceb0 --- /dev/null +++ b/colmap/include/colmap/ui/automatic_reconstruction_widget.h @@ -0,0 +1,63 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_AUTOMATIC_RECONSTRUCTION_WIDGET_H_ +#define COLMAP_SRC_UI_AUTOMATIC_RECONSTRUCTION_WIDGET_H_ + +#include "controllers/automatic_reconstruction.h" +#include "ui/options_widget.h" +#include "ui/thread_control_widget.h" + +namespace colmap { + +class MainWindow; + +class AutomaticReconstructionWidget : public OptionsWidget { + public: + AutomaticReconstructionWidget(MainWindow* main_window); + + void Run(); + + private: + void RenderResult(); + + MainWindow* main_window_; + AutomaticReconstructionController::Options options_; + ThreadControlWidget* thread_control_widget_; + QComboBox* data_type_cb_; + QComboBox* quality_cb_; + QComboBox* mesher_cb_; + QAction* render_result_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_AUTOMATIC_RECONSTRUCTION_WIDGET_H_ diff --git a/colmap/include/colmap/ui/bundle_adjustment_widget.h b/colmap/include/colmap/ui/bundle_adjustment_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..e057587487504e4ad6cb12dd846d70fc1b43a5b2 --- /dev/null +++ b/colmap/include/colmap/ui/bundle_adjustment_widget.h @@ -0,0 +1,66 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_BUNDLE_ADJUSTMENT_WIDGET_H_ +#define COLMAP_SRC_UI_BUNDLE_ADJUSTMENT_WIDGET_H_ + +#include +#include + +#include "base/reconstruction.h" +#include "ui/options_widget.h" +#include "ui/thread_control_widget.h" +#include "util/option_manager.h" + +namespace colmap { + +class MainWindow; + +class BundleAdjustmentWidget : public OptionsWidget { + public: + BundleAdjustmentWidget(MainWindow* main_window, OptionManager* options); + + void Show(Reconstruction* reconstruction); + + private: + void Run(); + void Render(); + + MainWindow* main_window_; + OptionManager* options_; + Reconstruction* reconstruction_; + ThreadControlWidget* thread_control_widget_; + QAction* render_action_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_BUNDLE_ADJUSTMENT_WIDGET_H_ diff --git a/colmap/include/colmap/ui/colormaps.h b/colmap/include/colmap/ui/colormaps.h new file mode 100644 index 0000000000000000000000000000000000000000..a14c802dd7a9a8dae4e7f2405043129416de8fab --- /dev/null +++ b/colmap/include/colmap/ui/colormaps.h @@ -0,0 +1,176 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_COLORMAPS_H_ +#define COLMAP_SRC_UI_COLORMAPS_H_ + +#include + +#include "base/reconstruction.h" +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Base class for 3D point color mapping. +class PointColormapBase { + public: + PointColormapBase(); + virtual ~PointColormapBase() = default; + + virtual void Prepare(EIGEN_STL_UMAP(camera_t, Camera) & cameras, + EIGEN_STL_UMAP(image_t, Image) & images, + EIGEN_STL_UMAP(point3D_t, Point3D) & points3D, + std::vector& reg_image_ids) = 0; + + virtual Eigen::Vector4f ComputeColor(const point3D_t point3D_id, + const Point3D& point3D) = 0; + + void UpdateScale(std::vector* values); + float AdjustScale(const float gray); + + float scale; + float min; + float max; + float range; + float min_q; + float max_q; +}; + +// Map color according to RGB value from image. +class PointColormapPhotometric : public PointColormapBase { + public: + void Prepare(EIGEN_STL_UMAP(camera_t, Camera) & cameras, + EIGEN_STL_UMAP(image_t, Image) & images, + EIGEN_STL_UMAP(point3D_t, Point3D) & points3D, + std::vector& reg_image_ids) override; + + Eigen::Vector4f ComputeColor(const point3D_t point3D_id, + const Point3D& point3D) override; +}; + +// Map color according to error. +class PointColormapError : public PointColormapBase { + public: + void Prepare(EIGEN_STL_UMAP(camera_t, Camera) & cameras, + EIGEN_STL_UMAP(image_t, Image) & images, + EIGEN_STL_UMAP(point3D_t, Point3D) & points3D, + std::vector& reg_image_ids) override; + + Eigen::Vector4f ComputeColor(const point3D_t point3D_id, + const Point3D& point3D) override; +}; + +// Map color according to track length. +class PointColormapTrackLen : public PointColormapBase { + public: + void Prepare(EIGEN_STL_UMAP(camera_t, Camera) & cameras, + EIGEN_STL_UMAP(image_t, Image) & images, + EIGEN_STL_UMAP(point3D_t, Point3D) & points3D, + std::vector& reg_image_ids) override; + + Eigen::Vector4f ComputeColor(const point3D_t point3D_id, + const Point3D& point3D) override; +}; + +// Map color according to ground-resolution. +class PointColormapGroundResolution : public PointColormapBase { + public: + void Prepare(EIGEN_STL_UMAP(camera_t, Camera) & cameras, + EIGEN_STL_UMAP(image_t, Image) & images, + EIGEN_STL_UMAP(point3D_t, Point3D) & points3D, + std::vector& reg_image_ids) override; + + Eigen::Vector4f ComputeColor(const point3D_t point3D_id, + const Point3D& point3D) override; + + private: + std::unordered_map resolutions_; +}; + +// Base class for image color mapping. +class ImageColormapBase { + public: + ImageColormapBase(); + virtual ~ImageColormapBase() = default; + + virtual void Prepare(EIGEN_STL_UMAP(camera_t, Camera) & cameras, + EIGEN_STL_UMAP(image_t, Image) & images, + EIGEN_STL_UMAP(point3D_t, Point3D) & points3D, + std::vector& reg_image_ids) = 0; + + virtual void ComputeColor(const Image& image, Eigen::Vector4f* plane_color, + Eigen::Vector4f* frame_color) = 0; + + const static Eigen::Vector4f kDefaultPlaneColor; + const static Eigen::Vector4f kDefaultFrameColor; +}; + +// Use uniform color for all images. +class ImageColormapUniform : public ImageColormapBase { + public: + void Prepare(EIGEN_STL_UMAP(camera_t, Camera) & cameras, + EIGEN_STL_UMAP(image_t, Image) & images, + EIGEN_STL_UMAP(point3D_t, Point3D) & points3D, + std::vector& reg_image_ids) override; + + void ComputeColor(const Image& image, Eigen::Vector4f* plane_color, + Eigen::Vector4f* frame_color) override; + + Eigen::Vector4f uniform_plane_color = kDefaultPlaneColor; + Eigen::Vector4f uniform_frame_color = kDefaultFrameColor; +}; + +// Use color for images with specific words in their name. +class ImageColormapNameFilter : public ImageColormapBase { + public: + void Prepare(EIGEN_STL_UMAP(camera_t, Camera) & cameras, + EIGEN_STL_UMAP(image_t, Image) & images, + EIGEN_STL_UMAP(point3D_t, Point3D) & points3D, + std::vector& reg_image_ids) override; + + void AddColorForWord(const std::string& word, + const Eigen::Vector4f& plane_color, + const Eigen::Vector4f& frame_color); + + void ComputeColor(const Image& image, Eigen::Vector4f* plane_color, + Eigen::Vector4f* frame_color) override; + + private: + // The plane and frame colors for different words. + std::vector< + std::pair>> + image_name_colors_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_COLORMAPS_H_ diff --git a/colmap/include/colmap/ui/database_management_widget.h b/colmap/include/colmap/ui/database_management_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..b11c25863449f544e2a1444edb6a5da2dfc517f4 --- /dev/null +++ b/colmap/include/colmap/ui/database_management_widget.h @@ -0,0 +1,188 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_DATABASE_MANAGEMENT_WIDGET_H_ +#define COLMAP_SRC_UI_DATABASE_MANAGEMENT_WIDGET_H_ + +#include + +#include +#include + +#include "base/database.h" +#include "ui/image_viewer_widget.h" +#include "util/misc.h" +#include "util/option_manager.h" + +namespace colmap { + +//////////////////////////////////////////////////////////////////////////////// +// Matches +//////////////////////////////////////////////////////////////////////////////// + +class TwoViewInfoTab : public QWidget { + public: + TwoViewInfoTab() {} + TwoViewInfoTab(QWidget* parent, OptionManager* options, Database* database); + + void Clear(); + + protected: + void InitializeTable(const QStringList& table_header); + void ShowMatches(); + void FillTable(); + + OptionManager* options_; + Database* database_; + + const Image* image_; + std::vector> matches_; + std::vector configs_; + std::vector sorted_matches_idxs_; + + QTableWidget* table_widget_; + QLabel* info_label_; + FeatureImageViewerWidget* matches_viewer_widget_; +}; + +class MatchesTab : public TwoViewInfoTab { + public: + MatchesTab(QWidget* parent, OptionManager* options, Database* database); + + void Reload(const std::vector& images, const image_t image_id); +}; + +class TwoViewGeometriesTab : public TwoViewInfoTab { + public: + TwoViewGeometriesTab(QWidget* parent, OptionManager* options, + Database* database); + + void Reload(const std::vector& images, const image_t image_id); +}; + +class OverlappingImagesWidget : public QWidget { + public: + OverlappingImagesWidget(QWidget* parent, OptionManager* options, + Database* database); + + void ShowMatches(const std::vector& images, const image_t image_id); + + private: + void closeEvent(QCloseEvent* event); + + QWidget* parent_; + + OptionManager* options_; + + QTabWidget* tab_widget_; + MatchesTab* matches_tab_; + TwoViewGeometriesTab* two_view_geometries_tab_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Images, Cameras +//////////////////////////////////////////////////////////////////////////////// + +class CameraTab : public QWidget { + public: + CameraTab(QWidget* parent, Database* database); + + void Reload(); + void Clear(); + + private: + void itemChanged(QTableWidgetItem* item); + void Add(); + void SetModel(); + + Database* database_; + + std::vector cameras_; + + QTableWidget* table_widget_; + QLabel* info_label_; +}; + +class ImageTab : public QWidget { + public: + ImageTab(QWidget* parent, CameraTab* camera_tab, OptionManager* options, + Database* database); + + void Reload(); + void Clear(); + + private: + void itemChanged(QTableWidgetItem* item); + + void ShowImage(); + void ShowMatches(); + void SetCamera(); + void SplitCamera(); + + CameraTab* camera_tab_; + + OptionManager* options_; + Database* database_; + + std::vector images_; + + QTableWidget* table_widget_; + QLabel* info_label_; + + OverlappingImagesWidget* overlapping_images_widget_; + + FeatureImageViewerWidget* image_viewer_widget_; +}; + +class DatabaseManagementWidget : public QWidget { + public: + DatabaseManagementWidget(QWidget* parent, OptionManager* options); + + private: + void showEvent(QShowEvent* event); + void hideEvent(QHideEvent* event); + + void ClearMatches(); + void ClearTwoViewGeometries(); + + QWidget* parent_; + + OptionManager* options_; + Database database_; + + QTabWidget* tab_widget_; + ImageTab* image_tab_; + CameraTab* camera_tab_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_DATABASE_MANAGEMENT_WIDGET_H_ diff --git a/colmap/include/colmap/ui/dense_reconstruction_widget.h b/colmap/include/colmap/ui/dense_reconstruction_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..cad0267e75c2a133147dec574db470113b799a07 --- /dev/null +++ b/colmap/include/colmap/ui/dense_reconstruction_widget.h @@ -0,0 +1,108 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_DENSE_RECONSTRUCTION_WIDGET_H_ +#define COLMAP_SRC_UI_DENSE_RECONSTRUCTION_WIDGET_H_ + +#include +#include + +#include "mvs/fusion.h" +#include "ui/image_viewer_widget.h" +#include "ui/options_widget.h" +#include "ui/thread_control_widget.h" +#include "util/option_manager.h" + +namespace colmap { + +class MainWindow; + +class DenseReconstructionOptionsWidget : public QWidget { + public: + DenseReconstructionOptionsWidget(QWidget* parent, OptionManager* options); +}; + +class DenseReconstructionWidget : public QWidget { + public: + DenseReconstructionWidget(MainWindow* main_window, OptionManager* options); + + void Show(Reconstruction* reconstruction); + + private: + void showEvent(QShowEvent* event); + + void Undistort(); + void Stereo(); + void Fusion(); + void PoissonMeshing(); + void DelaunayMeshing(); + + void SelectWorkspacePath(); + std::string GetWorkspacePath(); + void RefreshWorkspace(); + + void WriteFusedPoints(); + void ShowMeshingInfo(); + + QWidget* GenerateTableButtonWidget(const std::string& image_name, + const std::string& type); + + MainWindow* main_window_; + OptionManager* options_; + Reconstruction* reconstruction_; + ThreadControlWidget* thread_control_widget_; + DenseReconstructionOptionsWidget* options_widget_; + ImageViewerWidget* image_viewer_widget_; + QLineEdit* workspace_path_text_; + QTableWidget* table_widget_; + QPushButton* undistortion_button_; + QPushButton* stereo_button_; + QPushButton* fusion_button_; + QPushButton* poisson_meshing_button_; + QPushButton* delaunay_meshing_button_; + QAction* refresh_workspace_action_; + QAction* write_fused_points_action_; + QAction* show_meshing_info_action_; + + bool photometric_done_; + bool geometric_done_; + + std::string images_path_; + std::string depth_maps_path_; + std::string normal_maps_path_; + + std::vector fused_points_; + std::vector> fused_points_visibility_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_DENSE_RECONSTRUCTION_WIDGET_H_ diff --git a/colmap/include/colmap/ui/feature_extraction_widget.h b/colmap/include/colmap/ui/feature_extraction_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..f9fb165fc9632fccab4c7de4c1f3ac74aaf883a4 --- /dev/null +++ b/colmap/include/colmap/ui/feature_extraction_widget.h @@ -0,0 +1,78 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_FEATURE_EXTRACTION_WIDGET_H_ +#define COLMAP_SRC_UI_FEATURE_EXTRACTION_WIDGET_H_ + +#include +#include + +#include "util/misc.h" +#include "util/option_manager.h" + +namespace colmap { + +class FeatureExtractionWidget : public QWidget { + public: + FeatureExtractionWidget(QWidget* parent, OptionManager* options); + + private: + void showEvent(QShowEvent* event); + void hideEvent(QHideEvent* event); + + void ReadOptions(); + void WriteOptions(); + + QGroupBox* CreateCameraModelBox(); + + void SelectCameraModel(const int code); + void Extract(); + + QWidget* parent_; + + OptionManager* options_; + + QComboBox* camera_model_cb_; + QCheckBox* single_camera_cb_; + QCheckBox* single_camera_per_folder_cb_; + QRadioButton* camera_params_exif_rb_; + QRadioButton* camera_params_custom_rb_; + QLabel* camera_params_info_; + QLineEdit* camera_params_text_; + + std::vector camera_model_ids_; + + QTabWidget* tab_widget_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_FEATURE_EXTRACTION_WIDGET_H_ diff --git a/colmap/include/colmap/ui/feature_matching_widget.h b/colmap/include/colmap/ui/feature_matching_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..94b867e5685d66586d0c5ac1e2c10fc80b7428f0 --- /dev/null +++ b/colmap/include/colmap/ui/feature_matching_widget.h @@ -0,0 +1,57 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_FEATURE_MATCHING_WIDGET_H_ +#define COLMAP_SRC_UI_FEATURE_MATCHING_WIDGET_H_ + +#include +#include + +#include "util/misc.h" +#include "util/option_manager.h" + +namespace colmap { + +class FeatureMatchingWidget : public QWidget { + public: + FeatureMatchingWidget(QWidget* parent, OptionManager* options); + + private: + void showEvent(QShowEvent* event); + void hideEvent(QHideEvent* event); + + QWidget* parent_; + QTabWidget* tab_widget_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_FEATURE_MATCHING_WIDGET_H_ diff --git a/colmap/include/colmap/ui/image_viewer_widget.h b/colmap/include/colmap/ui/image_viewer_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..868258bb3fa2a5ee4115c5f66c4b54b677c5895b --- /dev/null +++ b/colmap/include/colmap/ui/image_viewer_widget.h @@ -0,0 +1,143 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_IMAGE_VIEWER_WIDGET_H_ +#define COLMAP_SRC_UI_IMAGE_VIEWER_WIDGET_H_ + +#include +#include + +#include "base/database.h" +#include "base/projection.h" +#include "base/reconstruction.h" +#include "ui/qt_utils.h" +#include "util/option_manager.h" + +namespace colmap { + +class ModelViewerWidget; + +class ImageViewerGraphicsScene : public QGraphicsScene { + public: + ImageViewerGraphicsScene(); + + QGraphicsPixmapItem* ImagePixmapItem() const; + + private: + QGraphicsPixmapItem* image_pixmap_item_ = nullptr; +}; + +class ImageViewerWidget : public QWidget { + public: + explicit ImageViewerWidget(QWidget* parent); + + void ShowBitmap(const Bitmap& bitmap); + void ShowPixmap(const QPixmap& pixmap); + void ReadAndShow(const std::string& path); + + private: + static const double kZoomFactor; + + ImageViewerGraphicsScene graphics_scene_; + QGraphicsView* graphics_view_; + + protected: + void resizeEvent(QResizeEvent* event); + void closeEvent(QCloseEvent* event); + void ZoomIn(); + void ZoomOut(); + void Save(); + + QGridLayout* grid_layout_; + QHBoxLayout* button_layout_; +}; + +class FeatureImageViewerWidget : public ImageViewerWidget { + public: + FeatureImageViewerWidget(QWidget* parent, const std::string& switch_text); + + void ReadAndShowWithKeypoints(const std::string& path, + const FeatureKeypoints& keypoints, + const std::vector& tri_mask); + + void ReadAndShowWithMatches(const std::string& path1, + const std::string& path2, + const FeatureKeypoints& keypoints1, + const FeatureKeypoints& keypoints2, + const FeatureMatches& matches); + + protected: + void ShowOrHide(); + + QPixmap image1_; + QPixmap image2_; + bool switch_state_; + QPushButton* switch_button_; + const std::string switch_text_; +}; + +class DatabaseImageViewerWidget : public FeatureImageViewerWidget { + public: + DatabaseImageViewerWidget(QWidget* parent, + ModelViewerWidget* model_viewer_widget, + OptionManager* options); + + void ShowImageWithId(const image_t image_id); + + private: + void ResizeTable(); + void DeleteImage(); + + ModelViewerWidget* model_viewer_widget_; + + OptionManager* options_; + + QPushButton* delete_button_; + + image_t image_id_; + + QTableWidget* table_widget_; + QTableWidgetItem* image_id_item_; + QTableWidgetItem* camera_id_item_; + QTableWidgetItem* camera_model_item_; + QTableWidgetItem* camera_params_item_; + QTableWidgetItem* qvec_item_; + QTableWidgetItem* tvec_item_; + QTableWidgetItem* dimensions_item_; + QTableWidgetItem* num_points2D_item_; + QTableWidgetItem* num_points3D_item_; + QTableWidgetItem* num_obs_item_; + QTableWidgetItem* name_item_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_IMAGE_VIEWER_WIDGET_H_ diff --git a/colmap/include/colmap/ui/license_widget.h b/colmap/include/colmap/ui/license_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..0a43902b34bf7dfa7fcc25c699a2cc132ecddb53 --- /dev/null +++ b/colmap/include/colmap/ui/license_widget.h @@ -0,0 +1,55 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_LICENSE_WIDGET_H_ +#define COLMAP_SRC_UI_LICENSE_WIDGET_H_ + +#include + +namespace colmap { + +class LicenseWidget : public QTextEdit { + public: + explicit LicenseWidget(QWidget* parent); + + private: + QString GetCOLMAPLicense() const; + QString GetLSDLicense() const; + QString GetPBALicense() const; + QString GetPoissonReconLicense() const; + QString GetSiftGPULicense() const; + QString GetSQLiteLicense() const; + QString GetVLFeatLicense() const; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_LICENSE_WIDGET_H_ diff --git a/colmap/include/colmap/ui/line_painter.h b/colmap/include/colmap/ui/line_painter.h new file mode 100644 index 0000000000000000000000000000000000000000..4815340851d2cf47d29b7ad946a9a7bfa66d83bd --- /dev/null +++ b/colmap/include/colmap/ui/line_painter.h @@ -0,0 +1,71 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_LINE_PAINTER_H_ +#define COLMAP_SRC_UI_LINE_PAINTER_H_ + +#include +#include + +#include "ui/point_painter.h" + +namespace colmap { + +class LinePainter { + public: + LinePainter(); + ~LinePainter(); + + struct Data { + Data() {} + Data(const PointPainter::Data& p1, const PointPainter::Data& p2) + : point1(p1), point2(p2) {} + + PointPainter::Data point1; + PointPainter::Data point2; + }; + + void Setup(); + void Upload(const std::vector& data); + void Render(const QMatrix4x4& pmv_matrix, const int width, const int height, + const float line_width); + + private: + QOpenGLShaderProgram shader_program_; + QOpenGLVertexArrayObject vao_; + QOpenGLBuffer vbo_; + + size_t num_geoms_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_LINE_PAINTER_H_ diff --git a/colmap/include/colmap/ui/log_widget.h b/colmap/include/colmap/ui/log_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..859273c9be3ba1a60d31d608d9e0d5e1dead50a1 --- /dev/null +++ b/colmap/include/colmap/ui/log_widget.h @@ -0,0 +1,102 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_LOG_WIDGET_H_ +#define COLMAP_SRC_UI_LOG_WIDGET_H_ + +#include +#include + +#include +#include + +#include "util/option_manager.h" + +namespace colmap { + +template > +class StandardOutputRedirector : public std::basic_streambuf { + typedef void (*cb_func_ptr)(const Elem*, std::streamsize count, void* data); + + public: + StandardOutputRedirector(std::ostream& stream, cb_func_ptr cb_func, + void* data) + : stream_(stream), cb_func_(cb_func), data_(data) { + buf_ = stream_.rdbuf(this); + }; + + ~StandardOutputRedirector() { stream_.rdbuf(buf_); } + + std::streamsize xsputn(const Elem* ptr, std::streamsize count) { + cb_func_(ptr, count, data_); + return count; + } + + typename Tr::int_type overflow(typename Tr::int_type v) { + Elem ch = Tr::to_char_type(v); + cb_func_(&ch, 1, data_); + return Tr::not_eof(v); + } + + private: + std::basic_ostream& stream_; + std::streambuf* buf_; + cb_func_ptr cb_func_; + void* data_; +}; + +class LogWidget : public QWidget { + public: + LogWidget(QWidget* parent, const int max_num_blocks = 100000); + ~LogWidget(); + + void Append(const std::string& text); + void Flush(); + void Clear(); + + private: + static void Update(const char* text, std::streamsize count, + void* text_box_ptr); + + void SaveLog(); + + QMutex mutex_; + std::string text_queue_; + QPlainTextEdit* text_box_; + std::ofstream log_file_; + StandardOutputRedirector>* cout_redirector_; + StandardOutputRedirector>* cerr_redirector_; + StandardOutputRedirector>* clog_redirector_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_LOG_WIDGET_H_ diff --git a/colmap/include/colmap/ui/main_window.h b/colmap/include/colmap/ui/main_window.h new file mode 100644 index 0000000000000000000000000000000000000000..a855e9f0c4edf35b0b3c2db7b162bfd56923459a --- /dev/null +++ b/colmap/include/colmap/ui/main_window.h @@ -0,0 +1,242 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_MAIN_WINDOW_H_ +#define COLMAP_SRC_UI_MAIN_WINDOW_H_ + +#include +#include +#include + +#include "base/reconstruction.h" +#include "controllers/incremental_mapper.h" +#include "ui/automatic_reconstruction_widget.h" +#include "ui/bundle_adjustment_widget.h" +#include "ui/database_management_widget.h" +#include "ui/dense_reconstruction_widget.h" +#include "ui/feature_extraction_widget.h" +#include "ui/feature_matching_widget.h" +#include "ui/license_widget.h" +#include "ui/log_widget.h" +#include "ui/match_matrix_widget.h" +#include "ui/model_viewer_widget.h" +#include "ui/project_widget.h" +#include "ui/reconstruction_manager_widget.h" +#include "ui/reconstruction_options_widget.h" +#include "ui/reconstruction_stats_widget.h" +#include "ui/render_options_widget.h" +#include "ui/undistortion_widget.h" +#include "util/bitmap.h" + +namespace colmap { + +class MainWindow : public QMainWindow { + public: + explicit MainWindow(const OptionManager& options); + + void ImportReconstruction(const std::string& path); + + protected: + void closeEvent(QCloseEvent* event); + + private: + friend class AutomaticReconstructionWidget; + friend class BundleAdjustmentWidget; + friend class DenseReconstructionWidget; + + void CreateWidgets(); + void CreateActions(); + void CreateMenus(); + void CreateToolbar(); + void CreateStatusbar(); + void CreateControllers(); + + void ProjectNew(); + bool ProjectOpen(); + void ProjectEdit(); + void ProjectSave(); + void ProjectSaveAs(); + void Import(); + void ImportFrom(); + void Export(); + void ExportAll(); + void ExportAs(); + void ExportAsText(); + + void FeatureExtraction(); + void FeatureMatching(); + void DatabaseManagement(); + + void AutomaticReconstruction(); + + void ReconstructionStart(); + void ReconstructionStep(); + void ReconstructionPause(); + void ReconstructionReset(); + void ReconstructionOptions(); + void ReconstructionFinish(); + void ReconstructionNormalize(); + bool ReconstructionOverwrite(); + + void BundleAdjustment(); + void DenseReconstruction(); + + void Render(); + void RenderNow(); + void RenderToggle(); + void RenderOptions(); + void RenderSelectedReconstruction(); + void RenderClear(); + + void SelectReconstructionIdx(const size_t); + size_t SelectedReconstructionIdx(); + bool HasSelectedReconstruction(); + bool IsSelectedReconstructionValid(); + + void GrabImage(); + void UndistortImages(); + + void ReconstructionStats(); + void MatchMatrix(); + void ShowLog(); + void ExtractColors(); + + void SetOptions(); + void ResetOptions(); + + void About(); + void Documentation(); + void Support(); + + void ShowInvalidProjectError(); + void UpdateTimer(); + + void EnableBlockingActions(); + void DisableBlockingActions(); + + void UpdateWindowTitle(); + + OptionManager options_; + + ReconstructionManager reconstruction_manager_; + std::unique_ptr mapper_controller_; + + Timer timer_; + + ModelViewerWidget* model_viewer_widget_; + ProjectWidget* project_widget_; + FeatureExtractionWidget* feature_extraction_widget_; + FeatureMatchingWidget* feature_matching_widget_; + DatabaseManagementWidget* database_management_widget_; + AutomaticReconstructionWidget* automatic_reconstruction_widget_; + ReconstructionOptionsWidget* reconstruction_options_widget_; + BundleAdjustmentWidget* bundle_adjustment_widget_; + DenseReconstructionWidget* dense_reconstruction_widget_; + RenderOptionsWidget* render_options_widget_; + LogWidget* log_widget_; + UndistortionWidget* undistortion_widget_; + ReconstructionManagerWidget* reconstruction_manager_widget_; + ReconstructionStatsWidget* reconstruction_stats_widget_; + MatchMatrixWidget* match_matrix_widget_; + LicenseWidget* license_widget_; + ThreadControlWidget* thread_control_widget_; + + QToolBar* file_toolbar_; + QToolBar* preprocessing_toolbar_; + QToolBar* reconstruction_toolbar_; + QToolBar* render_toolbar_; + QToolBar* extras_toolbar_; + + QDockWidget* dock_log_widget_; + + QTimer* statusbar_timer_; + QLabel* statusbar_timer_label_; + + QAction* action_project_new_; + QAction* action_project_open_; + QAction* action_project_edit_; + QAction* action_project_save_; + QAction* action_project_save_as_; + QAction* action_import_; + QAction* action_import_from_; + QAction* action_export_; + QAction* action_export_all_; + QAction* action_export_as_; + QAction* action_export_as_text_; + QAction* action_quit_; + + QAction* action_feature_extraction_; + QAction* action_feature_matching_; + QAction* action_database_management_; + + QAction* action_automatic_reconstruction_; + + QAction* action_reconstruction_start_; + QAction* action_reconstruction_step_; + QAction* action_reconstruction_pause_; + QAction* action_reconstruction_reset_; + QAction* action_reconstruction_finish_; + QAction* action_reconstruction_normalize_; + QAction* action_reconstruction_options_; + + QAction* action_bundle_adjustment_; + QAction* action_dense_reconstruction_; + + QAction* action_render_; + QAction* action_render_now_; + QAction* action_render_toggle_; + QAction* action_render_reset_view_; + QAction* action_render_options_; + + QAction* action_reconstruction_stats_; + QAction* action_match_matrix_; + QAction* action_log_show_; + QAction* action_grab_image_; + QAction* action_grab_movie_; + QAction* action_undistort_; + QAction* action_extract_colors_; + QAction* action_set_options_; + QAction* action_reset_options_; + + QAction* action_about_; + QAction* action_documentation_; + QAction* action_support_; + QAction* action_license_; + + std::vector blocking_actions_; + + // Necessary for OS X to avoid duplicate closeEvents. + bool window_closed_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_MAIN_WINDOW_H_ diff --git a/colmap/include/colmap/ui/match_matrix_widget.h b/colmap/include/colmap/ui/match_matrix_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..96effbb96475f2992eaa5a29b1ebb38998d0944f --- /dev/null +++ b/colmap/include/colmap/ui/match_matrix_widget.h @@ -0,0 +1,53 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_MATCH_MATRIX_WIDGET_H_ +#define COLMAP_SRC_UI_MATCH_MATRIX_WIDGET_H_ + +#include "ui/image_viewer_widget.h" +#include "util/option_manager.h" + +namespace colmap { + +// Widget to visualize match matrix. +class MatchMatrixWidget : public ImageViewerWidget { + public: + MatchMatrixWidget(QWidget* parent, OptionManager* options); + + void Show(); + + private: + OptionManager* options_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_MATCH_MATRIX_WIDGET_H_ diff --git a/colmap/include/colmap/ui/model_viewer_widget.h b/colmap/include/colmap/ui/model_viewer_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..106163c964dfba76c5079954984c999fb0fc5baf --- /dev/null +++ b/colmap/include/colmap/ui/model_viewer_widget.h @@ -0,0 +1,214 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_MODEL_VIEWER_WIDGET_H_ +#define COLMAP_SRC_UI_MODEL_VIEWER_WIDGET_H_ + +#include +#include + +#include + +#include "base/database.h" +#include "base/reconstruction.h" +#include "ui/colormaps.h" +#include "ui/image_viewer_widget.h" +#include "ui/line_painter.h" +#include "ui/movie_grabber_widget.h" +#include "ui/point_painter.h" +#include "ui/point_viewer_widget.h" +#include "ui/render_options.h" +#include "ui/triangle_painter.h" +#include "util/option_manager.h" + +namespace colmap { + +class ModelViewerWidget : public QOpenGLWidget, + protected QOpenGLFunctions_3_2_Core { + public: + const float kInitNearPlane = 1.0f; + const float kMinNearPlane = 1e-3f; + const float kMaxNearPlane = 1e5f; + const float kNearPlaneScaleSpeed = 0.02f; + const float kFarPlane = 1e5f; + const float kInitFocusDistance = 100.0f; + const float kMinFocusDistance = 1e-5f; + const float kMaxFocusDistance = 1e8f; + const float kFieldOfView = 25.0f; + const float kFocusSpeed = 2.0f; + const float kInitPointSize = 1.0f; + const float kMinPointSize = 0.5f; + const float kMaxPointSize = 100.0f; + const float kPointScaleSpeed = 0.1f; + const float kInitImageSize = 0.2f; + const float kMinImageSize = 1e-6f; + const float kMaxImageSize = 1e3f; + const float kImageScaleSpeed = 0.1f; + const int kDoubleClickInterval = 250; + + ModelViewerWidget(QWidget* parent, OptionManager* options); + + void ReloadReconstruction(); + void ClearReconstruction(); + + int GetProjectionType() const; + + // Takes ownwership of the colormap objects. + void SetPointColormap(PointColormapBase* colormap); + void SetImageColormap(ImageColormapBase* colormap); + + void UpdateMovieGrabber(); + + void EnableCoordinateGrid(); + void DisableCoordinateGrid(); + + void ChangeFocusDistance(const float delta); + void ChangeNearPlane(const float delta); + void ChangePointSize(const float delta); + void ChangeCameraSize(const float delta); + + void RotateView(const float x, const float y, const float prev_x, + const float prev_y); + void TranslateView(const float x, const float y, const float prev_x, + const float prev_y); + + void ResetView(); + + QMatrix4x4 ModelViewMatrix() const; + void SetModelViewMatrix(const QMatrix4x4& matrix); + + void SelectObject(const int x, const int y); + void SelectMoviewGrabberView(const size_t view_idx); + + QImage GrabImage(); + void GrabMovie(); + + void ShowPointInfo(const point3D_t point3D_id); + void ShowImageInfo(const image_t image_id); + + float PointSize() const; + float ImageSize() const; + void SetPointSize(const float point_size); + void SetImageSize(const float image_size); + + void SetBackgroundColor(const float r, const float g, const float b); + + // Copy of current scene data that is displayed + Reconstruction* reconstruction = nullptr; + EIGEN_STL_UMAP(camera_t, Camera) cameras; + EIGEN_STL_UMAP(image_t, Image) images; + EIGEN_STL_UMAP(point3D_t, Point3D) points3D; + std::vector reg_image_ids; + + QLabel* statusbar_status_label; + + protected: + void initializeGL() override; + void resizeGL(int width, int height) override; + void paintGL() override; + + private: + void mousePressEvent(QMouseEvent* event) override; + void mouseReleaseEvent(QMouseEvent* event) override; + void mouseMoveEvent(QMouseEvent* event) override; + void wheelEvent(QWheelEvent* event) override; + + void SetupPainters(); + void SetupView(); + + void Upload(); + void UploadCoordinateGridData(); + void UploadPointData(const bool selection_mode = false); + void UploadPointConnectionData(); + void UploadImageData(const bool selection_mode = false); + void UploadImageConnectionData(); + void UploadMovieGrabberData(); + + void ComposeProjectionMatrix(); + + float ZoomScale() const; + float AspectRatio() const; + float OrthographicWindowExtent() const; + + Eigen::Vector3f PositionToArcballVector(const float x, const float y) const; + + OptionManager* options_; + + QMatrix4x4 model_view_matrix_; + QMatrix4x4 projection_matrix_; + + LinePainter coordinate_axes_painter_; + LinePainter coordinate_grid_painter_; + + PointPainter point_painter_; + LinePainter point_connection_painter_; + + LinePainter image_line_painter_; + TrianglePainter image_triangle_painter_; + LinePainter image_connection_painter_; + + LinePainter movie_grabber_path_painter_; + LinePainter movie_grabber_line_painter_; + TrianglePainter movie_grabber_triangle_painter_; + + PointViewerWidget* point_viewer_widget_; + DatabaseImageViewerWidget* image_viewer_widget_; + MovieGrabberWidget* movie_grabber_widget_; + + std::unique_ptr point_colormap_; + std::unique_ptr image_colormap_; + + bool mouse_is_pressed_; + QTimer mouse_press_timer_; + QPoint prev_mouse_pos_; + + float focus_distance_; + + std::vector> selection_buffer_; + image_t selected_image_id_; + point3D_t selected_point3D_id_; + size_t selected_movie_grabber_view_; + + bool coordinate_grid_enabled_; + + // Size of points (dynamic): does not require re-uploading of points. + float point_size_; + // Size of image models (not dynamic): requires re-uploading of image models. + float image_size_; + // Near clipping plane. + float near_plane_; + + float background_color_[3]; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_MODEL_VIEWER_WIDGET_H_ diff --git a/colmap/include/colmap/ui/movie_grabber_widget.h b/colmap/include/colmap/ui/movie_grabber_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..295d7754c6aab7f25a9158d5f9dd9f9468540337 --- /dev/null +++ b/colmap/include/colmap/ui/movie_grabber_widget.h @@ -0,0 +1,100 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_MOVIE_GRABBER_WIDGET_H_ +#define COLMAP_SRC_UI_MOVIE_GRABBER_WIDGET_H_ + +#include + +#include +#include +#include + +#include "base/reconstruction.h" + +namespace colmap { + +class ModelViewerWidget; + +class MovieGrabberWidget : public QWidget { + public: + MovieGrabberWidget(QWidget* parent, ModelViewerWidget* model_viewer_widget); + + // List of views, used to visualize the movie grabber camera path. + std::vector views; + + struct ViewData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + QMatrix4x4 model_view_matrix; + float point_size = -1.0f; + float image_size = -1.0f; + }; + + private: + // Add, delete, clear viewpoints. + void Add(); + void Delete(); + void Clear(); + + // Assemble movie from current viewpoints. + void Assemble(); + + // Event slot for time modification. + void TimeChanged(QTableWidgetItem* item); + + // Event slot for changed selection. + void SelectionChanged(const QItemSelection& selected, + const QItemSelection& deselected); + + // Update state when viewpoints reordered. + void UpdateViews(); + + ModelViewerWidget* model_viewer_widget_; + + QPushButton* assemble_button_; + QPushButton* add_button_; + QPushButton* delete_button_; + QPushButton* clear_button_; + QTableWidget* table_; + + QSpinBox* frame_rate_sb_; + QCheckBox* smooth_cb_; + QDoubleSpinBox* smoothness_sb_; + + EIGEN_STL_UMAP(const QTableWidgetItem*, ViewData) view_data_; +}; + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM( + colmap::MovieGrabberWidget::ViewData) + +#endif // COLMAP_SRC_UI_MOVIE_GRABBER_WIDGET_H_ diff --git a/colmap/include/colmap/ui/options_widget.h b/colmap/include/colmap/ui/options_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..cf6f8015be61a35933e0e6ba6b9a4549467c1e3d --- /dev/null +++ b/colmap/include/colmap/ui/options_widget.h @@ -0,0 +1,104 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_OPTIONS_WIDGET_H_ +#define COLMAP_SRC_UI_OPTIONS_WIDGET_H_ + +#include + +#include +#include + +namespace colmap { + +class OptionsWidget : public QWidget { + public: + explicit OptionsWidget(QWidget* parent); + + void AddOptionRow(const std::string& label_text, QWidget* widget, + void* option); + void AddWidgetRow(const std::string& label_text, QWidget* widget); + void AddLayoutRow(const std::string& label_text, QLayout* layout); + + QSpinBox* AddOptionInt(int* option, const std::string& label_text, + const int min = 0, + const int max = static_cast(1e7)); + QDoubleSpinBox* AddOptionDouble(double* option, const std::string& label_text, + const double min = 0, const double max = 1e7, + const double step = 0.01, + const int decimals = 2); + QDoubleSpinBox* AddOptionDoubleLog( + double* option, const std::string& label_text, const double min = 0, + const double max = 1e7, const double step = 0.01, const int decimals = 2); + QCheckBox* AddOptionBool(bool* option, const std::string& label_text); + QLineEdit* AddOptionText(std::string* option, const std::string& label_text); + QLineEdit* AddOptionFilePath(std::string* option, + const std::string& label_text); + QLineEdit* AddOptionDirPath(std::string* option, + const std::string& label_text); + + void AddSpacer(); + void AddSection(const std::string& title); + + void ReadOptions(); + void WriteOptions(); + + protected: + void showEvent(QShowEvent* event); + void closeEvent(QCloseEvent* event); + void hideEvent(QHideEvent* event); + + void ShowOption(void* option); + void HideOption(void* option); + + void ShowWidget(QWidget* option); + void HideWidget(QWidget* option); + + void ShowLayout(QLayout* option); + void HideLayout(QLayout* option); + + QGridLayout* grid_layout_; + + std::unordered_map> option_rows_; + std::unordered_map> widget_rows_; + std::unordered_map> layout_rows_; + + std::vector> options_int_; + std::vector> options_double_; + std::vector> options_double_log_; + std::vector> options_bool_; + std::vector> options_text_; + std::vector> options_path_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_OPTIONS_WIDGET_H_ diff --git a/colmap/include/colmap/ui/point_painter.h b/colmap/include/colmap/ui/point_painter.h new file mode 100644 index 0000000000000000000000000000000000000000..e3be243a88513435c712cafc02ef59960ed1467e --- /dev/null +++ b/colmap/include/colmap/ui/point_painter.h @@ -0,0 +1,69 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_POINT_PAINTER_H_ +#define COLMAP_SRC_UI_POINT_PAINTER_H_ + +#include +#include + +namespace colmap { + +class PointPainter { + public: + PointPainter(); + ~PointPainter(); + + struct Data { + Data() : x(0), y(0), z(0), r(0), g(0), b(0), a(0) {} + Data(const float x_, const float y_, const float z_, const float r_, + const float g_, const float b_, const float a_) + : x(x_), y(y_), z(z_), r(r_), g(g_), b(b_), a(a_) {} + + float x, y, z; + float r, g, b, a; + }; + + void Setup(); + void Upload(const std::vector& data); + void Render(const QMatrix4x4& pmv_matrix, const float point_size); + + private: + QOpenGLShaderProgram shader_program_; + QOpenGLVertexArrayObject vao_; + QOpenGLBuffer vbo_; + + size_t num_geoms_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_POINT_PAINTER_H_ diff --git a/colmap/include/colmap/ui/point_viewer_widget.h b/colmap/include/colmap/ui/point_viewer_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..40c3da8412ba246702a69356025eae675718fade --- /dev/null +++ b/colmap/include/colmap/ui/point_viewer_widget.h @@ -0,0 +1,90 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_POINT_VIEWER_WIDGET_H_ +#define COLMAP_SRC_UI_POINT_VIEWER_WIDGET_H_ + +#include +#include + +#include "base/reconstruction.h" +#include "util/option_manager.h" + +namespace colmap { + +class ModelViewerWidget; + +class PointViewerWidget : public QWidget { + public: + PointViewerWidget(QWidget* parent, ModelViewerWidget* model_viewer_widget, + OptionManager* option); + + void Show(const point3D_t point3D_id); + + private: + void closeEvent(QCloseEvent* event); + + void ResizeInfoTable(); + void ClearLocations(); + void UpdateImages(); + void ZoomIn(); + void ZoomOut(); + void Delete(); + + ModelViewerWidget* model_viewer_widget_; + + OptionManager* options_; + + QPushButton* delete_button_; + + point3D_t point3D_id_; + + QTableWidget* info_table_; + QTableWidgetItem* xyz_item_; + QTableWidgetItem* rgb_item_; + QTableWidgetItem* error_item_; + + QTableWidget* location_table_; + std::vector location_pixmaps_; + std::vector location_labels_; + std::vector image_ids_; + std::vector reproj_errors_; + std::vector image_names_; + + QPushButton* zoom_in_button_; + QPushButton* zoom_out_button_; + + double zoom_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_POINT_VIEWER_WIDGET_H_ diff --git a/colmap/include/colmap/ui/project_widget.h b/colmap/include/colmap/ui/project_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..3110378ef07f41be4fcd173eaa1ef2ab78d73eb3 --- /dev/null +++ b/colmap/include/colmap/ui/project_widget.h @@ -0,0 +1,74 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_PROJECT_WIDGET_H_ +#define COLMAP_SRC_UI_PROJECT_WIDGET_H_ + +#include +#include + +#include "util/misc.h" +#include "util/option_manager.h" + +namespace colmap { + +class ProjectWidget : public QWidget { + public: + ProjectWidget(QWidget* parent, OptionManager* options); + + bool IsValid() const; + void Reset(); + + std::string GetDatabasePath() const; + std::string GetImagePath() const; + void SetDatabasePath(const std::string& path); + void SetImagePath(const std::string& path); + + private: + void Save(); + void SelectNewDatabasePath(); + void SelectExistingDatabasePath(); + void SelectImagePath(); + QString DefaultDirectory(); + + OptionManager* options_; + + // Whether file dialog was opened previously. + bool prev_selected_; + + // Text boxes that hold the currently selected paths. + QLineEdit* database_path_text_; + QLineEdit* image_path_text_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_PROJECT_WIDGET_H_ diff --git a/colmap/include/colmap/ui/qt_utils.h b/colmap/include/colmap/ui/qt_utils.h new file mode 100644 index 0000000000000000000000000000000000000000..d4f3ca9b84ff3d835e9ae43bfe230e2d89fa1113 --- /dev/null +++ b/colmap/include/colmap/ui/qt_utils.h @@ -0,0 +1,65 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_QT_UTILS_H_ +#define COLMAP_SRC_UI_QT_UTILS_H_ + +#include + +#include +#include + +#include "feature/types.h" +#include "util/bitmap.h" +#include "util/types.h" + +namespace colmap { + +Eigen::Matrix4f QMatrixToEigen(const QMatrix4x4& matrix); + +QMatrix4x4 EigenToQMatrix(const Eigen::Matrix4f& matrix); + +QImage BitmapToQImageRGB(const Bitmap& bitmap); + +void DrawKeypoints(QPixmap* image, const FeatureKeypoints& points, + const QColor& color = Qt::red); + +QPixmap ShowImagesSideBySide(const QPixmap& image1, const QPixmap& image2); + +QPixmap DrawMatches(const QPixmap& image1, const QPixmap& image2, + const FeatureKeypoints& points1, + const FeatureKeypoints& points2, + const FeatureMatches& matches, + const QColor& keypoints_color = Qt::red); + +} // namespace colmap + +#endif // COLMAP_SRC_UI_QT_UTILS_H_ diff --git a/colmap/include/colmap/ui/reconstruction_manager_widget.h b/colmap/include/colmap/ui/reconstruction_manager_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..affcfe6d9e97094203ed14c0c693e0a89dbf22f1 --- /dev/null +++ b/colmap/include/colmap/ui/reconstruction_manager_widget.h @@ -0,0 +1,59 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_RECONSTRUCTION_MANAGER_WIDGET_H_ +#define COLMAP_SRC_UI_RECONSTRUCTION_MANAGER_WIDGET_H_ + +#include + +#include "base/reconstruction_manager.h" + +namespace colmap { + +class ReconstructionManagerWidget : public QComboBox { + public: + const static size_t kNewestReconstructionIdx; + + ReconstructionManagerWidget( + QWidget* parent, const ReconstructionManager* reconstruction_manager); + + void Update(); + + size_t SelectedReconstructionIdx() const; + void SelectReconstruction(const size_t idx); + + private: + const ReconstructionManager* reconstruction_manager_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_RECONSTRUCTION_MANAGER_WIDGET_H_ diff --git a/colmap/include/colmap/ui/reconstruction_options_widget.h b/colmap/include/colmap/ui/reconstruction_options_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..1bdb001d910b7ad9d31aa27e44da7e519564b867 --- /dev/null +++ b/colmap/include/colmap/ui/reconstruction_options_widget.h @@ -0,0 +1,80 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_RECONSTRUCTION_OPTIONS_WIDGET_H_ +#define COLMAP_SRC_UI_RECONSTRUCTION_OPTIONS_WIDGET_H_ + +#include +#include + +#include "ui/options_widget.h" +#include "util/option_manager.h" + +namespace colmap { + +class MapperGeneralOptionsWidget : public OptionsWidget { + public: + MapperGeneralOptionsWidget(QWidget* parent, OptionManager* options); +}; + +class MapperTriangulationOptionsWidget : public OptionsWidget { + public: + MapperTriangulationOptionsWidget(QWidget* parent, OptionManager* options); +}; + +class MapperRegistrationOptionsWidget : public OptionsWidget { + public: + MapperRegistrationOptionsWidget(QWidget* parent, OptionManager* options); +}; + +class MapperInitializationOptionsWidget : public OptionsWidget { + public: + MapperInitializationOptionsWidget(QWidget* parent, OptionManager* options); +}; + +class MapperBundleAdjustmentOptionsWidget : public OptionsWidget { + public: + MapperBundleAdjustmentOptionsWidget(QWidget* parent, OptionManager* options); +}; + +class MapperFilteringOptionsWidget : public OptionsWidget { + public: + MapperFilteringOptionsWidget(QWidget* parent, OptionManager* options); +}; + +class ReconstructionOptionsWidget : public QWidget { + public: + ReconstructionOptionsWidget(QWidget* parent, OptionManager* options); +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_RECONSTRUCTION_OPTIONS_WIDGET_H_ diff --git a/colmap/include/colmap/ui/reconstruction_stats_widget.h b/colmap/include/colmap/ui/reconstruction_stats_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..fe363a2e29a9769e02d259ef47860a5cd50674ec --- /dev/null +++ b/colmap/include/colmap/ui/reconstruction_stats_widget.h @@ -0,0 +1,55 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_RECONSTRUCTION_STATS_WIDGET_H_ +#define COLMAP_SRC_UI_RECONSTRUCTION_STATS_WIDGET_H_ + +#include + +#include "base/reconstruction.h" + +namespace colmap { + +class ReconstructionStatsWidget : public QWidget { + public: + explicit ReconstructionStatsWidget(QWidget* parent); + + void Show(const Reconstruction& reconstruction); + + private: + void AddStatistic(const QString& header, const QString& content); + + QTableWidget* stats_table_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_RECONSTRUCTION_STATS_WIDGET_H_ diff --git a/colmap/include/colmap/ui/render_options.h b/colmap/include/colmap/ui/render_options.h new file mode 100644 index 0000000000000000000000000000000000000000..f3e53fb44456b3ef9dd77166abb9d8b43ca1a403 --- /dev/null +++ b/colmap/include/colmap/ui/render_options.h @@ -0,0 +1,69 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_RENDER_OPTIONS_H_ +#define COLMAP_SRC_UI_RENDER_OPTIONS_H_ + +#include + +namespace colmap { + +struct RenderOptions { + enum ProjectionType { + PERSPECTIVE, + ORTHOGRAPHIC, + }; + + // Minimum track length for a point to be rendered. + int min_track_len = 3; + + // Maximum error for a point to be rendered. + double max_error = 2; + + // The rate of registered images at which to refresh. + int refresh_rate = 1; + + // Whether to automatically adjust the refresh rate. The bigger the + // reconstruction gets, the less frequently the scene is rendered. + bool adapt_refresh_rate = true; + + // Whether to visualize image connections. + bool image_connections = false; + + // The projection type of the renderer. + int projection_type = ProjectionType::PERSPECTIVE; + + bool Check() const; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_RENDER_OPTIONS_H_ diff --git a/colmap/include/colmap/ui/render_options_widget.h b/colmap/include/colmap/ui/render_options_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..3d84468e3a6f19c3158b5eda80c22ff8011aa50f --- /dev/null +++ b/colmap/include/colmap/ui/render_options_widget.h @@ -0,0 +1,99 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_RENDER_OPTIONS_WIDGET_H_ +#define COLMAP_SRC_UI_RENDER_OPTIONS_WIDGET_H_ + +#include +#include + +#include "sfm/incremental_mapper.h" +#include "ui/model_viewer_widget.h" +#include "ui/options_widget.h" + +namespace colmap { + +class RenderOptionsWidget : public OptionsWidget { + public: + RenderOptionsWidget(QWidget* parent, OptionManager* options, + ModelViewerWidget* model_viewer_widget); + + size_t counter; + bool automatic_update; + + QAction* action_render_now; + + private: + void closeEvent(QCloseEvent* event); + + void Apply(); + void ApplyProjection(); + void ApplyPointColormap(); + void ApplyImageColormap(); + void ApplyBackgroundColor(); + + void SelectColor(const std::string& title, Eigen::Vector4f* color); + void SelectPointColormap(const int idx); + void SelectImageColormap(const int idx); + + void IncreasePointSize(); + void DecreasePointSize(); + void IncreaseCameraSize(); + void DecreaseCameraSize(); + + void ImageColormapNameFilterAddWord(); + void ImageColormapNameFilterClearWords(); + + OptionManager* options_; + ModelViewerWidget* model_viewer_widget_; + + Eigen::Vector4f background_color_; + + QComboBox* projection_cb_; + + QComboBox* point3D_colormap_cb_; + + double point3D_colormap_scale_; + double point3D_colormap_min_q_; + double point3D_colormap_max_q_; + + QComboBox* image_colormap_cb_; + QPushButton* select_image_plane_color_; + QPushButton* select_image_frame_color_; + QHBoxLayout* image_colormap_name_filter_layout_; + Eigen::Vector4f image_plane_color_; + Eigen::Vector4f image_frame_color_; + ImageColormapNameFilter image_colormap_name_filter_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_RENDER_OPTIONS_WIDGET_H_ diff --git a/colmap/include/colmap/ui/thread_control_widget.h b/colmap/include/colmap/ui/thread_control_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..207709f19010541daac1eaa5f955b061846c8259 --- /dev/null +++ b/colmap/include/colmap/ui/thread_control_widget.h @@ -0,0 +1,61 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_THREAD_CONTROL_WIDGET_WIDGET_H_ +#define COLMAP_SRC_UI_THREAD_CONTROL_WIDGET_WIDGET_H_ + +#include +#include + +#include + +#include "util/threading.h" + +namespace colmap { + +class ThreadControlWidget : public QWidget { + public: + explicit ThreadControlWidget(QWidget* parent); + + void StartThread(const QString& progress_text, const bool stoppable, + std::unique_ptr thread); + void StartFunction(const QString& progress_text, + const std::function& func); + + private: + QProgressDialog* progress_bar_; + QAction* destructor_; + std::unique_ptr thread_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_THREAD_CONTROL_WIDGET_WIDGET_H_ diff --git a/colmap/include/colmap/ui/triangle_painter.h b/colmap/include/colmap/ui/triangle_painter.h new file mode 100644 index 0000000000000000000000000000000000000000..87efc27238e38c56891bffb344a2eb82fb5c32c9 --- /dev/null +++ b/colmap/include/colmap/ui/triangle_painter.h @@ -0,0 +1,72 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_TRIANGLE_PAINTER_H_ +#define COLMAP_SRC_UI_TRIANGLE_PAINTER_H_ + +#include +#include + +#include "ui/point_painter.h" + +namespace colmap { + +class TrianglePainter { + public: + TrianglePainter(); + ~TrianglePainter(); + + struct Data { + Data() {} + Data(const PointPainter::Data& p1, const PointPainter::Data& p2, + const PointPainter::Data& p3) + : point1(p1), point2(p2), point3(p3) {} + + PointPainter::Data point1; + PointPainter::Data point2; + PointPainter::Data point3; + }; + + void Setup(); + void Upload(const std::vector& data); + void Render(const QMatrix4x4& pmv_matrix); + + private: + QOpenGLShaderProgram shader_program_; + QOpenGLVertexArrayObject vao_; + QOpenGLBuffer vbo_; + + size_t num_geoms_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_TRIANGLE_PAINTER_H_ diff --git a/colmap/include/colmap/ui/undistortion_widget.h b/colmap/include/colmap/ui/undistortion_widget.h new file mode 100644 index 0000000000000000000000000000000000000000..7f97b6ab22d2bc454635a56d0852ce2ac3f837da --- /dev/null +++ b/colmap/include/colmap/ui/undistortion_widget.h @@ -0,0 +1,69 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_UNDISTORTION_WIDGET_H_ +#define COLMAP_SRC_UI_UNDISTORTION_WIDGET_H_ + +#include +#include + +#include "base/reconstruction.h" +#include "base/undistortion.h" +#include "ui/options_widget.h" +#include "ui/thread_control_widget.h" +#include "util/misc.h" +#include "util/option_manager.h" + +namespace colmap { + +class UndistortionWidget : public OptionsWidget { + public: + UndistortionWidget(QWidget* parent, const OptionManager* options); + + void Show(const Reconstruction& reconstruction); + bool IsValid() const; + + private: + void Undistort(); + + const OptionManager* options_; + const Reconstruction* reconstruction_; + + ThreadControlWidget* thread_control_widget_; + + QComboBox* output_format_; + UndistortCameraOptions undistortion_options_; + std::string output_path_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UI_UNDISTORTION_WIDGET_H_ diff --git a/colmap/include/colmap/util/alignment.h b/colmap/include/colmap/util/alignment.h new file mode 100644 index 0000000000000000000000000000000000000000..c35f41afadb26234e6d29cc287616d63070ae321 --- /dev/null +++ b/colmap/include/colmap/util/alignment.h @@ -0,0 +1,100 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_ALIGNMENT_H_ +#define COLMAP_SRC_UTIL_ALIGNMENT_H_ + +#include +#include +#include + +#include +#include +#include + +#ifndef EIGEN_ALIGNED_ALLOCATOR +#define EIGEN_ALIGNED_ALLOCATOR Eigen::aligned_allocator +#endif + +// Equivalent to EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION but with support for +// initializer lists, which is a C++11 feature and not supported by the Eigen. +// The initializer list extension is inspired by Theia and StackOverflow code. +#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(...) \ + namespace std { \ + template <> \ + class vector<__VA_ARGS__, std::allocator<__VA_ARGS__>> \ + : public vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__>> { \ + typedef vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__>> \ + vector_base; \ + \ + public: \ + typedef __VA_ARGS__ value_type; \ + typedef vector_base::allocator_type allocator_type; \ + typedef vector_base::size_type size_type; \ + typedef vector_base::iterator iterator; \ + explicit vector(const allocator_type& a = allocator_type()) \ + : vector_base(a) {} \ + template \ + vector(InputIterator first, InputIterator last, \ + const allocator_type& a = allocator_type()) \ + : vector_base(first, last, a) {} \ + vector(const vector& c) : vector_base(c) {} \ + explicit vector(size_type num, const value_type& val = value_type()) \ + : vector_base(num, val) {} \ + vector(iterator start, iterator end) : vector_base(start, end) {} \ + vector& operator=(const vector& x) { \ + vector_base::operator=(x); \ + return *this; \ + } \ + vector(initializer_list<__VA_ARGS__> list) \ + : vector_base(list.begin(), list.end()) {} \ + }; \ + } // namespace std + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Vector2d) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Vector4d) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Vector4f) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix2d) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix2f) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix4d) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix4f) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Affine3d) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Affine3f) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Quaterniond) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Quaternionf) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix) + +#define EIGEN_STL_UMAP(KEY, VALUE) \ + std::unordered_map, std::equal_to, \ + Eigen::aligned_allocator>> + +#endif // COLMAP_SRC_UTIL_ALIGNMENT_H_ diff --git a/colmap/include/colmap/util/bitmap.h b/colmap/include/colmap/util/bitmap.h new file mode 100644 index 0000000000000000000000000000000000000000..4c9abac023b5d805937076fd0094e24ff1a1a0cd --- /dev/null +++ b/colmap/include/colmap/util/bitmap.h @@ -0,0 +1,285 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_BITMAP_H_ +#define COLMAP_SRC_UTIL_BITMAP_H_ + +#include +#include +#include +#include +#include +#include +#include + +#ifdef _WIN32 +#define NOMINMAX +#include +#endif +#include + +#include "util/string.h" + +namespace colmap { + +// Templated bitmap color class. +template +struct BitmapColor { + BitmapColor(); + BitmapColor(const T gray); + BitmapColor(const T r, const T g, const T b); + + template + BitmapColor Cast() const; + + bool operator==(const BitmapColor& rhs) const; + bool operator!=(const BitmapColor& rhs) const; + + template + friend std::ostream& operator<<(std::ostream& output, + const BitmapColor& color); + + T r; + T g; + T b; +}; + +// Wrapper class around FreeImage bitmaps. +class Bitmap { + public: + Bitmap(); + + // Copy constructor. + Bitmap(const Bitmap& other); + // Move constructor. + Bitmap(Bitmap&& other); + + // Create bitmap object from existing FreeImage bitmap object. Note that + // this class takes ownership of the object. + explicit Bitmap(FIBITMAP* data); + + // Copy assignment. + Bitmap& operator=(const Bitmap& other); + // Move assignment. + Bitmap& operator=(Bitmap&& other); + + // Allocate bitmap by overwriting the existing data. + bool Allocate(const int width, const int height, const bool as_rgb); + + // Deallocate the bitmap by releasing the existing data. + void Deallocate(); + + // Get pointer to underlying FreeImage object. + inline const FIBITMAP* Data() const; + inline FIBITMAP* Data(); + + // Dimensions of bitmap. + inline int Width() const; + inline int Height() const; + inline int Channels() const; + + // Number of bits per pixel. This is 8 for grey and 24 for RGB image. + inline unsigned int BitsPerPixel() const; + + // Scan width of bitmap which differs from the actual image width to achieve + // 32 bit aligned memory. Also known as pitch or stride. + inline unsigned int ScanWidth() const; + + // Check whether image is grey- or colorscale. + inline bool IsRGB() const; + inline bool IsGrey() const; + + // Number of bytes required to store image. + size_t NumBytes() const; + + // Copy raw image data to array. + std::vector ConvertToRawBits() const; + std::vector ConvertToRowMajorArray() const; + std::vector ConvertToColMajorArray() const; + + // Manipulate individual pixels. For grayscale images, only the red element + // of the RGB color is used. + bool GetPixel(const int x, const int y, BitmapColor* color) const; + bool SetPixel(const int x, const int y, const BitmapColor& color); + + // Get pointer to y-th scanline, where the 0-th scanline is at the top. + const uint8_t* GetScanline(const int y) const; + + // Fill entire bitmap with uniform color. For grayscale images, the first + // element of the vector is used. + void Fill(const BitmapColor& color); + + // Interpolate color at given floating point position. + bool InterpolateNearestNeighbor(const double x, const double y, + BitmapColor* color) const; + bool InterpolateBilinear(const double x, const double y, + BitmapColor* color) const; + + // Extract EXIF information from bitmap. Returns false if no EXIF information + // is embedded in the bitmap. + bool ExifCameraModel(std::string* camera_model) const; + bool ExifFocalLength(double* focal_length) const; + bool ExifLatitude(double* latitude) const; + bool ExifLongitude(double* longitude) const; + bool ExifAltitude(double* altitude) const; + + // Read bitmap at given path and convert to grey- or colorscale. + bool Read(const std::string& path, const bool as_rgb = true); + + // Write image to file. Flags can be used to set e.g. the JPEG quality. + // Consult the FreeImage documentation for all available flags. + bool Write(const std::string& path, + const FREE_IMAGE_FORMAT format = FIF_UNKNOWN, + const int flags = 0) const; + + // Smooth the image using a Gaussian kernel. + void Smooth(const float sigma_x, const float sigma_y); + + // Rescale image to the new dimensions. + void Rescale(const int new_width, const int new_height, + const FREE_IMAGE_FILTER filter = FILTER_BILINEAR); + + // Clone the image to a new bitmap object. + Bitmap Clone() const; + Bitmap CloneAsGrey() const; + Bitmap CloneAsRGB() const; + + // Clone metadata from this bitmap object to another target bitmap object. + void CloneMetadata(Bitmap* target) const; + + // Read specific EXIF tag. + bool ReadExifTag(const FREE_IMAGE_MDMODEL model, const std::string& tag_name, + std::string* result) const; + + private: + typedef std::unique_ptr FIBitmapPtr; + + void SetPtr(FIBITMAP* data); + + static bool IsPtrGrey(FIBITMAP* data); + static bool IsPtrRGB(FIBITMAP* data); + static bool IsPtrSupported(FIBITMAP* data); + + FIBitmapPtr data_; + int width_; + int height_; + int channels_; +}; + +// Jet colormap inspired by Matlab. Grayvalues are expected in the range [0, 1] +// and are converted to RGB values in the same range. +class JetColormap { + public: + static float Red(const float gray); + static float Green(const float gray); + static float Blue(const float gray); + + private: + static float Interpolate(const float val, const float y0, const float x0, + const float y1, const float x1); + static float Base(const float val); +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +namespace internal { + +template +T2 BitmapColorCast(const T1 value) { + return std::min(static_cast(std::numeric_limits::max()), + std::max(static_cast(std::numeric_limits::min()), + std::round(value))); +} + +} // namespace internal + +template +BitmapColor::BitmapColor() : r(0), g(0), b(0) {} + +template +BitmapColor::BitmapColor(const T gray) : r(gray), g(gray), b(gray) {} + +template +BitmapColor::BitmapColor(const T r, const T g, const T b) + : r(r), g(g), b(b) {} + +template +template +BitmapColor BitmapColor::Cast() const { + BitmapColor color; + color.r = internal::BitmapColorCast(r); + color.g = internal::BitmapColorCast(g); + color.b = internal::BitmapColorCast(b); + return color; +} + +template +bool BitmapColor::operator==(const BitmapColor& rhs) const { + return r == rhs.r && g == rhs.g && b == rhs.b; +} + +template +bool BitmapColor::operator!=(const BitmapColor& rhs) const { + return r != rhs.r || g != rhs.g || b != rhs.b; +} + +template +std::ostream& operator<<(std::ostream& output, const BitmapColor& color) { + output << StringPrintf("RGB(%f, %f, %f)", static_cast(color.r), + static_cast(color.g), + static_cast(color.b)); + return output; +} + +FIBITMAP* Bitmap::Data() { return data_.get(); } +const FIBITMAP* Bitmap::Data() const { return data_.get(); } + +int Bitmap::Width() const { return width_; } +int Bitmap::Height() const { return height_; } +int Bitmap::Channels() const { return channels_; } + +unsigned int Bitmap::BitsPerPixel() const { + return FreeImage_GetBPP(data_.get()); +} + +unsigned int Bitmap::ScanWidth() const { + return FreeImage_GetPitch(data_.get()); +} + +bool Bitmap::IsRGB() const { return channels_ == 3; } + +bool Bitmap::IsGrey() const { return channels_ == 1; } + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_BITMAP_H_ diff --git a/colmap/include/colmap/util/cache.h b/colmap/include/colmap/util/cache.h new file mode 100644 index 0000000000000000000000000000000000000000..d5eaca8c2d2b432ea702322a9357c5a70b788d64 --- /dev/null +++ b/colmap/include/colmap/util/cache.h @@ -0,0 +1,273 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_CACHE_H_ +#define COLMAP_SRC_UTIL_CACHE_H_ + +#include +#include +#include + +#include "util/logging.h" + +namespace colmap { + +// Least Recently Used cache implementation. Whenever the cache size is +// exceeded, the least recently used (by Get and GetMutable) is deleted. +template +class LRUCache { + public: + LRUCache(const size_t max_num_elems, + const std::function& getter_func); + virtual ~LRUCache() = default; + + // The number of elements in the cache. + size_t NumElems() const; + size_t MaxNumElems() const; + + // Check whether the element with the given key exists. + bool Exists(const key_t& key) const; + + // Get the value of an element either from the cache or compute the new value. + const value_t& Get(const key_t& key); + value_t& GetMutable(const key_t& key); + + // Manually set the value of an element. Note that the ownership of the value + // is moved to the cache, which invalidates the object on the caller side. + virtual void Set(const key_t& key, value_t&& value); + + // Pop least recently used element from cache. + virtual void Pop(); + + // Clear all elements from cache. + virtual void Clear(); + + protected: + typedef typename std::pair key_value_pair_t; + typedef typename std::list::iterator list_iterator_t; + + // Maximum number of least-recently-used elements the cache remembers. + const size_t max_num_elems_; + + // List to keep track of the least-recently-used elements. + std::list elems_list_; + + // Mapping from key to location in the list. + std::unordered_map elems_map_; + + // Function to compute new values if not in the cache. + const std::function getter_func_; +}; + +// Least Recently Used cache implementation that is constrained by a maximum +// memory limitation of its elements. Whenever the memory limit is exceeded, the +// least recently used (by Get and GetMutable) is deleted. Each element must +// implement a `size_t NumBytes()` method that returns its size in memory. +template +class MemoryConstrainedLRUCache : public LRUCache { + public: + MemoryConstrainedLRUCache( + const size_t max_num_bytes, + const std::function& getter_func); + + size_t NumBytes() const; + size_t MaxNumBytes() const; + void UpdateNumBytes(const key_t& key); + + void Set(const key_t& key, value_t&& value) override; + void Pop() override; + void Clear() override; + + private: + using typename LRUCache::key_value_pair_t; + using typename LRUCache::list_iterator_t; + using LRUCache::max_num_elems_; + using LRUCache::elems_list_; + using LRUCache::elems_map_; + using LRUCache::getter_func_; + + const size_t max_num_bytes_; + size_t num_bytes_; + std::unordered_map elems_num_bytes_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +LRUCache::LRUCache( + const size_t max_num_elems, + const std::function& getter_func) + : max_num_elems_(max_num_elems), getter_func_(getter_func) { + CHECK(getter_func); + CHECK_GT(max_num_elems, 0); +} + +template +size_t LRUCache::NumElems() const { + return elems_map_.size(); +} + +template +size_t LRUCache::MaxNumElems() const { + return max_num_elems_; +} + +template +bool LRUCache::Exists(const key_t& key) const { + return elems_map_.find(key) != elems_map_.end(); +} + +template +const value_t& LRUCache::Get(const key_t& key) { + return GetMutable(key); +} + +template +value_t& LRUCache::GetMutable(const key_t& key) { + const auto it = elems_map_.find(key); + if (it == elems_map_.end()) { + Set(key, std::move(getter_func_(key))); + return elems_map_[key]->second; + } else { + elems_list_.splice(elems_list_.begin(), elems_list_, it->second); + return it->second->second; + } +} + +template +void LRUCache::Set(const key_t& key, value_t&& value) { + auto it = elems_map_.find(key); + elems_list_.push_front(key_value_pair_t(key, std::move(value))); + if (it != elems_map_.end()) { + elems_list_.erase(it->second); + elems_map_.erase(it); + } + elems_map_[key] = elems_list_.begin(); + if (elems_map_.size() > max_num_elems_) { + Pop(); + } +} + +template +void LRUCache::Pop() { + if (!elems_list_.empty()) { + auto last = elems_list_.end(); + --last; + elems_map_.erase(last->first); + elems_list_.pop_back(); + } +} + +template +void LRUCache::Clear() { + elems_list_.clear(); + elems_map_.clear(); +} + +template +MemoryConstrainedLRUCache::MemoryConstrainedLRUCache( + const size_t max_num_bytes, + const std::function& getter_func) + : LRUCache(std::numeric_limits::max(), getter_func), + max_num_bytes_(max_num_bytes), + num_bytes_(0) { + CHECK_GT(max_num_bytes, 0); +} + +template +size_t MemoryConstrainedLRUCache::NumBytes() const { + return num_bytes_; +} + +template +size_t MemoryConstrainedLRUCache::MaxNumBytes() const { + return max_num_bytes_; +} + +template +void MemoryConstrainedLRUCache::Set(const key_t& key, + value_t&& value) { + auto it = elems_map_.find(key); + elems_list_.push_front(key_value_pair_t(key, std::move(value))); + if (it != elems_map_.end()) { + elems_list_.erase(it->second); + elems_map_.erase(it); + } + elems_map_[key] = elems_list_.begin(); + + const size_t num_bytes = value.NumBytes(); + num_bytes_ += num_bytes; + elems_num_bytes_.emplace(key, num_bytes); + + while (num_bytes_ > max_num_bytes_ && elems_map_.size() > 1) { + Pop(); + } +} + +template +void MemoryConstrainedLRUCache::Pop() { + if (!elems_list_.empty()) { + auto last = elems_list_.end(); + --last; + num_bytes_ -= elems_num_bytes_.at(last->first); + CHECK_GE(num_bytes_, 0); + elems_num_bytes_.erase(last->first); + elems_map_.erase(last->first); + elems_list_.pop_back(); + } +} + +template +void MemoryConstrainedLRUCache::UpdateNumBytes( + const key_t& key) { + auto& num_bytes = elems_num_bytes_.at(key); + num_bytes_ -= num_bytes; + CHECK_GE(num_bytes_, 0); + num_bytes = LRUCache::Get(key).NumBytes(); + num_bytes_ += num_bytes; + + while (num_bytes_ > max_num_bytes_ && elems_map_.size() > 1) { + Pop(); + } +} + +template +void MemoryConstrainedLRUCache::Clear() { + LRUCache::Clear(); + num_bytes_ = 0; + elems_num_bytes_.clear(); +} + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_CACHE_H_ diff --git a/colmap/include/colmap/util/camera_specs.h b/colmap/include/colmap/util/camera_specs.h new file mode 100644 index 0000000000000000000000000000000000000000..2ef5296eada77d88f365cef4b0a04efe31ce54ae --- /dev/null +++ b/colmap/include/colmap/util/camera_specs.h @@ -0,0 +1,49 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_CAMERA_SPECS_H_ +#define COLMAP_SRC_UTIL_CAMERA_SPECS_H_ + +#include +#include +#include + +namespace colmap { + +// { make1 : ({ model1 : sensor-width in mm }, ...), ... } +typedef std::vector> camera_make_specs_t; +typedef std::unordered_map camera_specs_t; + +camera_specs_t InitializeCameraSpecs(); + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_CAMERA_SPECS_H_ diff --git a/colmap/include/colmap/util/cuda.h b/colmap/include/colmap/util/cuda.h new file mode 100644 index 0000000000000000000000000000000000000000..66e19e6c176cc3fa2acec2fe404f80ccea63e784 --- /dev/null +++ b/colmap/include/colmap/util/cuda.h @@ -0,0 +1,43 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_CUDA_H_ +#define COLMAP_SRC_UTIL_CUDA_H_ + +namespace colmap { + +int GetNumCudaDevices(); + +void SetBestCudaDevice(const int gpu_index); + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_CUDA_H_ diff --git a/colmap/include/colmap/util/cudacc.h b/colmap/include/colmap/util/cudacc.h new file mode 100644 index 0000000000000000000000000000000000000000..77be0c248e53c282a0f3ddd2fb7b18d47d0c8723 --- /dev/null +++ b/colmap/include/colmap/util/cudacc.h @@ -0,0 +1,66 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_CUDACC_H_ +#define COLMAP_SRC_UTIL_CUDACC_H_ + +#include + +#include + +#define CUDA_SAFE_CALL(error) CudaSafeCall(error, __FILE__, __LINE__) +#define CUDA_CHECK() CudaCheck(__FILE__, __LINE__) +#define CUDA_SYNC_AND_CHECK() CudaSyncAndCheck(__FILE__, __LINE__) + +namespace colmap { + +class CudaTimer { + public: + CudaTimer(); + ~CudaTimer(); + + void Print(const std::string& message); + + private: + cudaEvent_t start_; + cudaEvent_t stop_; + float elapsed_time_; +}; + +void CudaSafeCall(const cudaError_t error, const std::string& file, + const int line); + +void CudaCheck(const char* file, const int line); +void CudaSyncAndCheck(const char* file, const int line); + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_CUDACC_H_ diff --git a/colmap/include/colmap/util/endian.h b/colmap/include/colmap/util/endian.h new file mode 100644 index 0000000000000000000000000000000000000000..5d7b4ed85cf0c35c793e0826727d2cc39198a7df --- /dev/null +++ b/colmap/include/colmap/util/endian.h @@ -0,0 +1,165 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_ENDIAN_H_ +#define COLMAP_SRC_UTIL_ENDIAN_H_ + +#include +#include + +namespace colmap { + +// Reverse the order of each byte. +template +T ReverseBytes(const T& data); + +// Check the order in which bytes are stored in computer memory. +bool IsLittleEndian(); +bool IsBigEndian(); + +// Convert data between endianness and the native format. Note that, for float +// and double types, these functions are only valid if the format is IEEE-754. +// This is the case for pretty much most processors. +template +T LittleEndianToNative(const T x); +template +T BigEndianToNative(const T x); +template +T NativeToLittleEndian(const T x); +template +T NativeToBigEndian(const T x); + +// Read data in little endian format for cross-platform support. +template +T ReadBinaryLittleEndian(std::istream* stream); +template +void ReadBinaryLittleEndian(std::istream* stream, std::vector* data); + +// Write data in little endian format for cross-platform support. +template +void WriteBinaryLittleEndian(std::ostream* stream, const T& data); +template +void WriteBinaryLittleEndian(std::ostream* stream, const std::vector& data); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +T ReverseBytes(const T& data) { + T data_reversed = data; + std::reverse(reinterpret_cast(&data_reversed), + reinterpret_cast(&data_reversed) + sizeof(T)); + return data_reversed; +} + +inline bool IsLittleEndian() { +#ifdef BOOST_BIG_ENDIAN + return false; +#else + return true; +#endif +} + +inline bool IsBigEndian() { +#ifdef BOOST_BIG_ENDIAN + return true; +#else + return false; +#endif +} + +template +T LittleEndianToNative(const T x) { + if (IsLittleEndian()) { + return x; + } else { + return ReverseBytes(x); + } +} + +template +T BigEndianToNative(const T x) { + if (IsBigEndian()) { + return x; + } else { + return ReverseBytes(x); + } +} + +template +T NativeToLittleEndian(const T x) { + if (IsLittleEndian()) { + return x; + } else { + return ReverseBytes(x); + } +} + +template +T NativeToBigEndian(const T x) { + if (IsBigEndian()) { + return x; + } else { + return ReverseBytes(x); + } +} + +template +T ReadBinaryLittleEndian(std::istream* stream) { + T data_little_endian; + stream->read(reinterpret_cast(&data_little_endian), sizeof(T)); + return LittleEndianToNative(data_little_endian); +} + +template +void ReadBinaryLittleEndian(std::istream* stream, std::vector* data) { + for (size_t i = 0; i < data->size(); ++i) { + (*data)[i] = ReadBinaryLittleEndian(stream); + } +} + +template +void WriteBinaryLittleEndian(std::ostream* stream, const T& data) { + const T data_little_endian = NativeToLittleEndian(data); + stream->write(reinterpret_cast(&data_little_endian), sizeof(T)); +} + +template +void WriteBinaryLittleEndian(std::ostream* stream, const std::vector& data) { + for (const auto& elem : data) { + WriteBinaryLittleEndian(stream, elem); + } +} + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_ENDIAN_H_ diff --git a/colmap/include/colmap/util/logging.h b/colmap/include/colmap/util/logging.h new file mode 100644 index 0000000000000000000000000000000000000000..28117919c055211668d2a720c1d36ed6d74976f5 --- /dev/null +++ b/colmap/include/colmap/util/logging.h @@ -0,0 +1,93 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_LOGGING_H_ +#define COLMAP_SRC_UTIL_LOGGING_H_ + +#include + +#include + +#include "util/string.h" + +// Option checker macros. In contrast to glog, this function does not abort the +// program, but simply returns false on failure. +#define CHECK_OPTION_IMPL(expr) \ + __CheckOptionImpl(__FILE__, __LINE__, (expr), #expr) +#define CHECK_OPTION(expr) \ + if (!__CheckOptionImpl(__FILE__, __LINE__, (expr), #expr)) { \ + return false; \ + } +#define CHECK_OPTION_OP(name, op, val1, val2) \ + if (!__CheckOptionOpImpl(__FILE__, __LINE__, (val1 op val2), val1, val2, \ + #val1, #val2, #op)) { \ + return false; \ + } +#define CHECK_OPTION_EQ(val1, val2) CHECK_OPTION_OP(_EQ, ==, val1, val2) +#define CHECK_OPTION_NE(val1, val2) CHECK_OPTION_OP(_NE, !=, val1, val2) +#define CHECK_OPTION_LE(val1, val2) CHECK_OPTION_OP(_LE, <=, val1, val2) +#define CHECK_OPTION_LT(val1, val2) CHECK_OPTION_OP(_LT, <, val1, val2) +#define CHECK_OPTION_GE(val1, val2) CHECK_OPTION_OP(_GE, >=, val1, val2) +#define CHECK_OPTION_GT(val1, val2) CHECK_OPTION_OP(_GT, >, val1, val2) + +namespace colmap { + +// Initialize glog at the beginning of the program. +void InitializeGlog(char** argv); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +const char* __GetConstFileBaseName(const char* file); + +bool __CheckOptionImpl(const char* file, const int line, const bool result, + const char* expr_str); + +template +bool __CheckOptionOpImpl(const char* file, const int line, const bool result, + const T1& val1, const T2& val2, const char* val1_str, + const char* val2_str, const char* op_str) { + if (result) { + return true; + } else { + std::cerr << StringPrintf("[%s:%d] Check failed: %s %s %s (%s vs. %s)", + __GetConstFileBaseName(file), line, val1_str, + op_str, val2_str, std::to_string(val1).c_str(), + std::to_string(val2).c_str()) + << std::endl; + return false; + } +} + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_LOGGING_H_ diff --git a/colmap/include/colmap/util/math.h b/colmap/include/colmap/util/math.h new file mode 100644 index 0000000000000000000000000000000000000000..7a656460a675af9f8a876fc409ce100c059d0b4a --- /dev/null +++ b/colmap/include/colmap/util/math.h @@ -0,0 +1,321 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_MATH_H_ +#define COLMAP_SRC_UTIL_MATH_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include "util/logging.h" + +#ifndef M_PI +#define M_PI 3.14159265358979323846264338327950288 +#endif + +namespace colmap { + +// Return 1 if number is positive, -1 if negative, and 0 if the number is 0. +template +int SignOfNumber(const T val); + +// Check if the given floating point number is a not-a-number (NaN) value. +inline bool IsNaN(const float x); +inline bool IsNaN(const double x); + +// Check if the given floating point number is a infinity. +inline bool IsInf(const float x); +inline bool IsInf(const double x); + +// Clip the given value to a low and maximum value. +template +inline T Clip(const T& value, const T& low, const T& high); + +// Convert angle in degree to radians. +inline float DegToRad(const float deg); +inline double DegToRad(const double deg); + +// Convert angle in radians to degree. +inline float RadToDeg(const float rad); +inline double RadToDeg(const double rad); + +// Determine median value in vector. Returns NaN for empty vectors. +template +double Median(const std::vector& elems); + +// Determine mean value in a vector. +template +double Mean(const std::vector& elems); + +// Determine sample variance in a vector. +template +double Variance(const std::vector& elems); + +// Determine sample standard deviation in a vector. +template +double StdDev(const std::vector& elems); + +// Check if any of the values in the vector is less than the given threshold. +template +bool AnyLessThan(std::vector elems, T threshold); + +// Check if any of the values in the vector is greater than the given threshold. +template +bool AnyGreaterThan(std::vector elems, T threshold); + +// Generate N-choose-K combinations. +// +// Note that elements in range [first, last) must be in sorted order, +// according to `std::less`. +template +bool NextCombination(Iterator first, Iterator middle, Iterator last); + +// Sigmoid function. +template +T Sigmoid(const T x, const T alpha = 1); + +// Scale values according to sigmoid transform. +// +// x \in [0, 1] -> x \in [-x0, x0] -> sigmoid(x, alpha) -> x \in [0, 1] +// +// @param x Value to be scaled in the range [0, 1]. +// @param x0 Spread that determines the range x is scaled to. +// @param alpha Exponential sigmoid factor. +// +// @return The scaled value in the range [0, 1]. +template +T ScaleSigmoid(T x, const T alpha = 1, const T x0 = 10); + +// Binomial coefficient or all combinations, defined as n! / ((n - k)! k!). +size_t NChooseK(const size_t n, const size_t k); + +// Cast value from one type to another and truncate instead of overflow, if the +// input value is out of range of the output data type. +template +T2 TruncateCast(const T1 value); + +// Compute the n-th percentile in the given sequence. +template +T Percentile(const std::vector& elems, const double p); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +namespace internal { + +template +bool NextCombination(Iterator first1, Iterator last1, Iterator first2, + Iterator last2) { + if ((first1 == last1) || (first2 == last2)) { + return false; + } + Iterator m1 = last1; + Iterator m2 = last2; + --m2; + while (--m1 != first1 && *m1 >= *m2) { + } + bool result = (m1 == first1) && *first1 >= *m2; + if (!result) { + while (first2 != m2 && *m1 >= *first2) { + ++first2; + } + first1 = m1; + std::iter_swap(first1, first2); + ++first1; + ++first2; + } + if ((first1 != last1) && (first2 != last2)) { + m1 = last1; + m2 = first2; + while ((m1 != first1) && (m2 != last2)) { + std::iter_swap(--m1, m2); + ++m2; + } + std::reverse(first1, m1); + std::reverse(first1, last1); + std::reverse(m2, last2); + std::reverse(first2, last2); + } + return !result; +} + +} // namespace internal + +template +int SignOfNumber(const T val) { + return (T(0) < val) - (val < T(0)); +} + +bool IsNaN(const float x) { return x != x; } +bool IsNaN(const double x) { return x != x; } + +bool IsInf(const float x) { return !IsNaN(x) && IsNaN(x - x); } +bool IsInf(const double x) { return !IsNaN(x) && IsNaN(x - x); } + +template +T Clip(const T& value, const T& low, const T& high) { + return std::max(low, std::min(value, high)); +} + +float DegToRad(const float deg) { + return deg * 0.0174532925199432954743716805978692718781530857086181640625f; +} + +double DegToRad(const double deg) { + return deg * 0.0174532925199432954743716805978692718781530857086181640625; +} + +// Convert angle in radians to degree. +float RadToDeg(const float rad) { + return rad * 57.29577951308232286464772187173366546630859375f; +} + +double RadToDeg(const double rad) { + return rad * 57.29577951308232286464772187173366546630859375; +} + +template +double Median(const std::vector& elems) { + CHECK(!elems.empty()); + + const size_t mid_idx = elems.size() / 2; + + std::vector ordered_elems = elems; + std::nth_element(ordered_elems.begin(), ordered_elems.begin() + mid_idx, + ordered_elems.end()); + + if (elems.size() % 2 == 0) { + const T mid_element1 = ordered_elems[mid_idx]; + const T mid_element2 = *std::max_element(ordered_elems.begin(), + ordered_elems.begin() + mid_idx); + return (mid_element1 + mid_element2) / 2.0; + } else { + return ordered_elems[mid_idx]; + } +} + +template +T Percentile(const std::vector& elems, const double p) { + CHECK(!elems.empty()); + CHECK_GE(p, 0); + CHECK_LE(p, 100); + + const int idx = static_cast(std::round(p / 100 * (elems.size() - 1))); + const size_t percentile_idx = + std::max(0, std::min(static_cast(elems.size() - 1), idx)); + + std::vector ordered_elems = elems; + std::nth_element(ordered_elems.begin(), + ordered_elems.begin() + percentile_idx, ordered_elems.end()); + + return ordered_elems.at(percentile_idx); +} + +template +double Mean(const std::vector& elems) { + CHECK(!elems.empty()); + double sum = 0; + for (const auto el : elems) { + sum += static_cast(el); + } + return sum / elems.size(); +} + +template +double Variance(const std::vector& elems) { + const double mean = Mean(elems); + double var = 0; + for (const auto el : elems) { + const double diff = el - mean; + var += diff * diff; + } + return var / (elems.size() - 1); +} + +template +double StdDev(const std::vector& elems) { + return std::sqrt(Variance(elems)); +} + +template +bool AnyLessThan(std::vector elems, T threshold) { + for (const auto& el : elems) { + if (el < threshold) { + return true; + } + } + return false; +} + +template +bool AnyGreaterThan(std::vector elems, T threshold) { + for (const auto& el : elems) { + if (el > threshold) { + return true; + } + } + return false; +} + +template +bool NextCombination(Iterator first, Iterator middle, Iterator last) { + return internal::NextCombination(first, middle, middle, last); +} + +template +T Sigmoid(const T x, const T alpha) { + return T(1) / (T(1) + exp(-x * alpha)); +} + +template +T ScaleSigmoid(T x, const T alpha, const T x0) { + const T t0 = Sigmoid(-x0, alpha); + const T t1 = Sigmoid(x0, alpha); + x = (Sigmoid(2 * x0 * x - x0, alpha) - t0) / (t1 - t0); + return x; +} + +template +T2 TruncateCast(const T1 value) { + return std::min( + static_cast(std::numeric_limits::max()), + std::max(static_cast(std::numeric_limits::min()), value)); +} + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_MATH_H_ diff --git a/colmap/include/colmap/util/matrix.h b/colmap/include/colmap/util/matrix.h new file mode 100644 index 0000000000000000000000000000000000000000..9024d4a5e4adaa42bba813d74cee1034f011a482 --- /dev/null +++ b/colmap/include/colmap/util/matrix.h @@ -0,0 +1,97 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_MATRIX_H_ +#define COLMAP_SRC_UTIL_MATRIX_H_ + +#include +#include +#include + +namespace colmap { + +// Check if the given floating point array contains a NaN value. +template +inline bool IsNaN(const Eigen::MatrixBase& x); + +// Check if the given floating point array contains infinity. +template +inline bool IsInf(const Eigen::MatrixBase& x); + +// Perform RQ decomposition on matrix. The RQ decomposition transforms a matrix +// A into the product of an upper triangular matrix R (also known as +// right-triangular) and an orthogonal matrix Q. +template +void DecomposeMatrixRQ(const MatrixType& A, MatrixType* R, MatrixType* Q); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +bool IsNaN(const Eigen::MatrixBase& x) { + return !(x.array() == x.array()).all(); +} + +template +bool IsInf(const Eigen::MatrixBase& x) { + return !((x - x).array() == (x - x).array()).all(); +} + +template +void DecomposeMatrixRQ(const MatrixType& A, MatrixType* R, MatrixType* Q) { + const MatrixType A_flipud_transpose = + A.transpose().rowwise().reverse().eval(); + + const Eigen::HouseholderQR QR(A_flipud_transpose); + const MatrixType& Q0 = QR.householderQ(); + const MatrixType& R0 = QR.matrixQR(); + + *R = R0.transpose().colwise().reverse().eval(); + *R = R->rowwise().reverse().eval(); + for (int i = 0; i < R->rows(); ++i) { + for (int j = 0; j < R->cols() && (R->cols() - j) > (R->rows() - i); ++j) { + (*R)(i, j) = 0; + } + } + + *Q = Q0.transpose().colwise().reverse().eval(); + + // Make the decomposition unique by requiring that det(Q) > 0. + if (Q->determinant() < 0) { + Q->row(1) *= -1.0; + R->col(1) *= -1.0; + } +} + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_MATRIX_H_ diff --git a/colmap/include/colmap/util/misc.h b/colmap/include/colmap/util/misc.h new file mode 100644 index 0000000000000000000000000000000000000000..ba8c7c5032397d26f0f54d9b1d68af0896154713 --- /dev/null +++ b/colmap/include/colmap/util/misc.h @@ -0,0 +1,199 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_MISC_H_ +#define COLMAP_SRC_UTIL_MISC_H_ + +#include +#include +#include +#include + +#include + +#include "util/endian.h" +#include "util/logging.h" +#include "util/string.h" + +namespace colmap { + +#ifndef STRINGIFY +#define STRINGIFY(s) STRINGIFY_(s) +#define STRINGIFY_(s) #s +#endif // STRINGIFY + +enum class CopyType { COPY, HARD_LINK, SOFT_LINK }; + +// Append trailing slash to string if it does not yet end with a slash. +std::string EnsureTrailingSlash(const std::string& str); + +// Check whether file name has the file extension (case insensitive). +bool HasFileExtension(const std::string& file_name, const std::string& ext); + +// Split the path into its root and extension, for example, +// "dir/file.jpg" into "dir/file" and ".jpg". +void SplitFileExtension(const std::string& path, std::string* root, + std::string* ext); + +// Copy or link file from source to destination path +void FileCopy(const std::string& src_path, const std::string& dst_path, + CopyType type = CopyType::COPY); + +// Check if the path points to an existing directory. +bool ExistsFile(const std::string& path); + +// Check if the path points to an existing directory. +bool ExistsDir(const std::string& path); + +// Check if the path points to an existing file or directory. +bool ExistsPath(const std::string& path); + +// Create the directory if it does not exist. +void CreateDirIfNotExists(const std::string& path, bool recursive = false); + +// Extract the base name of a path, e.g., "image.jpg" for "/dir/image.jpg". +std::string GetPathBaseName(const std::string& path); + +// Get the path of the parent directory for the given path. +std::string GetParentDir(const std::string& path); + +// Join multiple paths into one path. +template +std::string JoinPaths(T const&... paths); + +// Return list of files in directory. +std::vector GetFileList(const std::string& path); + +// Return list of files, recursively in all sub-directories. +std::vector GetRecursiveFileList(const std::string& path); + +// Return list of directories, recursively in all sub-directories. +std::vector GetDirList(const std::string& path); + +// Return list of directories, recursively in all sub-directories. +std::vector GetRecursiveDirList(const std::string& path); + +// Get the size in bytes of a file. +size_t GetFileSize(const std::string& path); + +// Print first-order heading with over- and underscores to `std::cout`. +void PrintHeading1(const std::string& heading); + +// Print second-order heading with underscores to `std::cout`. +void PrintHeading2(const std::string& heading); + +// Check if vector contains elements. +template +bool VectorContainsValue(const std::vector& vector, const T value); + +template +bool VectorContainsDuplicateValues(const std::vector& vector); + +// Parse CSV line to a list of values. +template +std::vector CSVToVector(const std::string& csv); + +// Concatenate values in list to comma-separated list. +template +std::string VectorToCSV(const std::vector& values); + +// Read contiguous binary blob from file. +template +void ReadBinaryBlob(const std::string& path, std::vector* data); + +// Write contiguous binary blob to file. +template +void WriteBinaryBlob(const std::string& path, const std::vector& data); + +// Read each line of a text file into a separate element. Empty lines are +// ignored and leading/trailing whitespace is removed. +std::vector ReadTextFileLines(const std::string& path); + +// Remove an argument from the list of command-line arguments. +void RemoveCommandLineArgument(const std::string& arg, int* argc, char** argv); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +std::string JoinPaths(T const&... paths) { + boost::filesystem::path result; + int unpack[]{0, (result = result / boost::filesystem::path(paths), 0)...}; + static_cast(unpack); + return result.string(); +} + +template +bool VectorContainsValue(const std::vector& vector, const T value) { + return std::find_if(vector.begin(), vector.end(), [value](const T element) { + return element == value; + }) != vector.end(); +} + +template +bool VectorContainsDuplicateValues(const std::vector& vector) { + std::vector unique_vector = vector; + return std::unique(unique_vector.begin(), unique_vector.end()) != + unique_vector.end(); +} + +template +std::string VectorToCSV(const std::vector& values) { + std::string string; + for (const T value : values) { + string += std::to_string(value) + ", "; + } + return string.substr(0, string.length() - 2); +} + +template +void ReadBinaryBlob(const std::string& path, std::vector* data) { + std::ifstream file(path, std::ios::binary | std::ios::ate); + CHECK(file.is_open()) << path; + file.seekg(0, std::ios::end); + const size_t num_bytes = file.tellg(); + CHECK_EQ(num_bytes % sizeof(T), 0); + data->resize(num_bytes / sizeof(T)); + file.seekg(0, std::ios::beg); + ReadBinaryLittleEndian(&file, data); +} + +template +void WriteBinaryBlob(const std::string& path, const std::vector& data) { + std::ofstream file(path, std::ios::binary); + CHECK(file.is_open()) << path; + WriteBinaryLittleEndian(&file, data); +} + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_MISC_H_ diff --git a/colmap/include/colmap/util/opengl_utils.h b/colmap/include/colmap/util/opengl_utils.h new file mode 100644 index 0000000000000000000000000000000000000000..11a4ed19fd614337577789657cfcdff0ff5de029 --- /dev/null +++ b/colmap/include/colmap/util/opengl_utils.h @@ -0,0 +1,110 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_OPENGL_UTILS_H_ +#define COLMAP_SRC_OPENGL_UTILS_H_ + +#ifdef GUI_ENABLED +#include +#include +#include +#include +#include +#include +#endif + +#include "util/threading.h" + +namespace colmap { + +#ifdef DEBUG +#define glDebugLog() glError(__FILE__, __LINE__) +#else +#define glDebugLog() +#endif + +#ifdef GUI_ENABLED + +// This class manages a thread-safe OpenGL context. Note that this class must be +// instantiated in the main Qt thread, since an OpenGL context must be created +// in it. The context can then be made current in any other thread. +class OpenGLContextManager : public QObject { + public: + OpenGLContextManager(int opengl_major_version = 2, + int opengl_minor_version = 1); + + // Make the OpenGL context available by moving it from the thread where it was + // created to the current thread and making it current. + bool MakeCurrent(); + + private: + QOffscreenSurface surface_; + QOpenGLContext context_; + QThread* parent_thread_; + QThread* current_thread_; + QAction* make_current_action_; +}; + +// Run and wait for the thread, that uses the OpenGLContextManager, e.g.: +// +// class TestThread : public Thread { +// private: +// void Run() { opengl_context_.MakeCurrent(); } +// OpenGLContextManager opengl_context_; +// }; +// QApplication app(argc, argv); +// TestThread thread; +// RunThreadWithOpenGLContext(&thread); +// +void RunThreadWithOpenGLContext(Thread* thread); + +// Get the OpenGL errors and print them to stderr. +void GLError(const char* file, const int line); + +#else + +// Dummy implementation when GUI support is disabled +class OpenGLContextManager { + public: + OpenGLContextManager(int opengl_major_version = 2, + int opengl_minor_version = 1) {} + inline bool MakeCurrent() { return false; } +}; + +inline void RunThreadWithOpenGLContext(Thread* thread) {} + +inline void GLError(const char* file, const int line) {} + +#endif + +} // namespace colmap + +#endif // COLMAP_SRC_OPENGL_UTILS_H_ diff --git a/colmap/include/colmap/util/option_manager.h b/colmap/include/colmap/util/option_manager.h new file mode 100644 index 0000000000000000000000000000000000000000..e6a375a9cdfbd5bc6d8225b48999c51b698026d4 --- /dev/null +++ b/colmap/include/colmap/util/option_manager.h @@ -0,0 +1,244 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_OPTION_MANAGER_H_ +#define COLMAP_SRC_UTIL_OPTION_MANAGER_H_ + +#include + +#include + +#include "util/logging.h" + +namespace colmap { + +struct ImageReaderOptions; +struct SiftExtractionOptions; +struct SiftMatchingOptions; +struct ExhaustiveMatchingOptions; +struct SequentialMatchingOptions; +struct VocabTreeMatchingOptions; +struct SpatialMatchingOptions; +struct TransitiveMatchingOptions; +struct ImagePairsMatchingOptions; +struct BundleAdjustmentOptions; +struct IncrementalMapperOptions; +struct RenderOptions; + +namespace mvs { +struct PatchMatchOptions; +struct StereoFusionOptions; +struct PoissonMeshingOptions; +struct DelaunayMeshingOptions; +} // namespace mvs + +class OptionManager { + public: + OptionManager(bool add_project_options = true); + + // Create "optimal" set of options for different reconstruction scenarios. + // Note that the existing options are modified, so if your parameters are + // already low quality, they will be further modified. + void ModifyForIndividualData(); + void ModifyForVideoData(); + void ModifyForInternetData(); + + // Create "optimal" set of options for different quality settings. + // Note that the existing options are modified, so if your parameters are + // already low quality, they will be further degraded. + void ModifyForLowQuality(); + void ModifyForMediumQuality(); + void ModifyForHighQuality(); + void ModifyForExtremeQuality(); + + void AddAllOptions(); + void AddLogOptions(); + void AddRandomOptions(); + void AddDatabaseOptions(); + void AddImageOptions(); + void AddExtractionOptions(); + void AddMatchingOptions(); + void AddExhaustiveMatchingOptions(); + void AddSequentialMatchingOptions(); + void AddVocabTreeMatchingOptions(); + void AddSpatialMatchingOptions(); + void AddTransitiveMatchingOptions(); + void AddImagePairsMatchingOptions(); + void AddBundleAdjustmentOptions(); + void AddMapperOptions(); + void AddPatchMatchStereoOptions(); + void AddStereoFusionOptions(); + void AddPoissonMeshingOptions(); + void AddDelaunayMeshingOptions(); + void AddRenderOptions(); + + template + void AddRequiredOption(const std::string& name, T* option, + const std::string& help_text = ""); + template + void AddDefaultOption(const std::string& name, T* option, + const std::string& help_text = ""); + + void Reset(); + void ResetOptions(const bool reset_paths); + + bool Check(); + + void Parse(const int argc, char** argv); + bool Read(const std::string& path); + bool ReRead(const std::string& path); + void Write(const std::string& path) const; + + std::shared_ptr project_path; + std::shared_ptr database_path; + std::shared_ptr image_path; + + std::shared_ptr image_reader; + std::shared_ptr sift_extraction; + + std::shared_ptr sift_matching; + std::shared_ptr exhaustive_matching; + std::shared_ptr sequential_matching; + std::shared_ptr vocab_tree_matching; + std::shared_ptr spatial_matching; + std::shared_ptr transitive_matching; + std::shared_ptr image_pairs_matching; + + std::shared_ptr bundle_adjustment; + std::shared_ptr mapper; + + std::shared_ptr patch_match_stereo; + std::shared_ptr stereo_fusion; + std::shared_ptr poisson_meshing; + std::shared_ptr delaunay_meshing; + + std::shared_ptr render; + + private: + template + void AddAndRegisterRequiredOption(const std::string& name, T* option, + const std::string& help_text = ""); + template + void AddAndRegisterDefaultOption(const std::string& name, T* option, + const std::string& help_text = ""); + + template + void RegisterOption(const std::string& name, const T* option); + + std::shared_ptr desc_; + + std::vector> options_bool_; + std::vector> options_int_; + std::vector> options_double_; + std::vector> options_string_; + + bool added_log_options_; + bool added_random_options_; + bool added_database_options_; + bool added_image_options_; + bool added_extraction_options_; + bool added_match_options_; + bool added_exhaustive_match_options_; + bool added_sequential_match_options_; + bool added_vocab_tree_match_options_; + bool added_spatial_match_options_; + bool added_transitive_match_options_; + bool added_image_pairs_match_options_; + bool added_ba_options_; + bool added_mapper_options_; + bool added_patch_match_stereo_options_; + bool added_stereo_fusion_options_; + bool added_poisson_meshing_options_; + bool added_delaunay_meshing_options_; + bool added_render_options_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +void OptionManager::AddRequiredOption(const std::string& name, T* option, + const std::string& help_text) { + desc_->add_options()(name.c_str(), + boost::program_options::value(option)->required(), + help_text.c_str()); +} + +template +void OptionManager::AddDefaultOption(const std::string& name, T* option, + const std::string& help_text) { + desc_->add_options()( + name.c_str(), + boost::program_options::value(option)->default_value(*option), + help_text.c_str()); +} + +template +void OptionManager::AddAndRegisterRequiredOption(const std::string& name, + T* option, + const std::string& help_text) { + desc_->add_options()(name.c_str(), + boost::program_options::value(option)->required(), + help_text.c_str()); + RegisterOption(name, option); +} + +template +void OptionManager::AddAndRegisterDefaultOption(const std::string& name, + T* option, + const std::string& help_text) { + desc_->add_options()( + name.c_str(), + boost::program_options::value(option)->default_value(*option), + help_text.c_str()); + RegisterOption(name, option); +} + +template +void OptionManager::RegisterOption(const std::string& name, const T* option) { + if (std::is_same::value) { + options_bool_.emplace_back(name, reinterpret_cast(option)); + } else if (std::is_same::value) { + options_int_.emplace_back(name, reinterpret_cast(option)); + } else if (std::is_same::value) { + options_double_.emplace_back(name, reinterpret_cast(option)); + } else if (std::is_same::value) { + options_string_.emplace_back(name, + reinterpret_cast(option)); + } else { + LOG(FATAL) << "Unsupported option type"; + } +} + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_OPTION_MANAGER_H_ diff --git a/colmap/include/colmap/util/ply.h b/colmap/include/colmap/util/ply.h new file mode 100644 index 0000000000000000000000000000000000000000..3468bfb95ecb34b2370fe2bcc4b5f9fd0963ff92 --- /dev/null +++ b/colmap/include/colmap/util/ply.h @@ -0,0 +1,101 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_PLY_H_ +#define COLMAP_SRC_UTIL_PLY_H_ + +#include +#include + +#include "types.h" + +namespace colmap { + +struct PlyPoint { + float x = 0.0f; + float y = 0.0f; + float z = 0.0f; + float nx = 0.0f; + float ny = 0.0f; + float nz = 0.0f; + uint8_t r = 0; + uint8_t g = 0; + uint8_t b = 0; +}; + +struct PlyMeshVertex { + PlyMeshVertex() : x(0), y(0), z(0) {} + PlyMeshVertex(const float x, const float y, const float z) + : x(x), y(y), z(z) {} + + float x = 0.0f; + float y = 0.0f; + float z = 0.0f; +}; + +struct PlyMeshFace { + PlyMeshFace() : vertex_idx1(0), vertex_idx2(0), vertex_idx3(0) {} + PlyMeshFace(const size_t vertex_idx1, const size_t vertex_idx2, + const size_t vertex_idx3) + : vertex_idx1(vertex_idx1), + vertex_idx2(vertex_idx2), + vertex_idx3(vertex_idx3) {} + + size_t vertex_idx1 = 0; + size_t vertex_idx2 = 0; + size_t vertex_idx3 = 0; +}; + +struct PlyMesh { + std::vector vertices; + std::vector faces; +}; + +// Read PLY point cloud from text or binary file. +std::vector ReadPly(const std::string& path); + +// Write PLY point cloud to text or binary file. +void WriteTextPlyPoints(const std::string& path, + const std::vector& points, + const bool write_normal = true, + const bool write_rgb = true); +void WriteBinaryPlyPoints(const std::string& path, + const std::vector& points, + const bool write_normal = true, + const bool write_rgb = true); + +// Write PLY mesh to text or binary file. +void WriteTextPlyMesh(const std::string& path, const PlyMesh& mesh); +void WriteBinaryPlyMesh(const std::string& path, const PlyMesh& mesh); + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_PLY_H_ diff --git a/colmap/include/colmap/util/random.h b/colmap/include/colmap/util/random.h new file mode 100644 index 0000000000000000000000000000000000000000..3dfe0b43cb922bba706b62c266d217c1ebaee637 --- /dev/null +++ b/colmap/include/colmap/util/random.h @@ -0,0 +1,132 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_RANDOM_H_ +#define COLMAP_SRC_UTIL_RANDOM_H_ + +#include +#include +#include +#include + +#include "util/logging.h" + +namespace colmap { + +extern thread_local std::unique_ptr PRNG; + +extern int kDefaultPRNGSeed; + +// Initialize the PRNG with the given seed. +// +// @param seed The seed for the PRNG. If the seed is -1, the current time +// is used as the seed. +void SetPRNGSeed(unsigned seed = kDefaultPRNGSeed); + +// Generate uniformly distributed random integer number. +// +// This implementation is unbiased and thread-safe in contrast to `rand()`. +template +T RandomInteger(const T min, const T max); + +// Generate uniformly distributed random real number. +// +// This implementation is unbiased and thread-safe in contrast to `rand()`. +template +T RandomReal(const T min, const T max); + +// Generate Gaussian distributed random real number. +// +// This implementation is unbiased and thread-safe in contrast to `rand()`. +template +T RandomGaussian(const T mean, const T stddev); + +// Fisher-Yates shuffling. +// +// Note that the vector may not contain more values than UINT32_MAX. This +// restriction comes from the fact that the 32-bit version of the +// Mersenne Twister PRNG is significantly faster. +// +// @param elems Vector of elements to shuffle. +// @param num_to_shuffle Optional parameter, specifying the number of first +// N elements in the vector to shuffle. +template +void Shuffle(const uint32_t num_to_shuffle, std::vector* elems); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +T RandomInteger(const T min, const T max) { + if (PRNG == nullptr) { + SetPRNGSeed(); + } + + std::uniform_int_distribution distribution(min, max); + + return distribution(*PRNG); +} + +template +T RandomReal(const T min, const T max) { + if (PRNG == nullptr) { + SetPRNGSeed(); + } + + std::uniform_real_distribution distribution(min, max); + + return distribution(*PRNG); +} + +template +T RandomGaussian(const T mean, const T stddev) { + if (PRNG == nullptr) { + SetPRNGSeed(); + } + + std::normal_distribution distribution(mean, stddev); + return distribution(*PRNG); +} + +template +void Shuffle(const uint32_t num_to_shuffle, std::vector* elems) { + CHECK_LE(num_to_shuffle, elems->size()); + const uint32_t last_idx = static_cast(elems->size() - 1); + for (uint32_t i = 0; i < num_to_shuffle; ++i) { + const auto j = RandomInteger(i, last_idx); + std::swap((*elems)[i], (*elems)[j]); + } +} + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_RANDOM_H_ diff --git a/colmap/include/colmap/util/sqlite3_utils.h b/colmap/include/colmap/util/sqlite3_utils.h new file mode 100644 index 0000000000000000000000000000000000000000..ba700ad38073758340c491a36824e6af74ff3801 --- /dev/null +++ b/colmap/include/colmap/util/sqlite3_utils.h @@ -0,0 +1,73 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_SQLITE3_UTILS_ +#define COLMAP_SRC_UTIL_SQLITE3_UTILS_ + +#include +#include +#include + +#include + +namespace colmap { + +inline int SQLite3CallHelper(const int result_code, const std::string& filename, + const int line_number) { + switch (result_code) { + case SQLITE_OK: + case SQLITE_ROW: + case SQLITE_DONE: + return result_code; + default: + fprintf(stderr, "SQLite error [%s, line %i]: %s\n", filename.c_str(), + line_number, sqlite3_errstr(result_code)); + exit(EXIT_FAILURE); + } +} + +#define SQLITE3_CALL(func) SQLite3CallHelper(func, __FILE__, __LINE__) + +#define SQLITE3_EXEC(database, sql, callback) \ + { \ + char* err_msg = nullptr; \ + const int result_code = \ + sqlite3_exec(database, sql, callback, nullptr, &err_msg); \ + if (result_code != SQLITE_OK) { \ + fprintf(stderr, "SQLite error [%s, line %i]: %s\n", __FILE__, __LINE__, \ + err_msg); \ + sqlite3_free(err_msg); \ + } \ + } + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_SQLITE3_UTILS_ diff --git a/colmap/include/colmap/util/string.h b/colmap/include/colmap/util/string.h new file mode 100644 index 0000000000000000000000000000000000000000..ebe98c68c22f404ac58c6d26afd5ef336635d157 --- /dev/null +++ b/colmap/include/colmap/util/string.h @@ -0,0 +1,73 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_STRING_H_ +#define COLMAP_SRC_UTIL_STRING_H_ + +#include +#include + +namespace colmap { + +// Format string by replacing embedded format specifiers with their respective +// values, see `printf` for more details. This is a modified implementation +// of Google's BSD-licensed StringPrintf function. +std::string StringPrintf(const char* format, ...); + +// Replace all occurrences of `old_str` with `new_str` in the given string. +std::string StringReplace(const std::string& str, const std::string& old_str, + const std::string& new_str); + +// Get substring of string after search key +std::string StringGetAfter(const std::string& str, const std::string& key); + +// Split string into list of words using the given delimiters. +std::vector StringSplit(const std::string& str, + const std::string& delim); + +// Check whether a string starts with a certain prefix. +bool StringStartsWith(const std::string& str, const std::string& prefix); + +// Remove whitespace from string on both, left, or right sides. +void StringTrim(std::string* str); +void StringLeftTrim(std::string* str); +void StringRightTrim(std::string* str); + +// Convert string to lower/upper case. +void StringToLower(std::string* str); +void StringToUpper(std::string* str); + +// Check whether the sub-string is contained in the given string. +bool StringContains(const std::string& str, const std::string& sub_str); + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_STRING_H_ diff --git a/colmap/include/colmap/util/testing.h b/colmap/include/colmap/util/testing.h new file mode 100644 index 0000000000000000000000000000000000000000..032df3b873794544376502210e4927cb9902f5e5 --- /dev/null +++ b/colmap/include/colmap/util/testing.h @@ -0,0 +1,47 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_TESTING_H_ +#define COLMAP_SRC_UTIL_TESTING_H_ + +#include + +#define BOOST_TEST_MAIN + +#ifndef TEST_NAME +#error "TEST_NAME not defined" +#endif + +#define BOOST_TEST_MODULE TEST_NAME + +#include + +#endif // COLMAP_SRC_UTIL_TESTING_H_ diff --git a/colmap/include/colmap/util/threading.h b/colmap/include/colmap/util/threading.h new file mode 100644 index 0000000000000000000000000000000000000000..1f0c969d49cb45404a1b3c9e0bfb3af93738739c --- /dev/null +++ b/colmap/include/colmap/util/threading.h @@ -0,0 +1,421 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_THREADING_ +#define COLMAP_SRC_UTIL_THREADING_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "util/timer.h" + +namespace colmap { + +#ifdef __clang__ +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wkeyword-macro" +#endif + +#ifdef __clang__ +#pragma clang diagnostic pop // -Wkeyword-macro +#endif + +// Helper class to create single threads with simple controls and timing, e.g.: +// +// class MyThread : public Thread { +// enum { +// PROCESSED_CALLBACK, +// }; +// +// MyThread() { RegisterCallback(PROCESSED_CALLBACK); } +// void Run() { +// // Some setup routine... note that this optional. +// if (setup_valid) { +// SignalValidSetup(); +// } else { +// SignalInvalidSetup(); +// } +// +// // Some pre-processing... +// for (const auto& item : items) { +// BlockIfPaused(); +// if (IsStopped()) { +// // Tear down... +// break; +// } +// // Process item... +// Callback(PROCESSED_CALLBACK); +// } +// } +// }; +// +// MyThread thread; +// thread.AddCallback(MyThread::PROCESSED_CALLBACK, []() { +// std::cout << "Processed item"; }) +// thread.AddCallback(MyThread::STARTED_CALLBACK, []() { +// std::cout << "Start"; }) +// thread.AddCallback(MyThread::FINISHED_CALLBACK, []() { +// std::cout << "Finished"; }) +// thread.Start(); +// // thread.CheckValidSetup(); +// // Pause, resume, stop, ... +// thread.Wait(); +// thread.Timer().PrintElapsedSeconds(); +// +class Thread { + public: + enum { + STARTED_CALLBACK = INT_MIN, + FINISHED_CALLBACK, + }; + + Thread(); + virtual ~Thread() = default; + + // Control the state of the thread. + virtual void Start(); + virtual void Stop(); + virtual void Pause(); + virtual void Resume(); + virtual void Wait(); + + // Check the state of the thread. + bool IsStarted(); + bool IsStopped(); + bool IsPaused(); + bool IsRunning(); + bool IsFinished(); + + // To be called from inside the main run function. This blocks the main + // caller, if the thread is paused, until the thread is resumed. + void BlockIfPaused(); + + // To be called from outside. This blocks the caller until the thread is + // setup, i.e. it signaled that its setup was valid or not. If it never gives + // this signal, this call will block the caller infinitely. Check whether + // setup is valid. Note that the result is only meaningful if the thread gives + // a setup signal. + bool CheckValidSetup(); + + // Set callbacks that can be triggered within the main run function. + void AddCallback(const int id, const std::function& func); + + // Get timing information of the thread, properly accounting for pause times. + const Timer& GetTimer() const; + + protected: + // This is the main run function to be implemented by the child class. If you + // are looping over data and want to support the pause operation, call + // `BlockIfPaused` at appropriate places in the loop. To support the stop + // operation, check the `IsStopped` state and early return from this method. + virtual void Run() = 0; + + // Register a new callback. Note that only registered callbacks can be + // set/reset and called from within the thread. Hence, this method should be + // called from the derived thread constructor. + void RegisterCallback(const int id); + + // Call back to the function with the specified name, if it exists. + void Callback(const int id) const; + + // Get the unique identifier of the current thread. + std::thread::id GetThreadId() const; + + // Signal that the thread is setup. Only call this function once. + void SignalValidSetup(); + void SignalInvalidSetup(); + + private: + // Wrapper around the main run function to set the finished flag. + void RunFunc(); + + std::thread thread_; + std::mutex mutex_; + std::condition_variable pause_condition_; + std::condition_variable setup_condition_; + + Timer timer_; + + bool started_; + bool stopped_; + bool paused_; + bool pausing_; + bool finished_; + bool setup_; + bool setup_valid_; + + std::unordered_map>> callbacks_; +}; + +// A thread pool class to submit generic tasks (functors) to a pool of workers: +// +// ThreadPool thread_pool; +// thread_pool.AddTask([]() { /* Do some work */ }); +// auto future = thread_pool.AddTask([]() { /* Do some work */ return 1; }); +// const auto result = future.get(); +// for (int i = 0; i < 10; ++i) { +// thread_pool.AddTask([](const int i) { /* Do some work */ }); +// } +// thread_pool.Wait(); +// +class ThreadPool { + public: + static const int kMaxNumThreads = -1; + + explicit ThreadPool(const int num_threads = kMaxNumThreads); + ~ThreadPool(); + + inline size_t NumThreads() const; + + // Add new task to the thread pool. + template + auto AddTask(func_t&& f, args_t&&... args) + -> std::future::type>; + + // Stop the execution of all workers. + void Stop(); + + // Wait until tasks are finished. + void Wait(); + + // Get the unique identifier of the current thread. + std::thread::id GetThreadId() const; + + // Get the index of the current thread. In a thread pool of size N, + // the thread index defines the 0-based index of the thread in the pool. + // In other words, there are the thread indices 0, ..., N-1. + int GetThreadIndex(); + + private: + void WorkerFunc(const int index); + + std::vector workers_; + std::queue> tasks_; + + std::mutex mutex_; + std::condition_variable task_condition_; + std::condition_variable finished_condition_; + + bool stopped_; + int num_active_workers_; + + std::unordered_map thread_id_to_index_; +}; + +// A job queue class for the producer-consumer paradigm. +// +// JobQueue job_queue; +// +// std::thread producer_thread([&job_queue]() { +// for (int i = 0; i < 10; ++i) { +// job_queue.Push(i); +// } +// }); +// +// std::thread consumer_thread([&job_queue]() { +// for (int i = 0; i < 10; ++i) { +// const auto job = job_queue.Pop(); +// if (job.IsValid()) { /* Do some work */ } +// else { break; } +// } +// }); +// +// producer_thread.join(); +// consumer_thread.join(); +// +template +class JobQueue { + public: + class Job { + public: + Job() : valid_(false) {} + explicit Job(T data) : data_(std::move(data)), valid_(true) {} + + // Check whether the data is valid. + bool IsValid() const { return valid_; } + + // Get reference to the data. + T& Data() { return data_; } + const T& Data() const { return data_; } + + private: + T data_; + bool valid_; + }; + + JobQueue(); + explicit JobQueue(const size_t max_num_jobs); + ~JobQueue(); + + // The number of pushed and not popped jobs in the queue. + size_t Size(); + + // Push a new job to the queue. Waits if the number of jobs is exceeded. + bool Push(T data); + + // Pop a job from the queue. Waits if there is no job in the queue. + Job Pop(); + + // Wait for all jobs to be popped and then stop the queue. + void Wait(); + + // Stop the queue and return from all push/pop calls with false. + void Stop(); + + // Clear all pushed and not popped jobs from the queue. + void Clear(); + + private: + size_t max_num_jobs_; + std::atomic stop_; + std::queue jobs_; + std::mutex mutex_; + std::condition_variable push_condition_; + std::condition_variable pop_condition_; + std::condition_variable empty_condition_; +}; + +// Return the number of logical CPU cores if num_threads <= 0, +// otherwise return the input value of num_threads. +int GetEffectiveNumThreads(const int num_threads); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +size_t ThreadPool::NumThreads() const { return workers_.size(); } + +template +auto ThreadPool::AddTask(func_t&& f, args_t&&... args) + -> std::future::type> { + typedef typename std::result_of::type return_t; + + auto task = std::make_shared>( + std::bind(std::forward(f), std::forward(args)...)); + + std::future result = task->get_future(); + + { + std::unique_lock lock(mutex_); + if (stopped_) { + throw std::runtime_error("Cannot add task to stopped thread pool."); + } + tasks_.emplace([task]() { (*task)(); }); + } + + task_condition_.notify_one(); + + return result; +} + +template +JobQueue::JobQueue() : JobQueue(std::numeric_limits::max()) {} + +template +JobQueue::JobQueue(const size_t max_num_jobs) + : max_num_jobs_(max_num_jobs), stop_(false) {} + +template +JobQueue::~JobQueue() { + Stop(); +} + +template +size_t JobQueue::Size() { + std::unique_lock lock(mutex_); + return jobs_.size(); +} + +template +bool JobQueue::Push(T data) { + std::unique_lock lock(mutex_); + while (jobs_.size() >= max_num_jobs_ && !stop_) { + pop_condition_.wait(lock); + } + if (stop_) { + return false; + } else { + jobs_.push(std::move(data)); + push_condition_.notify_one(); + return true; + } +} + +template +typename JobQueue::Job JobQueue::Pop() { + std::unique_lock lock(mutex_); + while (jobs_.empty() && !stop_) { + push_condition_.wait(lock); + } + if (stop_) { + return Job(); + } else { + Job job(std::move(jobs_.front())); + jobs_.pop(); + pop_condition_.notify_one(); + if (jobs_.empty()) { + empty_condition_.notify_all(); + } + return job; + } +} + +template +void JobQueue::Wait() { + std::unique_lock lock(mutex_); + while (!jobs_.empty()) { + empty_condition_.wait(lock); + } +} + +template +void JobQueue::Stop() { + stop_ = true; + push_condition_.notify_all(); + pop_condition_.notify_all(); +} + +template +void JobQueue::Clear() { + std::unique_lock lock(mutex_); + std::queue empty_jobs; + std::swap(jobs_, empty_jobs); +} + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_THREADING_ diff --git a/colmap/include/colmap/util/timer.h b/colmap/include/colmap/util/timer.h new file mode 100644 index 0000000000000000000000000000000000000000..1d2c73eb9636009a347a4f2b438fac92d003b375 --- /dev/null +++ b/colmap/include/colmap/util/timer.h @@ -0,0 +1,67 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_TIMER_H_ +#define COLMAP_SRC_UTIL_TIMER_H_ + +#include + +namespace colmap { + +class Timer { + public: + Timer(); + + void Start(); + void Restart(); + void Pause(); + void Resume(); + void Reset(); + + double ElapsedMicroSeconds() const; + double ElapsedSeconds() const; + double ElapsedMinutes() const; + double ElapsedHours() const; + + void PrintSeconds() const; + void PrintMinutes() const; + void PrintHours() const; + + private: + bool started_; + bool paused_; + std::chrono::high_resolution_clock::time_point start_time_; + std::chrono::high_resolution_clock::time_point pause_time_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_TIMER_H_ diff --git a/colmap/include/colmap/util/types.h b/colmap/include/colmap/util/types.h new file mode 100644 index 0000000000000000000000000000000000000000..6433c64d5445b3d8aefc558d8d1bdf979c8e9c91 --- /dev/null +++ b/colmap/include/colmap/util/types.h @@ -0,0 +1,122 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_TYPES_H_ +#define COLMAP_SRC_UTIL_TYPES_H_ + +#include "util/alignment.h" + +#ifdef _MSC_VER +#if _MSC_VER >= 1600 +#include +#else +typedef __int8 int8_t; +typedef __int16 int16_t; +typedef __int32 int32_t; +typedef __int64 int64_t; +typedef unsigned __int8 uint8_t; +typedef unsigned __int16 uint16_t; +typedef unsigned __int32 uint32_t; +typedef unsigned __int64 uint64_t; +#endif +#elif __GNUC__ >= 3 +#include +#endif + +// Define non-copyable or non-movable classes. +#define NON_COPYABLE(class_name) \ + class_name(class_name const&) = delete; \ + void operator=(class_name const& obj) = delete; +#define NON_MOVABLE(class_name) class_name(class_name&&) = delete; + +#include + +namespace Eigen { + +typedef Eigen::Matrix Matrix3x4f; +typedef Eigen::Matrix Matrix3x4d; +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Vector3ub; +typedef Eigen::Matrix Vector4ub; +typedef Eigen::Matrix Vector6d; + +} // namespace Eigen + +namespace colmap { + +//////////////////////////////////////////////////////////////////////////////// +// Index types, determines the maximum number of objects. +//////////////////////////////////////////////////////////////////////////////// + +// Unique identifier for cameras. +typedef uint32_t camera_t; + +// Unique identifier for images. +typedef uint32_t image_t; + +// Each image pair gets a unique ID, see `Database::ImagePairToPairId`. +typedef uint64_t image_pair_t; + +// Index per image, i.e. determines maximum number of 2D points per image. +typedef uint32_t point2D_t; + +// Unique identifier per added 3D point. Since we add many 3D points, +// delete them, and possibly re-add them again, the maximum number of allowed +// unique indices should be large. +typedef uint64_t point3D_t; + +// Values for invalid identifiers or indices. +const camera_t kInvalidCameraId = std::numeric_limits::max(); +const image_t kInvalidImageId = std::numeric_limits::max(); +const image_pair_t kInvalidImagePairId = + std::numeric_limits::max(); +const point2D_t kInvalidPoint2DIdx = std::numeric_limits::max(); +const point3D_t kInvalidPoint3DId = std::numeric_limits::max(); + +} // namespace colmap + +// This file provides specializations of the templated hash function for +// custom types. These are used for comparison in unordered sets/maps. +namespace std { + +// Hash function specialization for uint32_t pairs, e.g., image_t or camera_t. +template <> +struct hash> { + std::size_t operator()(const std::pair& p) const { + const uint64_t s = (static_cast(p.first) << 32) + + static_cast(p.second); + return std::hash()(s); + } +}; + +} // namespace std + +#endif // COLMAP_SRC_UTIL_TYPES_H_ diff --git a/colmap/include/colmap/util/version.h b/colmap/include/colmap/util/version.h new file mode 100644 index 0000000000000000000000000000000000000000..f759b65bbd9d69b5c1734f72ea8b3871a6309e14 --- /dev/null +++ b/colmap/include/colmap/util/version.h @@ -0,0 +1,50 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UTIL_VERSION_H_ +#define COLMAP_SRC_UTIL_VERSION_H_ + +#include "misc.h" + +namespace colmap { + +const static std::string COLMAP_VERSION = "3.8"; +const static int COLMAP_VERSION_NUMBER = 3800; +const static std::string COLMAP_COMMIT_ID = "43de802"; +const static std::string COLMAP_COMMIT_DATE = "2023-01-31"; + +std::string GetVersionInfo(); + +std::string GetBuildInfo(); + +} // namespace colmap + +#endif // COLMAP_SRC_UTIL_VERSION_H_ diff --git a/colmap/lib/libcolmap.a b/colmap/lib/libcolmap.a new file mode 100644 index 0000000000000000000000000000000000000000..04a965fd87d2564d5690ec24a0406bb64afbcd0b --- /dev/null +++ b/colmap/lib/libcolmap.a @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c093f7af144426bec0b746f0533da1b0ccdbb9dc83556135ba3a145e43805b36 +size 17871322 diff --git a/colmap/lib/libcolmap_cuda.a b/colmap/lib/libcolmap_cuda.a new file mode 100644 index 0000000000000000000000000000000000000000..f90dafe58b63db894731346cb40c5b4be3363c22 --- /dev/null +++ b/colmap/lib/libcolmap_cuda.a @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8d47fa961c1fb09b136334e9b516d782fc47cca52a3e8b443e78de463e4cd074 +size 14543686 diff --git a/colmap/lib/liblsd.a b/colmap/lib/liblsd.a new file mode 100644 index 0000000000000000000000000000000000000000..399a3f9bc71acac1fd508fb54cea6c6bc2fb6d44 Binary files /dev/null and b/colmap/lib/liblsd.a differ diff --git a/colmap/lib/libpba.a b/colmap/lib/libpba.a new file mode 100644 index 0000000000000000000000000000000000000000..bcb922877b07985cba27fdc97726ed6359056600 --- /dev/null +++ b/colmap/lib/libpba.a @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:88e638adc88c87696a6d3acc4a163971314e8a69db4c47b38be003f62a389a83 +size 1702890 diff --git a/colmap/lib/libpoisson_recon.a b/colmap/lib/libpoisson_recon.a new file mode 100644 index 0000000000000000000000000000000000000000..0edeaeb66fc6c42afd4bea3b22fb9ecc1162caad --- /dev/null +++ b/colmap/lib/libpoisson_recon.a @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9d634dbf6becaff49f27baeb4238ccc421503802532947ad224e9979a3b41057 +size 4112726 diff --git a/colmap/lib/libsift_gpu.a b/colmap/lib/libsift_gpu.a new file mode 100644 index 0000000000000000000000000000000000000000..c067f90508474e80d44fc7c01a98db60f108ff1d --- /dev/null +++ b/colmap/lib/libsift_gpu.a @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ff409b7022db0e398e852639661c831a5b0f3abf8aa88d21a315335da5f1fff1 +size 1327194 diff --git a/colmap/lib/libvlfeat.a b/colmap/lib/libvlfeat.a new file mode 100644 index 0000000000000000000000000000000000000000..c1e6b0a1a744b840288e9f9525bacd0fba7eaff7 Binary files /dev/null and b/colmap/lib/libvlfeat.a differ diff --git a/colmap/share/COLMAPConfig.cmake b/colmap/share/COLMAPConfig.cmake new file mode 100644 index 0000000000000000000000000000000000000000..891012f5fc3b9b1b89f3f391e5e69f36a21300db --- /dev/null +++ b/colmap/share/COLMAPConfig.cmake @@ -0,0 +1,255 @@ +# Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +# 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +# +# Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +# Find package module for COLMAP library. +# +# The following variables are set by this module: +# +# COLMAP_FOUND: TRUE if COLMAP is found. +# COLMAP_VERSION: COLMAP version. +# COLMAP_INCLUDE_DIRS: Include directories for COLMAP. +# COLMAP_LINK_DIRS: Link directories for COLMAP. +# COLMAP_LIBRARIES: Libraries required to link COLMAP. +# COLMAP_CUDA_ENABLED: Whether COLMAP was compiled with CUDA support. +# COLMAP_GUI_ENABLED: Whether COLMAP was compiled with the graphical UI. +# COLMAP_CGAL_ENABLED: Whether COLMAP was compiled with CGAL dependencies. + +get_filename_component(COLMAP_INSTALL_PREFIX ${CMAKE_CURRENT_LIST_FILE} PATH) +set(COLMAP_INSTALL_PREFIX "${COLMAP_INSTALL_PREFIX}/../..") + +set(COLMAP_FOUND FALSE) + +# Set hints for finding dependency packages. + +set(EIGEN3_INCLUDE_DIR_HINTS ) + +set(FLANN_INCLUDE_DIR_HINTS ) +set(FLANN_LIBRARY_DIR_HINTS ) + +set(LZ4_INCLUDE_DIR_HINTS ) +set(LZ4_LIBRARY_DIR_HINTS ) + +set(FREEIMAGE_INCLUDE_DIR_HINTS ) +set(FREEIMAGE_LIBRARY_DIR_HINTS ) + +set(METIS_INCLUDE_DIR_HINTS ) +set(METIS_LIBRARY_DIR_HINTS ) + +set(GLEW_INCLUDE_DIR_HINTS ) +set(GLEW_LIBRARY_DIR_HINTS ) + +set(GLOG_INCLUDE_DIR_HINTS ) +set(GLOG_LIBRARY_DIR_HINTS ) + +set(SQLite3_INCLUDE_DIR_HINTS ) +set(SQLite3_LIBRARY_DIR_HINTS ) + +# Find dependency packages. + +set(TEMP_CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH}) +set(CMAKE_MODULE_PATH ${COLMAP_INSTALL_PREFIX}/share/colmap/cmake) + +if(COLMAP_FIND_QUIETLY) + find_package(Ceres QUIET) + + find_package(Boost COMPONENTS + program_options + filesystem + system + unit_test_framework + QUIET) + + find_package(Eigen3 QUIET) + + find_package(FLANN QUIET) + find_package(LZ4 QUIET) + + find_package(FreeImage QUIET) + + find_package(Metis QUIET) + + find_package(Glog QUIET) + + find_package(SQLite3 QUIET) + + find_package(OpenGL QUIET) + find_package(Glew QUIET) +else() + find_package(Ceres REQUIRED) + + find_package(Boost COMPONENTS + program_options + filesystem + system + unit_test_framework + REQUIRED) + + find_package(Eigen3 REQUIRED) + + find_package(FLANN REQUIRED) + find_package(LZ4 REQUIRED) + + find_package(FreeImage REQUIRED) + + find_package(Metis REQUIRED) + + find_package(Glog REQUIRED) + + find_package(SQLite3 REQUIRED) + + find_package(OpenGL REQUIRED) + find_package(Glew REQUIRED) +endif() + +# Set the exported variables. + +set(COLMAP_FOUND TRUE) + +set(COLMAP_VERSION 3.8) + +set(COLMAP_OPENMP_ENABLED ON) + +set(COLMAP_CUDA_ENABLED ON) +set(COLMAP_CUDA_MIN_VERSION 7.0) + +set(COLMAP_GUI_ENABLED ON) + +set(COLMAP_CGAL_ENABLED ON) + +set(COLMAP_INCLUDE_DIRS + ${COLMAP_INSTALL_PREFIX}/include/ + ${COLMAP_INSTALL_PREFIX}/include/colmap + ${COLMAP_INSTALL_PREFIX}/include/colmap/lib + ${Boost_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${GLOG_INCLUDE_DIRS} + ${FLANN_INCLUDE_DIRS} + ${LZ4_INCLUDE_DIRS} + ${FREEIMAGE_INCLUDE_DIRS} + ${METIS_INCLUDE_DIRS} + ${CERES_INCLUDE_DIRS} + ${GLEW_INCLUDE_DIRS} + ${SQLite3_INCLUDE_DIRS} +) + +set(COLMAP_LINK_DIRS + ${COLMAP_INSTALL_PREFIX}/lib/colmap + ${Boost_LIBRARY_DIRS} +) + +set(COLMAP_INTERNAL_LIBRARIES + lsd + pba + poisson_recon + sqlite3 + sift_gpu + vlfeat +) + +set(COLMAP_EXTERNAL_LIBRARIES + ${CMAKE_DL_LIBS} + ${GLOG_LIBRARIES} + ${FLANN_LIBRARIES} + ${LZ4_LIBRARIES} + ${FREEIMAGE_LIBRARIES} + ${METIS_LIBRARIES} + ${CERES_LIBRARIES} + ${OPENGL_LIBRARIES} + ${GLEW_LIBRARIES} + ${SQLite3_LIBRARIES} +) + +if(UNIX) + list(APPEND COLMAP_EXTERNAL_LIBRARIES + ${Boost_FILESYSTEM_LIBRARY} + ${Boost_PROGRAM_OPTIONS_LIBRARY} + ${Boost_SYSTEM_LIBRARY} + pthread) +endif() + +if(COLMAP_OPENMP_ENABLED) + find_package(OpenMP QUIET) + add_definitions("-DOPENMP_ENABLED") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + list(APPEND COLMAP_EXTERNAL_LIBRARIES ${OpenMP_libomp_LIBRARY}) +endif() + +if(COLMAP_CUDA_ENABLED) + find_package(CUDA ${COLMAP_CUDA_MIN_VERSION} QUIET) + list(APPEND COLMAP_EXTERNAL_LIBRARIES ${CUDA_LIBRARIES}) + list(APPEND COLMAP_INTERNAL_LIBRARIES colmap_cuda) +endif() + +if(COLMAP_GUI_ENABLED) + find_package(Qt5 5.4 REQUIRED COMPONENTS Core OpenGL Widgets) + list(APPEND COLMAP_EXTERNAL_LIBRARIES + ${Qt5Core_LIBRARIES} + ${Qt5OpenGL_LIBRARIES} + ${Qt5Widgets_LIBRARIES}) + list(APPEND COLMAP_INCLUDE_DIRS + ${Qt5Core_INCLUDE_DIRS} + ${Qt5OpenGL_INCLUDE_DIRS} + ${Qt5Widgets_INCLUDE_DIRS}) +endif() + +if(COLMAP_CGAL_ENABLED) + find_package(CGAL REQUIRED) + list(APPEND COLMAP_EXTERNAL_LIBRARIES ${CGAL_LIBRARY} ${GMP_LIBRARIES}) +endif() + +set(COLMAP_LIBRARIES + colmap + ${COLMAP_INTERNAL_LIBRARIES} + ${COLMAP_EXTERNAL_LIBRARIES} +) + +# Cleanup of configuration variables. + +set(CMAKE_MODULE_PATH ${TEMP_CMAKE_MODULE_PATH}) + +unset(COLMAP_INSTALL_PREFIX) +unset(EIGEN3_INCLUDE_DIR_HINTS) +unset(FLANN_INCLUDE_DIR_HINTS) +unset(FLANN_LIBRARY_DIR_HINTS) +unset(LZ4_INCLUDE_DIR_HINTS) +unset(LZ4_LIBRARY_DIR_HINTS) +unset(FREEIMAGE_INCLUDE_DIR_HINTS) +unset(FREEIMAGE_LIBRARY_DIR_HINTS) +unset(METIS_INCLUDE_DIR_HINTS) +unset(METIS_LIBRARY_DIR_HINTS) +unset(GLEW_INCLUDE_DIR_HINTS) +unset(GLEW_LIBRARY_DIR_HINTS) +unset(GLOG_INCLUDE_DIR_HINTS) +unset(GLOG_LIBRARY_DIR_HINTS) +unset(SQLite3_INCLUDE_DIR_HINTS) +unset(SQLite3_LIBRARY_DIR_HINTS) +unset(QT5_CMAKE_CONFIG_DIR_HINTS) diff --git a/colmap/share/COLMAPConfigVersion.cmake b/colmap/share/COLMAPConfigVersion.cmake new file mode 100644 index 0000000000000000000000000000000000000000..c0d7444ebe5046da58408c9ce4823a31c160e1f9 --- /dev/null +++ b/colmap/share/COLMAPConfigVersion.cmake @@ -0,0 +1,38 @@ +# Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +# 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +# +# Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +set(PACKAGE_VERSION "3.8") + +if("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") + set(PACKAGE_VERSION_COMPATIBLE TRUE) +else() + set(PACKAGE_VERSION_COMPATIBLE FALSE) +endif() diff --git a/colmap/share/cmake/FindEigen3.cmake b/colmap/share/cmake/FindEigen3.cmake new file mode 100644 index 0000000000000000000000000000000000000000..a4a53317ed8c29c3d653af2f67db50a98e622152 --- /dev/null +++ b/colmap/share/cmake/FindEigen3.cmake @@ -0,0 +1,126 @@ +# Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +# 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +# +# Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +# Find package module for Eigen 3 library. +# +# The following variables are set by this module: +# +# EIGEN3_FOUND: TRUE if Eigen is found. +# EIGEN3_VERSION: Eigen library version. +# EIGEN3_INCLUDE_DIRS: Include directories for Eigen. +# +# The following variables control the behavior of this module: +# +# EIGEN3_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for Eigen includes. + +set(EIGEN3_INCLUDE_DIR_HINTS "" CACHE PATH "Eigen include directory") + +unset(EIGEN3_FOUND) +unset(EIGEN3_VERSION) +unset(EIGEN3_INCLUDE_DIRS) + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif() + + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif() + + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif() + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif() + +macro(_EIGEN3_CHECK_VERSION) + file(READ "${EIGEN3_INCLUDE_DIRS}/Eigen/src/Core/util/Macros.h" _EIGEN3_VERSION_HEADER) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _EIGEN3_WORLD_VERSION_MATCH "${_EIGEN3_VERSION_HEADER}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _EIGEN3_MAJOR_VERSION_MATCH "${_EIGEN3_VERSION_HEADER}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _EIGEN3_MINOR_VERSION_MATCH "${_EIGEN3_VERSION_HEADER}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else() + set(EIGEN3_VERSION_OK TRUE) + endif() + + if(NOT EIGEN3_VERSION_OK) + message(STATUS "Eigen version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIRS}, " + "but at least version ${Eigen3_FIND_VERSION} is required.") + endif() +endmacro() + +if(EIGEN3_INCLUDE_DIRS) + _EIGEN3_CHECK_VERSION() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) +else() + find_path(EIGEN3_INCLUDE_DIRS + NAMES + signature_of_eigen3_matrix_library + PATHS + ${EIGEN3_INCLUDE_DIR_HINTS} + /usr/include + /usr/local/include + /opt/include + /opt/local/include + PATH_SUFFIXES + eigen3 + eigen + ) + + if(EIGEN3_INCLUDE_DIRS) + _EIGEN3_CHECK_VERSION() + endif() + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIRS EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIRS) +endif() + +if(EIGEN3_FOUND) + message(STATUS "Found Eigen") + message(STATUS " Includes : ${EIGEN3_INCLUDE_DIRS}") +else() + if(Eigen3_FIND_REQUIRED) + message(FATAL_ERROR "Could not find Eigen") + endif() +endif() diff --git a/colmap/share/cmake/FindFLANN.cmake b/colmap/share/cmake/FindFLANN.cmake new file mode 100644 index 0000000000000000000000000000000000000000..5ec113c7405b5f0f86994bcccbeff3d296b8bd63 --- /dev/null +++ b/colmap/share/cmake/FindFLANN.cmake @@ -0,0 +1,93 @@ +# Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +# 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +# +# Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +# Find package module for FLANN library. +# +# The following variables are set by this module: +# +# FLANN_FOUND: TRUE if FLANN is found. +# FLANN_INCLUDE_DIRS: Include directories for FLANN. +# FLANN_LIBRARIES: Libraries required to link FLANN. +# +# The following variables control the behavior of this module: +# +# FLANN_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for FLANN includes. +# FLANN_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for FLANN libraries. + +set(FLANN_INCLUDE_DIR_HINTS "" CACHE PATH "FLANN include directory") +set(FLANN_LIBRARY_DIR_HINTS "" CACHE PATH "FLANN library directory") + +unset(FLANN_FOUND) +unset(FLANN_INCLUDE_DIRS) +unset(FLANN_LIBRARIES) + +list(APPEND FLANN_CHECK_INCLUDE_DIRS + ${FLANN_INCLUDE_DIR_HINTS} + /usr/include + /usr/local/include + /opt/include + /opt/local/include +) + +list(APPEND FLANN_CHECK_LIBRARY_DIRS + ${FLANN_LIBRARY_DIR_HINTS} + /usr/lib + /usr/local/lib + /opt/lib + /opt/local/lib +) + +find_path(FLANN_INCLUDE_DIRS + NAMES + flann/flann.hpp + PATHS + ${FLANN_CHECK_INCLUDE_DIRS}) +find_library(FLANN_LIBRARIES + NAMES + flann + PATHS + ${FLANN_CHECK_LIBRARY_DIRS}) + +if(FLANN_INCLUDE_DIRS AND FLANN_LIBRARIES) + set(FLANN_FOUND TRUE) +endif() + +if(FLANN_FOUND) + message(STATUS "Found FLANN") + message(STATUS " Includes : ${FLANN_INCLUDE_DIRS}") + message(STATUS " Libraries : ${FLANN_LIBRARIES}") +else() + if(FLANN_FIND_REQUIRED) + message(FATAL_ERROR "Could not find FLANN") + endif() +endif() diff --git a/colmap/share/cmake/FindFreeImage.cmake b/colmap/share/cmake/FindFreeImage.cmake new file mode 100644 index 0000000000000000000000000000000000000000..7939893caa43b457c851f58e213e5cd02cea3dc7 --- /dev/null +++ b/colmap/share/cmake/FindFreeImage.cmake @@ -0,0 +1,93 @@ +# Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +# 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +# +# Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +# Find package module for FreeImage library. +# +# The following variables are set by this module: +# +# FREEIMAGE_FOUND: TRUE if FreeImage is found. +# FREEIMAGE_INCLUDE_DIRS: Include directories for FreeImage. +# FREEIMAGE_LIBRARIES: Libraries required to link FreeImage. +# +# The following variables control the behavior of this module: +# +# FREEIMAGE_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for FreeImage includes. +# FREEIMAGE_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for FreeImage libraries. + +set(FREEIMAGE_INCLUDE_DIR_HINTS "" CACHE PATH "FreeImage include directory") +set(FREEIMAGE_LIBRARY_DIR_HINTS "" CACHE PATH "FreeImage library directory") + +unset(FREEIMAGE_FOUND) +unset(FREEIMAGE_INCLUDE_DIRS) +unset(FREEIMAGE_LIBRARIES) + +list(APPEND FREEIMAGE_CHECK_INCLUDE_DIRS + ${FREEIMAGE_INCLUDE_DIR_HINTS} + /usr/include + /usr/local/include + /opt/include + /opt/local/include +) + +list(APPEND FREEIMAGE_CHECK_LIBRARY_DIRS + ${FREEIMAGE_LIBRARY_DIR_HINTS} + /usr/lib + /usr/local/lib + /opt/lib + /opt/local/lib +) + +find_path(FREEIMAGE_INCLUDE_DIRS + NAMES + FreeImage.h + PATHS + ${FREEIMAGE_CHECK_INCLUDE_DIRS}) +find_library(FREEIMAGE_LIBRARIES + NAMES + freeimage + PATHS + ${FREEIMAGE_CHECK_LIBRARY_DIRS}) + +if(FREEIMAGE_INCLUDE_DIRS AND FREEIMAGE_LIBRARIES) + set(FREEIMAGE_FOUND TRUE) +endif() + +if(FREEIMAGE_FOUND) + message(STATUS "Found FreeImage") + message(STATUS " Includes : ${FREEIMAGE_INCLUDE_DIRS}") + message(STATUS " Libraries : ${FREEIMAGE_LIBRARIES}") +else() + if(FreeImage_FIND_REQUIRED) + message(FATAL_ERROR "Could not find FreeImage") + endif() +endif() diff --git a/colmap/share/cmake/FindGlew.cmake b/colmap/share/cmake/FindGlew.cmake new file mode 100644 index 0000000000000000000000000000000000000000..43800fe2d6e507d178a3ab31ecdeca51bfa0619a --- /dev/null +++ b/colmap/share/cmake/FindGlew.cmake @@ -0,0 +1,90 @@ +# Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +# 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +# +# Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +# Find package module for Glew library. +# +# The following variables are set by this module: +# +# GLEW_FOUND: TRUE if Glew is found. +# GLEW_INCLUDE_DIRS: Include directories for Glew. +# GLEW_LIBRARIES: Libraries required to link Glew. +# +# The following variables control the behavior of this module: +# +# GLEW_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for Glew includes. +# GLEW_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for Glew libraries. + +set(GLEW_INCLUDE_DIR_HINTS "" CACHE PATH "Glew include directory") +set(GLEW_LIBRARY_DIR_HINTS "" CACHE PATH "Glew library directory") + +unset(GLEW_FOUND) +unset(GLEW_INCLUDE_DIRS) +unset(GLEW_LIBRARIES) + +find_path(GLEW_INCLUDE_DIRS + NAMES + GL/glew.h + PATHS + ${GLEW_INCLUDE_DIR_HINTS} + /usr/include + /usr/local/include + /sw/include + /opt/include + /opt/local/include) +find_library(GLEW_LIBRARIES + NAMES + GLEW + Glew + glew + glew32 + PATHS + ${GLEW_LIBRARY_DIR_HINTS} + /usr/lib64 + /usr/lib + /usr/local/lib64 + /usr/local/lib + /sw/lib + /opt/lib + /opt/local/lib) + +if(GLEW_INCLUDE_DIRS AND GLEW_LIBRARIES) + set(GLEW_FOUND TRUE) + message(STATUS "Found Glew") + message(STATUS " Includes : ${GLEW_INCLUDE_DIRS}") + message(STATUS " Libraries : ${GLEW_LIBRARIES}") +else() + set(GLEW_FOUND FALSE) + if(Glew_FIND_REQUIRED) + message(FATAL_ERROR "Could not find Glew") + endif() +endif() diff --git a/colmap/share/cmake/FindGlog.cmake b/colmap/share/cmake/FindGlog.cmake new file mode 100644 index 0000000000000000000000000000000000000000..fb3674cd9ef6f6477521a8fc65ed6a214a7b73c5 --- /dev/null +++ b/colmap/share/cmake/FindGlog.cmake @@ -0,0 +1,108 @@ +# Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +# 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +# +# Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +# Find package module for Glog library. +# +# The following variables are set by this module: +# +# GLOG_FOUND: TRUE if Glog is found. +# GLOG_INCLUDE_DIRS: Include directories for Glog. +# GLOG_LIBRARIES: Libraries required to link Glog. +# +# The following variables control the behavior of this module: +# +# GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for Glog includes. +# GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for Glog libraries. + +set(GLOG_INCLUDE_DIR_HINTS "" CACHE PATH "Glog include directory") +set(GLOG_LIBRARY_DIR_HINTS "" CACHE PATH "Glog library directory") + +unset(GLOG_FOUND) +unset(GLOG_INCLUDE_DIRS) +unset(GLOG_LIBRARIES) + +include(FindPackageHandleStandardArgs) + +list(APPEND GLOG_CHECK_INCLUDE_DIRS + /usr/local/include + /usr/local/homebrew/include + /opt/local/var/macports/software + /opt/local/include + /usr/include) +list(APPEND GLOG_CHECK_PATH_SUFFIXES + glog/include + glog/Include + Glog/include + Glog/Include + src/windows) + +list(APPEND GLOG_CHECK_LIBRARY_DIRS + /usr/local/lib + /usr/local/homebrew/lib + /opt/local/lib + /usr/lib) +list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES + glog/lib + glog/Lib + Glog/lib + Glog/Lib + x64/Release) + +find_path(GLOG_INCLUDE_DIRS + NAMES + glog/logging.h + PATHS + ${GLOG_INCLUDE_DIR_HINTS} + ${GLOG_CHECK_INCLUDE_DIRS} + PATH_SUFFIXES + ${GLOG_CHECK_PATH_SUFFIXES}) +find_library(GLOG_LIBRARIES + NAMES + glog + libglog + PATHS + ${GLOG_LIBRARY_DIR_HINTS} + ${GLOG_CHECK_LIBRARY_DIRS} + PATH_SUFFIXES + ${GLOG_CHECK_LIBRARY_SUFFIXES}) + +if (GLOG_INCLUDE_DIRS AND GLOG_LIBRARIES) + set(GLOG_FOUND TRUE) + message(STATUS "Found Glog") + message(STATUS " Includes : ${GLOG_INCLUDE_DIRS}") + message(STATUS " Libraries : ${GLOG_LIBRARIES}") +else() + if(Glog_FIND_REQUIRED) + message(FATAL_ERROR "Could not find Glog") + endif() +endif() diff --git a/colmap/share/cmake/FindLZ4.cmake b/colmap/share/cmake/FindLZ4.cmake new file mode 100644 index 0000000000000000000000000000000000000000..a83392121c34375983dbe4d5265bf3c42e20dd6b --- /dev/null +++ b/colmap/share/cmake/FindLZ4.cmake @@ -0,0 +1,94 @@ +# Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +# 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +# +# Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +# Find package module for LZ4 library. +# +# The following variables are set by this module: +# +# LZ4_FOUND: TRUE if LZ4 is found. +# LZ4_INCLUDE_DIRS: Include directories for LZ4. +# LZ4_LIBRARIES: Libraries required to link LZ4. +# +# The following variables control the behavior of this module: +# +# LZ4_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for LZ4 includes. +# LZ4_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for LZ4 libraries. + +set(LZ4_INCLUDE_DIR_HINTS "" CACHE PATH "LZ4 include directory") +set(LZ4_LIBRARY_DIR_HINTS "" CACHE PATH "LZ4 library directory") + +unset(LZ4_FOUND) +unset(LZ4_INCLUDE_DIRS) +unset(LZ4_LIBRARIES) + +list(APPEND LZ4_CHECK_INCLUDE_DIRS + ${LZ4_INCLUDE_DIR_HINTS} + /usr/include + /usr/local/include + /opt/include + /opt/local/include +) + +list(APPEND LZ4_CHECK_LIBRARY_DIRS + ${LZ4_LIBRARY_DIR_HINTS} + /usr/lib + /usr/local/lib + /usr/lib/x86_64-linux-gnu + /opt/lib + /opt/local/lib +) + +find_path(LZ4_INCLUDE_DIRS + NAMES + lz4.h + PATHS + ${LZ4_CHECK_INCLUDE_DIRS}) +find_library(LZ4_LIBRARIES + NAMES + lz4 + PATHS + ${LZ4_CHECK_LIBRARY_DIRS}) + +if(LZ4_INCLUDE_DIRS AND LZ4_LIBRARIES) + set(LZ4_FOUND TRUE) +endif() + +if(LZ4_FOUND) + message(STATUS "Found LZ4") + message(STATUS " Includes : ${LZ4_INCLUDE_DIRS}") + message(STATUS " Libraries : ${LZ4_LIBRARIES}") +else() + if(LZ4_FIND_REQUIRED) + message(FATAL_ERROR "Could not find LZ4") + endif() +endif() diff --git a/colmap/share/cmake/FindMetis.cmake b/colmap/share/cmake/FindMetis.cmake new file mode 100644 index 0000000000000000000000000000000000000000..a94b6d3fab63974359f21b7a7ea6e5cd8cfbf916 --- /dev/null +++ b/colmap/share/cmake/FindMetis.cmake @@ -0,0 +1,104 @@ +# Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +# 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +# +# Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +# Find package module for Metis library. +# +# The following variables are set by this module: +# +# METIS_FOUND: TRUE if Metis is found. +# METIS_INCLUDE_DIRS: Include directories for Metis. +# METIS_LIBRARIES: Libraries required to link Metis. +# +# The following variables control the behavior of this module: +# +# METIS_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for Metis includes. +# METIS_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for Metis libraries. + +set(METIS_INCLUDE_DIR_HINTS "" CACHE PATH "Metis include directory") +set(METIS_LIBRARY_DIR_HINTS "" CACHE PATH "Metis library directory") + +unset(METIS_FOUND) +unset(METIS_INCLUDE_DIRS) +unset(METIS_LIBRARIES) + +find_package(metis CONFIG QUIET) +if(TARGET metis) + set(METIS_FOUND TRUE) + set(METIS_LIBRARIES metis) + if(METIS_FOUND) + message(STATUS "Found Metis") + message(STATUS " Target : ${METIS_LIBRARIES}") + endif() +else() + list(APPEND METIS_CHECK_INCLUDE_DIRS + ${METIS_INCLUDE_DIR_HINTS} + /usr/include + /usr/local/include + /opt/include + /opt/local/include + ) + + list(APPEND METIS_CHECK_LIBRARY_DIRS + ${METIS_LIBRARY_DIR_HINTS} + /usr/lib + /usr/local/lib + /opt/lib + /opt/local/lib + ) + + find_path(METIS_INCLUDE_DIRS + NAMES + metis.h + PATHS + ${METIS_CHECK_INCLUDE_DIRS}) + find_library(METIS_LIBRARIES + NAMES + metis + PATHS + ${METIS_CHECK_LIBRARY_DIRS}) + find_library(GK_LIBRARIES + NAMES + GKlib + PATHS + ${METIS_CHECK_LIBRARY_DIRS}) + + if(METIS_FOUND) + message(STATUS "Found Metis") + message(STATUS " Includes : ${METIS_INCLUDE_DIRS}") + message(STATUS " Libraries : ${METIS_LIBRARIES}") + endif() +endif() + +if(NOT METIS_FOUND AND METIS_FIND_REQUIRED) + message(FATAL_ERROR "Could not find Metis") +endif() diff --git a/colmap/share/cmake/FindSQLite3.cmake b/colmap/share/cmake/FindSQLite3.cmake new file mode 100644 index 0000000000000000000000000000000000000000..5d9c048a7a44ede0fb4a9e741eaf5f57056f8fba --- /dev/null +++ b/colmap/share/cmake/FindSQLite3.cmake @@ -0,0 +1,93 @@ +# Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +# 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 ETH Zurich and UNC Chapel Hill 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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. +# +# Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +# Find package module for SQLite3 library. +# +# The following variables are set by this module: +# +# SQLite3_FOUND: TRUE if SQLite3 is found. +# SQLite3_INCLUDE_DIRS: Include directories for SQLite3. +# SQLite3_LIBRARIES: Libraries required to link SQLite3. +# +# The following variables control the behavior of this module: +# +# SQLite3_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for SQLite3 includes. +# SQLite3_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for SQLite3 libraries. + +set(SQLite3_INCLUDE_DIR_HINTS "" CACHE PATH "SQLite3 include directory") +set(SQLite3_LIBRARY_DIR_HINTS "" CACHE PATH "SQLite3 library directory") + +unset(SQLite3_FOUND) +unset(SQLite3_INCLUDE_DIRS) +unset(SQLite3_LIBRARIES) + +list(APPEND SQLite3_CHECK_INCLUDE_DIRS + ${SQLite3_INCLUDE_DIR_HINTS} + /usr/include + /usr/local/include + /opt/include + /opt/local/include +) + +list(APPEND SQLite3_CHECK_LIBRARY_DIRS + ${SQLite3_LIBRARY_DIR_HINTS} + /usr/lib + /usr/local/lib + /opt/lib + /opt/local/lib +) + +find_path(SQLite3_INCLUDE_DIRS + NAMES + sqlite3.h + PATHS + ${SQLite3_CHECK_INCLUDE_DIRS}) +find_library(SQLite3_LIBRARIES + NAMES + sqlite3 + PATHS + ${SQLite3_CHECK_LIBRARY_DIRS}) + +if(SQLite3_INCLUDE_DIRS AND SQLite3_LIBRARIES) + set(SQLite3_FOUND TRUE) +endif() + +if(SQLite3_FOUND) + message(STATUS "Found SQLite3") + message(STATUS " Includes : ${SQLite3_INCLUDE_DIRS}") + message(STATUS " Libraries : ${SQLite3_LIBRARIES}") +else() + if(SQLite3_FIND_REQUIRED) + message(FATAL_ERROR "Could not find SQLite3") + endif() +endif()