PR Review.

fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h

@@ -180,15 +180,15 @@ bool Fixed3DLandmarkCostFunctor::operator()(const T* const position, const T* co
   T fy = calibration[1];
   for (uint i = 0; i < pts3d_.cols(); i++)
   {
-    // Get the covariance weigthing to point losses from a pose uncertainty
+    // Get the covariance weighting to point losses from a pose uncertainty
     // From https://arxiv.org/pdf/2103.15980.pdf , equation A.7:
     // dh( e A p )  =   dh(p')  *  d(e A p)
     //     d(e)          d(p')       d(e)
     // where e is a small increment around the SE(3) manifold of A, A is a pose, p is a point,
     // h is the projection function, and p' = Ap = g, the jacobian is thus 2x6:
     // J =
     // [ (fx/gz)      (0)    (-fx * gx / gz^2)  (-fx * gx gy / gz^2)    fx(1+gx^2/gz^2)     -fx gy/gz]
-    // [     0      (fy/gz)) (-fy * gy / gz^2)     -fy(1+gy^2/gz^2)   (-fy * gx gy / gz^2)  -fy gx/gz]
+    // [     0      (fy/gz)) (-fy * gy / gz^2)     -fy(1+gy^2/gz^2)    (fy * gx gy / gz^2)  fy gx/gz]
     T gx = pts3d_.cast<T>().col(i)[0];
     T gy = pts3d_.cast<T>().col(i)[1];
     T gz = pts3d_.cast<T>().col(i)[2];

fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_constraint.h

@@ -140,7 +140,7 @@ class Fixed3DLandmarkSimpleCovarianceConstraint : public fuse_core::Constraint
   /**
    * @brief Read-only access to the square root information matrix.
    *
-   * Order is (x, y, z, qx, qy, qz)
+   * Order is (u, v)
    */
   const fuse_core::Matrix2d& sqrtInformation() const
   {
@@ -150,7 +150,7 @@ class Fixed3DLandmarkSimpleCovarianceConstraint : public fuse_core::Constraint
   /**
    * @brief Compute the measurement covariance matrix.
    *
-   * Order is (x, y, z, qx, qy, qz)
+   * Order is (u, v)
    */
   fuse_core::Matrix2d covariance() const
   {
@@ -160,7 +160,7 @@ class Fixed3DLandmarkSimpleCovarianceConstraint : public fuse_core::Constraint
   /**
    * @brief Read-only access to the observation Matrix (Nx2).
    *
-   * Order is (x, y)
+   * Order is (x, y, z)
    */
   const fuse_core::MatrixXd& pts3d() const
   {
@@ -170,7 +170,7 @@ class Fixed3DLandmarkSimpleCovarianceConstraint : public fuse_core::Constraint
   /**
    * @brief Read-only access to the observation Matrix (Nx2).
    *
-   * Order is (x, y)
+   * Order is (u, v)
    */
   const fuse_core::MatrixXd& observations() const
   {

fuse_constraints/include/fuse_constraints/reprojection_error_cost_functor.h

@@ -60,7 +60,7 @@ namespace fuse_constraints
  * where, the matrix A and the vector b are fixed, p is the camera position variable, and q is the camera orientation
  * variable, K is the calibration matrix created from the calibration variable, X is the set of marker 3D points,
  * R_b(0:3) is the Rotation matrix from the fixed landmark orentation (b(3:6)), b(0:2) is the fixed landmark position
- * and x is the 2D ovservations.
+ * and x is the 2D observations.
  *
  * Note that the covariance submatrix for the quaternion is 3x3, representing errors in the orientation local
  * parameterization tangent space. In case the user is interested in implementing a cost function of the form
@@ -80,7 +80,7 @@ class ReprojectionErrorCostFunctor
    *
    * @param[in] A The residual weighting matrix, most likely derived from the square root information
    *              matrix in order (u, v)
-   * @param[in] b The 2D pose measurement or prior in order (u, v
+   * @param[in] b The 2D pose measurement or prior in order (u, v)
    *
    **/
   ReprojectionErrorCostFunctor(const fuse_core::Matrix2d& A, const fuse_core::Vector2d& b);

fuse_constraints/include/fuse_constraints/reprojection_error_snavelly_constraint.h

@@ -44,7 +44,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_variables/orientation_3d_stamped.h>
 #include <fuse_variables/position_3d_stamped.h>
-#include <fuse_variables/pinhole_camera_simple.h>
+#include <fuse_variables/pinhole_camera_radial.h>
 
 #include <boost/serialization/access.hpp>
 #include <boost/serialization/base_object.hpp>
@@ -91,7 +91,7 @@ class ReprojectionErrorSnavellyConstraint : public fuse_core::Constraint
    */
   ReprojectionErrorSnavellyConstraint(const std::string& source, const fuse_variables::Position3DStamped& position,
                                       const fuse_variables::Orientation3DStamped& orientation,
-                                      const fuse_variables::PinholeCameraSimple& calibraton,
+                                      const fuse_variables::PinholeCameraRadial& calibraton,
                                       const fuse_core::Vector2d& mean, const fuse_core::Matrix2d& covariance);
 
   /**

fuse_constraints/src/reprojection_error_snavelly_constraint.cpp

@@ -51,7 +51,7 @@ namespace fuse_constraints
 ReprojectionErrorSnavellyConstraint::ReprojectionErrorSnavellyConstraint(
     const std::string& source, const fuse_variables::Position3DStamped& position,
     const fuse_variables::Orientation3DStamped& orientation,
-    const fuse_variables::PinholeCameraSimple& calibration,
+    const fuse_variables::PinholeCameraRadial& calibration,
     const fuse_core::Vector2d& mean,
     const fuse_core::Matrix2d& covariance)
   : fuse_core::Constraint(source, { position.uuid(), orientation.uuid(), calibration.uuid() })

fuse_constraints/test/test_bal_problem.cpp

@@ -43,7 +43,7 @@
 #include <fuse_variables/point_3d_landmark.h>
 #include <fuse_variables/point_3d_fixed_landmark.h>
 #include <fuse_variables/position_3d_stamped.h>
-#include <fuse_variables/pinhole_camera_simple.h>
+#include <fuse_variables/pinhole_camera_radial.h>
 
 #include <ceres/covariance.h>
 #include <ceres/problem.h>
@@ -58,7 +58,7 @@
 
 using fuse_constraints::ReprojectionErrorSnavellyConstraint;
 using fuse_variables::Orientation3DStamped;
-using fuse_variables::PinholeCameraSimple;
+using fuse_variables::PinholeCameraRadial;
 using fuse_variables::Point3DFixedLandmark;
 using fuse_variables::Point3DLandmark;
 using fuse_variables::Position3DStamped;
@@ -230,7 +230,7 @@ struct SnavelyReprojectionErrorWithQuaternions
   template <typename T>
   bool operator()(const T* const camera, const T* const point, T* residuals) const
   {
-    // camera[0,1,2,3] is are the rotation of the camera as a quaternion.
+    // camera[0,1,2,3] is the rotation of the camera as a quaternion.
     //
     // We use QuaternionRotatePoint as it does not assume that the
     // quaternion is normalized, since one of the ways to run the
@@ -276,7 +276,7 @@ TEST(ReprojectionErrorSnavellyConstraint, Constructor)
   Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
   Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
   Point3DLandmark point(0);
-  PinholeCameraSimple calibration_variable(0);
+  PinholeCameraRadial calibration_variable(0);
 
   fuse_core::Vector2d mean;
   mean << 320.0, 240.0;  // Centre of a 640x480 camera
@@ -296,7 +296,7 @@ TEST(ReprojectionErrorSnavellyConstraint, Covariance)
   Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo"));
   Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo"));
   Point3DLandmark point(0);
-  PinholeCameraSimple calibration_variable(0);
+  PinholeCameraRadial calibration_variable(0);
 
   fuse_core::Vector2d mean;
   mean << 320.0, 240.0;  // Centre of a 640x480 camera
@@ -355,7 +355,7 @@ TEST(ReprojectionErrorSnavellyConstraint, BAL)
 
   std::vector<Position3DStamped> cams_p(bal_problem.num_cameras());
   std::vector<Orientation3DStamped> cams_q(bal_problem.num_cameras());
-  std::vector<PinholeCameraSimple> cams_k(bal_problem.num_cameras());
+  std::vector<PinholeCameraRadial> cams_k(bal_problem.num_cameras());
   for (int i = 0; i < bal_problem.num_cameras(); i++)
   {
     auto cam = bal_problem.camera(i);
@@ -456,7 +456,7 @@ TEST(ReprojectionErrorSnavellyConstraint, Serialization)
   Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
   Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
 
-  PinholeCameraSimple calibration_variable(0);
+  PinholeCameraRadial calibration_variable(0);
   calibration_variable.f() = 640;
   calibration_variable.r1() = 0.1;
   calibration_variable.r2() = 0.1;

fuse_constraints/test/test_reprojection_error_snavelly_constraint.cpp

@@ -43,7 +43,7 @@
 #include <fuse_variables/point_3d_landmark.h>
 #include <fuse_variables/point_3d_fixed_landmark.h>
 #include <fuse_variables/position_3d_stamped.h>
-#include <fuse_variables/pinhole_camera_simple.h>
+#include <fuse_variables/pinhole_camera_radial.h>
 
 #include <ceres/covariance.h>
 #include <ceres/problem.h>
@@ -55,7 +55,7 @@
 
 using fuse_constraints::ReprojectionErrorSnavellyConstraint;
 using fuse_variables::Orientation3DStamped;
-using fuse_variables::PinholeCameraSimple;
+using fuse_variables::PinholeCameraRadial;
 using fuse_variables::Point3DLandmark;
 using fuse_variables::Point3DFixedLandmark;
 using fuse_variables::Position3DStamped;
@@ -66,7 +66,7 @@ TEST(ReprojectionErrorSnavellyConstraint, Constructor)
   Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
   Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
   Point3DLandmark point(0);
-  PinholeCameraSimple calibration_variable(0);
+  PinholeCameraRadial calibration_variable(0);
 
   fuse_core::Vector2d mean;
   mean << 320.0, 240.0;  // Centre of a 640x480 camera
@@ -86,7 +86,7 @@ TEST(ReprojectionErrorSnavellyConstraint, Covariance)
   Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo"));
   Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo"));
   Point3DLandmark point(0);
-  PinholeCameraSimple calibration_variable(0);
+  PinholeCameraRadial calibration_variable(0);
 
   fuse_core::Vector2d mean;
   mean << 320.0, 240.0;  // Centre of a 640x480 camera
@@ -125,7 +125,7 @@ TEST(ReprojectionErrorSnavellyConstraint, Optimization)
   orientation_variable->y() = -0.189;
   orientation_variable->z() = 0.239;
 
-  auto calibration_variable = PinholeCameraSimple::make_shared(0);
+  auto calibration_variable = PinholeCameraRadial::make_shared(0);
   calibration_variable->f() = 638.34478759765620;
   calibration_variable->r1() = 0.13281739520782995;
   calibration_variable->r2() = -0.17255676937880005;
@@ -263,7 +263,7 @@ TEST(ReprojectionErrorSnavellyConstraint, Serialization)
   Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
   Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
 
-  PinholeCameraSimple calibration_variable(0);
+  PinholeCameraRadial calibration_variable(0);
   calibration_variable.f() = 638.34478759765620;
   calibration_variable.r1() = 0.13281739520782995;
   calibration_variable.r2() = -0.17255676937880005;

fuse_variables/CMakeLists.txt

@@ -36,7 +36,7 @@ add_library(${PROJECT_NAME}
   src/acceleration_linear_3d_stamped.cpp
   src/pinhole_camera.cpp
   src/pinhole_camera_fixed.cpp
-  src/pinhole_camera_simple.cpp
+  src/pinhole_camera_radial.cpp
   src/orientation_2d_stamped.cpp
   src/orientation_3d_stamped.cpp
   src/point_2d_fixed_landmark.cpp
@@ -248,24 +248,24 @@ if(CATKIN_ENABLE_TESTING)
   )
 
   # Camera Intrinsics tests
-  catkin_add_gtest(test_pinhole_camera_simple
-  test/test_pinhole_camera_simple.cpp
+  catkin_add_gtest(test_pinhole_camera_radial
+  test/test_pinhole_camera_radial.cpp
   )
-  add_dependencies(test_pinhole_camera_simple
+  add_dependencies(test_pinhole_camera_radial
   ${catkin_EXPORTED_TARGETS}
   )
-  target_include_directories(test_pinhole_camera_simple
+  target_include_directories(test_pinhole_camera_radial
   PRIVATE
     include
     ${catkin_INCLUDE_DIRS}
     ${CERES_INCLUDE_DIRS}
   )
-  target_link_libraries(test_pinhole_camera_simple
+  target_link_libraries(test_pinhole_camera_radial
   ${PROJECT_NAME}
   ${catkin_LIBRARIES}
   ${CERES_LIBRARIES}
   )
-  set_target_properties(test_pinhole_camera_simple
+  set_target_properties(test_pinhole_camera_radial
   PROPERTIES
     CXX_STANDARD 17
     CXX_STANDARD_REQUIRED YES

fuse_variables/include/fuse_variables/base_camera.h

@@ -71,6 +71,7 @@ class BaseCamera : public FixedSizeVariable<N>
   /**
    * @brief Construct a pinhole camera variable given a camera id and intrinsic parameters
    *
+   * @param[in] uuid        The UUID of the sensor
    * @param[in] camera_id  The id associated to a camera
    */
   explicit BaseCamera(const fuse_core::UUID& uuid, const uint64_t& camera_id):
@@ -79,10 +80,11 @@ class BaseCamera : public FixedSizeVariable<N>
   /**
    * @brief Construct a pinhole camera variable given a camera id
    *
-   * @param[in] camera_id  The id associated to a camera
+   * @param[in] camera_name  The id associated to a camera (e.g. which camera on a robot)
+   * @param[in] device_id  The device_id associated to the camera (e.g. which robot)
    */
-  explicit BaseCamera(const uint64_t& camera_id):
-  BaseCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) {}
+explicit BaseCamera(const uint64_t& camera_id, const fuse_core::UUID& device_id = fuse_core::uuid::NIL)
+  : BaseCamera(fuse_core::uuid::generate(detail::type(), camera_id, device_id)) {}
 
   /**
    * @brief Read-only access to the id

fuse_variables/include/fuse_variables/pinhole_camera.h

@@ -138,7 +138,6 @@ class PinholeCamera : public BaseCamera<4>
   /**
    * @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

fuse_variables/include/fuse_variables/pinhole_camera_simple.h → fuse_variables/include/fuse_variables/pinhole_camera_radial.h

@@ -34,8 +34,8 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef FUSE_VARIABLES_PINHOLE_CAMERA_SIMPLE_H
-#define FUSE_VARIABLES_PINHOLE_CAMERA_SIMPLE_H
+#ifndef FUSE_VARIABLES_PINHOLE_CAMERA_RADIAL_H
+#define FUSE_VARIABLES_PINHOLE_CAMERA_RADIAL_H
 
 #include <fuse_core/fuse_macros.h>
 #include <fuse_core/serialization.h>
@@ -58,10 +58,10 @@ namespace fuse_variables
  * construction and dependent on a user input database id. As such, the database id cannot be altered after
  * construction.
  */
-class PinholeCameraSimple : public BaseCamera<3>
+class PinholeCameraRadial : public BaseCamera<3>
 {
 public:
-  FUSE_VARIABLE_DEFINITIONS(PinholeCameraSimple);
+  FUSE_VARIABLE_DEFINITIONS(PinholeCameraRadial);
 
   /**
    * @brief Can be used to directly index variables in the data array
@@ -76,21 +76,21 @@ class PinholeCameraSimple : public BaseCamera<3>
   /**
    * @brief Default constructor
    */
-  PinholeCameraSimple() = default;
+  PinholeCameraRadial() = default;
 
   /**
    * @brief Construct a pinhole camera variable given a camera id
    *
    * @param[in] camera_id  The id associated to a camera
    */
-  explicit PinholeCameraSimple(const uint64_t& camera_id);
+  explicit PinholeCameraRadial(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 PinholeCameraSimple(const fuse_core::UUID& uuid, const uint64_t& camera_id,
+  explicit PinholeCameraRadial(const fuse_core::UUID& uuid, const uint64_t& camera_id,
                               const double& f, const double& r1, const double& r2);
 
   /**
@@ -137,7 +137,7 @@ class PinholeCameraSimple : public BaseCamera<3>
    *
    * @param[in] camera_id  The id associated to a camera_id
    */
-  PinholeCameraSimple(const fuse_core::UUID& uuid, const uint64_t& camera_id);
+  PinholeCameraRadial(const fuse_core::UUID& uuid, const uint64_t& camera_id);
 
 private:
   // Allow Boost Serialization access to private methods
@@ -162,6 +162,6 @@ class PinholeCameraSimple : public BaseCamera<3>
 
 }  // namespace fuse_variables
 
-BOOST_CLASS_EXPORT_KEY(fuse_variables::PinholeCameraSimple);
+BOOST_CLASS_EXPORT_KEY(fuse_variables::PinholeCameraRadial);
 
-#endif  // FUSE_VARIABLES_PINHOLE_CAMERA_SIMPLE_H
+#endif  // FUSE_VARIABLES_PINHOLE_CAMERA_RADIAL_H