Vision constraints

fuse_constraints/CMakeLists.txt

@@ -45,6 +45,9 @@ add_library(${PROJECT_NAME}
   src/absolute_pose_2d_stamped_constraint.cpp
   src/absolute_pose_3d_stamped_constraint.cpp
   src/fixed_3d_landmark_constraint.cpp
+  src/fixed_3d_landmark_simple_covariance_constraint.cpp
+  src/reprojection_error_constraint.cpp
+  src/reprojection_error_snavelly_constraint.cpp
   src/marginal_constraint.cpp
   src/marginal_cost_function.cpp
   src/marginalize_variables.cpp
@@ -266,6 +269,79 @@ if(CATKIN_ENABLE_TESTING)
       CXX_STANDARD_REQUIRED YES
   )
 
+  # Reprojection Error Constraint Tests
+  catkin_add_gtest(test_reprojection_error_constraint
+    test/test_reprojection_error_constraint.cpp
+  )
+  add_dependencies(test_reprojection_error_constraint
+    ${catkin_EXPORTED_TARGETS}
+  )
+  target_include_directories(test_reprojection_error_constraint
+    PRIVATE
+      include
+      ${catkin_INCLUDE_DIRS}
+      ${CERES_INCLUDE_DIRS}
+  )
+  target_link_libraries(test_reprojection_error_constraint
+    ${PROJECT_NAME}
+    ${catkin_LIBRARIES}
+    ${CERES_LIBRARIES}
+  )
+  set_target_properties(test_reprojection_error_constraint
+    PROPERTIES
+      CXX_STANDARD 17
+      CXX_STANDARD_REQUIRED YES
+  )
+
+  # Reprojection Error Snavelly Constraint Tests
+  catkin_add_gtest(test_reprojection_error_snavelly_constraint
+    test/test_reprojection_error_snavelly_constraint.cpp
+  )
+  add_dependencies(test_reprojection_error_snavelly_constraint
+    ${catkin_EXPORTED_TARGETS}
+  )
+  target_include_directories(test_reprojection_error_snavelly_constraint
+    PRIVATE
+      include
+      ${catkin_INCLUDE_DIRS}
+      ${CERES_INCLUDE_DIRS}
+  )
+  target_link_libraries(test_reprojection_error_snavelly_constraint
+    ${PROJECT_NAME}
+    ${catkin_LIBRARIES}
+    ${CERES_LIBRARIES}
+  )
+  set_target_properties(test_reprojection_error_snavelly_constraint
+    PROPERTIES
+      CXX_STANDARD 17
+      CXX_STANDARD_REQUIRED YES
+  )
+
+  # BAL Problem Tests
+  catkin_add_gtest(test_bal_problem
+    test/test_bal_problem.cpp
+    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test
+  )
+  add_dependencies(test_bal_problem
+    ${catkin_EXPORTED_TARGETS}
+  )
+  target_include_directories(test_bal_problem
+    PRIVATE
+      include
+      ${catkin_INCLUDE_DIRS}
+      ${CERES_INCLUDE_DIRS}
+  )
+  target_link_libraries(test_bal_problem
+    ${PROJECT_NAME}
+    ${catkin_LIBRARIES}
+    ${CERES_LIBRARIES}
+  )
+  set_target_properties(test_bal_problem
+    PROPERTIES
+      CXX_STANDARD 17
+      CXX_STANDARD_REQUIRED YES
+  )
+
   # Absolute Pose 3D Stamped Constraint Tests
   catkin_add_gtest(test_absolute_pose_3d_stamped_constraint
     test/test_absolute_pose_3d_stamped_constraint.cpp

fuse_constraints/include/fuse_constraints/fixed_3d_landmark_constraint.h

@@ -63,10 +63,10 @@ namespace fuse_constraints
  *
  * A landmark is represented as a 3D pose (3D position and a 3D orientation). This class takes 
  * the ground truth location of the 3D landmark and applies a reprojection-error based constraint
- * an constraint on the position, orientation and calibration of the camera that observed the landmark.
+ * on the position, orientation and calibration of the camera that observed the landmark.
  * 
  * In most cases, the camera calibration should be held fixed as a single landmark does not present enough
- * points to accurate constrain the pose AND the calibraton.
+ * points to accurately constrain the pose AND the calibraton.
  * 
  */
 class Fixed3DLandmarkConstraint : public fuse_core::Constraint

fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h

@@ -114,8 +114,6 @@ class Fixed3DLandmarkCostFunctor
   fuse_core::Vector7d b_;
   fuse_core::MatrixXd obs_;
   fuse_core::MatrixXd pts3d_;
-
-  NormalPriorOrientation3DCostFunctor orientation_functor_;
 };
 
 Fixed3DLandmarkCostFunctor::Fixed3DLandmarkCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector7d& b,
