Port pinhole camera from ROS1 (#4)

* add upstream pinhole-camera-related files * fix pinhole camera --------- Co-authored-by: Bilal Gill <[email protected]>
PickNikRobotics · Oct 7, 2024 · 190ff99 · 190ff99
1 parent 6212aeb
commit 190ff99
Show file tree

Hide file tree

Showing 9 changed files with 1,442 additions and 0 deletions.
diff --git a/.clang-tidy b/.clang-tidy
@@ -13,8 +13,12 @@ Checks: -*,
   -bugprone-exception-escape,
   -cppcoreguidelines-avoid-magic-numbers,
   -cppcoreguidelines-non-private-member-variables-in-classes,
+  -cppcoreguidelines-pro-bounds-constant-array-index,
+  -cppcoreguidelines-pro-bounds-pointer-arithmetic,
   -google-readability-casting,
+  -google-default-arguments,
   -misc-include-cleaner,
+  -misc-non-private-member-variables-in-classes,
   -modernize-use-trailing-return-type,
   -modernize-avoid-bind,
   -readability-identifier-length,

diff --git a/fuse_variables/CMakeLists.txt b/fuse_variables/CMakeLists.txt
@@ -30,6 +30,8 @@ add_library(
   src/acceleration_linear_3d_stamped.cpp
   src/orientation_2d_stamped.cpp
   src/orientation_3d_stamped.cpp
+  src/pinhole_camera.cpp
+  src/pinhole_camera_fixed.cpp
   src/point_2d_fixed_landmark.cpp
   src/point_2d_landmark.cpp
   src/point_3d_fixed_landmark.cpp

diff --git a/fuse_variables/include/fuse_variables/pinhole_camera.hpp b/fuse_variables/include/fuse_variables/pinhole_camera.hpp
@@ -0,0 +1,224 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Author:    Oscar Mendez
+ *  Created:   11.13.2023
+ *
+ *  Copyright (c) 2023, Locus Robotics
+ *  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 copyright holder 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 HOLDER 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 FUSE_VARIABLES_PINHOLE_CAMERA_H
+#define FUSE_VARIABLES_PINHOLE_CAMERA_H
+
+#include <fuse_core/ceres_macros.hpp>
+#include <fuse_core/fuse_macros.hpp>
+#include <fuse_core/manifold.hpp>
+#include <fuse_core/serialization.hpp>
+#include <fuse_core/uuid.hpp>
+#include <fuse_variables/fixed_size_variable.hpp>
+
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/base_object.hpp>
+#include <boost/serialization/export.hpp>
+
+#include <ostream>
+
+namespace fuse_variables
+{
+/**
+ * @brief Variable representing intrinsic parameters of a camera.
+ *
+ * The UUID of this class is constant after
+ * construction and dependent on a user input database id. As such, the database id cannot be altered after
+ * construction.
+ */
+class PinholeCamera : public FixedSizeVariable<4>
+{
+public:
+  FUSE_VARIABLE_DEFINITIONS(PinholeCamera);
+
+  /**
+   * @brief Can be used to directly index variables in the data array
+   */
+  enum : size_t
+  {
+    FX = 0,
+    FY = 1,
+    CX = 2,
+    CY = 3
+  };
+
+  /**
+   * @brief Default constructor
+   */
+  PinholeCamera() = default;
+
+  /**
+   * @brief Construct a pinhole camera variable given a camera id
+   *
+   * @param[in] camera_id  The id associated to a camera
+   */
+  explicit PinholeCamera(const uint64_t& camera_id);
+
+  /**
+   * @brief Construct a pinhole camera variable given a camera id and intrinsic parameters
+   *
+   * @param[in] camera_id  The id associated to a camera
+   */
+  explicit PinholeCamera(const fuse_core::UUID& uuid, const uint64_t& camera_id, const double& fx, const double& fy,
+                         const double& cx, const double& cy);
+
+  /**
+   * @brief Read-write access to the cx parameter.
+   */
+  double& cx()
+  {
+    return data_[CX];
+  }
+
+  /**
+   * @brief Read-only access to the cx parameter.
+   */
+  const double& cx() const
+  {
+    return data_[CX];
+  }
+
+  /**
+   * @brief Read-write access to the cy parameter.
+   */
+  double& cy()
+  {
+    return data_[CY];
+  }
+
+  /**
+   * @brief Read-only access to the cy parameter.
+   */
+  const double& cy() const
+  {
+    return data_[CY];
+  }
+
+  /**
+   * @brief Read-write access to the fx parameter.
+   */
+  double& fx()
+  {
+    return data_[FX];
+  }
+
+  /**
+   * @brief Read-only access to the fx parameter.
+   */
+  const double& fx() const
+  {
+    return data_[FX];
+  }
+
+  /**
+   * @brief Read-write access to the fy parameter.
+   */
+  double& fy()
+  {
+    return data_[FY];
+  }
+
+  /**
+   * @brief Read-only access to the fy parameter.
+   */
+  const double& fy() const
+  {
+    return data_[FY];
+  }
+
+  /**
+   * @brief Read-only access to the id
+   */
+  const uint64_t& id() const
+  {
+    return id_;
+  }
+
+  /**
+   * @brief Print a human-readable description of the variable to the provided
+   * stream.
+   *
+   * @param[out] stream The stream to write to. Defaults to stdout.
+   */
+  void print(std::ostream& stream = std::cout) const override;
+
+#if CERES_SUPPORTS_MANIFOLDS
+  /**
+   * @brief Create a null Ceres manifold
+   *
+   * Overriding the manifold() method prevents additional processing with the ManifoldAdapter
+   */
+  fuse_core::Manifold* manifold() const override
+  {
+    return nullptr;
+  }
+#endif
+
+protected:
+  /**
+   * @brief Construct a point 3D variable given a camera id
+   *
+   * @param[in] camera_id  The id associated to a camera
+   */
+  PinholeCamera(const fuse_core::UUID& uuid, const uint64_t& camera_id);
+
+private:
+  // Allow Boost Serialization access to private methods
+  friend class boost::serialization::access;
+  uint64_t id_{ 0 };
+
+  /**
+   * @brief The Boost Serialize method that serializes all of the data members
+   * in to/out of the archive
+   *
+   * @param[in/out] archive - The archive object that holds the serialized class
+   * members
+   * @param[in] version - The version of the archive being read/written.
+   * Generally unused.
+   */
+  template <class Archive>
+  void serialize(Archive& archive, const unsigned int /* version */)
+  {
+    archive& boost::serialization::base_object<FixedSizeVariable<SIZE>>(*this);
+    archive& id_;
+  }
+};
+
+}  // namespace fuse_variables
+
+BOOST_CLASS_EXPORT_KEY(fuse_variables::PinholeCamera);
+
+#endif  // FUSE_VARIABLES_PINHOLE_CAMERA_H
diff --git a/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp
@@ -0,0 +1,124 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Author:    Oscar Mendez
+ *  Created:   11.13.2023
+ *
+ *  Copyright (c) 2023, Locus Robotics
+ *  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 copyright holder 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 HOLDER 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 FUSE_VARIABLES_PINHOLE_CAMERA_FIXED_H
+#define FUSE_VARIABLES_PINHOLE_CAMERA_FIXED_H
+
+#include <fuse_core/ceres_macros.hpp>
+#include <fuse_core/fuse_macros.hpp>
+#include <fuse_core/manifold.hpp>
+#include <fuse_core/serialization.hpp>
+#include <fuse_variables/pinhole_camera.hpp>
+
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/base_object.hpp>
+#include <boost/serialization/export.hpp>
+
+namespace fuse_variables
+{
+/**
+ * @brief Variable representing a pinhole camera that exists across time.
+ *
+ */
+class PinholeCameraFixed : public PinholeCamera
+{
+public:
+  FUSE_VARIABLE_DEFINITIONS(PinholeCameraFixed);
+
+  /**
+   * @brief Default constructor
+   */
+  PinholeCameraFixed() = default;
+
+  /**
+   * @brief Construct a point 2D variable given a landmarks id
+   *
+   * @param[in] camera_id  The id associated to a camera
+   */
+  explicit PinholeCameraFixed(const uint64_t& camera_id);
+
+  /**
+   * @brief Construct a pinhole camera variable given a camera id and intrinsic parameters
+   *
+   * @param[in] camera_id  The id associated to a camera
+   */
+  explicit PinholeCameraFixed(const uint64_t& camera_id, const double& fx, const double& fy, const double& cx,
+                              const double& cy);
+  /**
+   * @brief Specifies if the value of the variable should not be changed during optimization
+   */
+  bool holdConstant() const override
+  {
+    return true;
+  }
+
+#if CERES_SUPPORTS_MANIFOLDS
+  /**
+   * @brief Create a null Ceres manifold
+   *
+   * Overriding the manifold() method prevents additional processing with the ManifoldAdapter
+   */
+  fuse_core::Manifold* manifold() const override
+  {
+    return nullptr;
+  }
+#endif
+
+private:
+  // Allow Boost Serialization access to private methods
+  friend class boost::serialization::access;
+
+  /**
+   * @brief The Boost Serialize method that serializes all of the data members
+   * in to/out of the archive
+   *
+   * @param[in/out] archive - The archive object that holds the serialized class
+   * members
+   * @param[in] version - The version of the archive being read/written.
+   * Generally unused.
+   */
+  template <class Archive>
+  void serialize(Archive& archive, const unsigned int /* version */)
+  {
+    archive& boost::serialization::base_object<PinholeCamera>(*this);
+  }
+};
+
+}  // namespace fuse_variables
+
+BOOST_CLASS_EXPORT_KEY(fuse_variables::PinholeCameraFixed);
+
+#endif  // FUSE_VARIABLES_PINHOLE_CAMERA_FIXED_H