@@ -124,7 +122,6 @@ Fixed3DLandmarkCostFunctor::Fixed3DLandmarkCostFunctor(const fuse_core::MatrixXd
   , b_(b)
   , obs_(obs)
   , pts3d_(pts3d.transpose())  // Transpose from Nx3 to 3xN to make math easier.
-  , orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>())  // Delta will not be scaled
 {
   assert(pts3d_.rows() == 3);  // Check if we have 3xN
 
@@ -179,13 +176,35 @@ bool Fixed3DLandmarkCostFunctor::operator()(const T* const position, const T* co
 
   auto d = (obs_.cast<T>() - xp.block(0, 0, xp.rows(), 2));
 
-  for (uint i = 0; i < 4; i++)
+  T fx = calibration[0];
+  T fy = calibration[1];
+  for (uint i = 0; i < pts3d_.cols(); i++)
   {
-    residual[i * 2] = d.row(i)[0];
-    residual[i * 2 + 1] = d.row(i)[1];
+    // Get the covariance weigthing 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]
+    T gx = pts3d_.cast<T>().col(i)[0];
+    T gy = pts3d_.cast<T>().col(i)[1];
+    T gz = pts3d_.cast<T>().col(i)[2];
+    T gz2 = gz*gz;
+    T gxyz = (gx*gy)/gz2;
+    Eigen::Matrix<T, 2, 6, Eigen::RowMajor> J;
+    J << fx/gz,   T(0), -fx * (gx / gz2),     -fx*gxyz,        fx*(T(1)+(gx*gx)/gz2), -fx * gy/gz,
+          T(0),  fy/gz, -fy * (gy / gz2), -fy*(T(1)+(gy*gy)/gz2),       fy*gxyz,       fy * gx/gz;
+    Eigen::Matrix<T, 2, 2, Eigen::RowMajor> A = J*A_*J.transpose();
+
+    // Weight Residuals
+    auto r = A * d.row(i).transpose();
+    residual[i * 2] = r[0];
+    residual[i * 2 + 1] = r[1];
   }
 
-  // TODO(omendez): how do we apply covariance weigthing to point losses from a pose uncertainty?
 
   return true;
 }

fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_constraint.h

@@ -0,0 +1,229 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Author: Oscar Mendez
+ *  Created on Mon Nov 12 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_CONSTRAINTS_FIXED_3D_LANDMARK_SIMPLE_COVARIANCE_CONSTRAINT_H
+#define FUSE_CONSTRAINTS_FIXED_3D_LANDMARK_SIMPLE_COVARIANCE_CONSTRAINT_H
+
+#include <fuse_core/constraint.h>
+#include <fuse_core/eigen.h>
+#include <fuse_core/fuse_macros.h>
+#include <fuse_core/serialization.h>
+#include <fuse_core/uuid.h>
+#include <fuse_variables/orientation_3d_stamped.h>
+#include <fuse_variables/position_3d_stamped.h>
+#include <fuse_variables/pinhole_camera.h>
+
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/base_object.hpp>
+#include <boost/serialization/export.hpp>
+#include <Eigen/Dense>
+
+#include <ostream>
+#include <string>
+#include <vector>
+
+namespace fuse_constraints
+{
+
+/**
+ * @brief A constraint that represents an observation of a 3D landmark (ARTag or Similar)
+ *
+ * A landmark is represented as a 3D pose (3D position and a 3D orientation). This class takes
+ * the ground truth location of the 3D landmark and applies a reprojection-error based constraint
+ * on the position, orientation and calibration of the camera that observed the landmark.
+ *
+ * In most cases, the camera calibration should be held fixed as a single landmark does not present enough
+ * points to accurately constrain the pose AND the calibraton.
+ *
+ */
+class Fixed3DLandmarkSimpleCovarianceConstraint : public fuse_core::Constraint
+{
+public:
+  FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(Fixed3DLandmarkSimpleCovarianceConstraint);
+
+  /**
+   * @brief Default constructor
+   */
+  Fixed3DLandmarkSimpleCovarianceConstraint() = default;
+
+  /**
+   * @brief Create a constraint using a known 3D fiducial marker
+   *
+   * @param[in] source        The name of the sensor or motion model that generated this constraint
+   * @param[in] position      The variable representing the position components of the landmark pose
+   * @param[in] orientation   The variable representing the orientation components of the marker pose
+   * @param[in] calibraton    The intrinsic calibration parameters of the camera (4x1 vector: cx, cy, fx, fy).
+   *                          NOTE: Best practice is to fix this variable unless we have several observations
+   *                          with the same camera
+   * @param[in] pts3d         Matrix of 3D points in marker coordiate frame.
+   * @param[in] observations  The 2D (pixel) observations of each marker's corners.
+   * @param[in] mean          The measured/prior pose of the marker as a vector (7x1 vector: x, y, z, qw, qx, qy, qz)
+   * @param[in] covariance    The detection covariance, in pixels (2x2 matrix: u, v)
+   */
+  Fixed3DLandmarkSimpleCovarianceConstraint(const std::string& source,
+                                            const fuse_variables::Position3DStamped& position,
+                                            const fuse_variables::Orientation3DStamped& orientation,
+                                            const fuse_variables::PinholeCamera& calibraton,
+                                            const fuse_core::MatrixXd& pts3d, const fuse_core::MatrixXd& observations,
+                                            const fuse_core::Vector7d& mean, const fuse_core::Matrix2d& covariance);
+
+  /**
+   * @brief Create a constraint using a known 3D fiducial marker. Convenience constructor for the special case of
+   *        an ARTag with 4 corners.
+   *
+   * @param[in] source        The name of the sensor or motion model that generated this constraint
+   * @param[in] position      The variable representing the position components of the marker pose
+   * @param[in] orientation   The variable representing the orientation components of the marker pose
+   * @param[in] calibraton    The intrinsic calibration parameters of the camera (4x1 vector: cx, cy, fx, fy).
+   *                          NOTE: Best practice is to fix this variable unless we have several observations
+   *                          with the same camera
+   * @param[in] marker_size   The size of the marker, in meters. Assumed to be square.
+   * @param[in] observations  The 2D (pixel) observations of each marker's corners.
+   * @param[in] mean          The measured/prior pose of the marker as a vector (7x1 vector: x, y, z, qw, qx, qy, qz)
+   * @param[in] covariance    The detection covariance, in pixels (2x2 matrix: u, v)
+   */
+  Fixed3DLandmarkSimpleCovarianceConstraint(const std::string& source,
+                                            const fuse_variables::Position3DStamped& position,
+                                            const fuse_variables::Orientation3DStamped& orientation,
+                                            const fuse_variables::PinholeCamera& calibraton, const double& marker_size,
+                                            const fuse_core::MatrixXd& observations, const fuse_core::Vector7d& mean,
+                                            const fuse_core::Matrix2d& covariance);
+
+  /**
+   * @brief Destructor
+   */
+  virtual ~Fixed3DLandmarkSimpleCovarianceConstraint() = default;
+
+  /**
+   * @brief Read-only access to the measured/prior vector of mean values.
+   *
+   * Order is (x, y, z, qw, qx, qy, qz)
+   */
+  const fuse_core::Vector7d& mean() const
+  {
+    return mean_;
+  }
+
+  /**
+   * @brief Read-only access to the square root information matrix.
+   *
+   * Order is (x, y, z, qx, qy, qz)
+   */
+  const fuse_core::Matrix2d& sqrtInformation() const
+  {
+    return sqrt_information_;
+  }
+
+  /**
+   * @brief Compute the measurement covariance matrix.
+   *
+   * Order is (x, y, z, qx, qy, qz)
+   */
+  fuse_core::Matrix2d covariance() const
+  {
+    return (sqrt_information_.transpose() * sqrt_information_).inverse();
+  }
+
+  /**
+   * @brief Read-only access to the observation Matrix (Nx2).
+   *
+   * Order is (x, y)
+   */
+  const fuse_core::MatrixXd& pts3d() const
+  {
+    return pts3d_;
+  }
+
+  /**
+   * @brief Read-only access to the observation Matrix (Nx2).
+   *
+   * Order is (x, y)
+   */
+  const fuse_core::MatrixXd& observations() const
+  {
+    return observations_;
+  }
+
+  /**
+   * @brief Print a human-readable description of the constraint to the provided stream.
+   *
+   * @param[out] stream The stream to write to. Defaults to stdout.
+   */
+  void print(std::ostream& stream = std::cout) const override;
+
+  /**
+   * @brief Construct an instance of this constraint's cost function
+   *
+   * The function caller will own the new cost function instance. It is the responsibility of the caller to delete
+   * the cost function object when it is no longer needed. If the pointer is provided to a Ceres::Problem object, the
+   * Ceres::Problem object will takes ownership of the pointer and delete it during destruction.
+   *
+   * @return A base pointer to an instance of a derived CostFunction.
+   */
+  ceres::CostFunction* costFunction() const override;
+
+protected:
+  fuse_core::MatrixXd pts3d_;             //!< The 3D points in marker Coordinate frame
+  fuse_core::MatrixXd observations_;      //!< The 2D observations (in pixel space) of the marker at postion mean_
+  fuse_core::Vector7d mean_;              //!< The measured/prior mean vector for this variable
+  fuse_core::Matrix2d sqrt_information_;  //!< The square root information matrix
+
+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<fuse_core::Constraint>(*this);
+    archive& pts3d_;
+    archive& observations_;
+    archive& mean_;
+    archive& sqrt_information_;
+  }
+};
+
+}  // namespace fuse_constraints
+
+BOOST_CLASS_EXPORT_KEY(fuse_constraints::Fixed3DLandmarkSimpleCovarianceConstraint);
+
+#endif  // FUSE_CONSTRAINTS_FIXED_3D_LANDMARK_SIMPLE_COVARIANCE_CONSTRAINT_H