Vision constraints #352

oscarmendezm · 2023-12-15T16:53:48Z

Adding improved vision constraints, including worked out jacobian for fixed landmark, some extra tests and a simple reprojection error constraint. Also adding a test using BAL, and python scripts used to generate test-cases.

Simple Pinhole and Fixed Landmark with Jacobian.

2x2 matrix

and fixing documentation

svwilliams

Round 1: I made it through the constraints include/src. I'll work on the tests tomorrow. And then the camera variables.

svwilliams · 2023-12-19T21:54:07Z

fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h

 {
- 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


Suggested change

// Get the covariance weigthing to point losses from a pose uncertainty

// Get the covariance weighting to point losses from a pose uncertainty

svwilliams · 2023-12-19T21:59:46Z

fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h

+ // [ (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]


I'm not going to pretend I independently verified this derivation.

I didn't manually compute the entire thing, but it make sense to me and I think it is the right way to do this given the info on the ArXiv. I could be wrong, but short of spending ages in Jacobian hell, it works?

svwilliams · 2023-12-19T22:10:23Z

fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h

+ 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;


I'm just matching the equations here to the comments posted above. Elements [2, 5] and [2, 6] have a different sign in the code versus the comments. Not sure which one is correct.

Comment was wrong. Verified against paper.

svwilliams · 2023-12-19T22:13:06Z

fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_constraint.h

+ /**
+ * @brief Read-only access to the square root information matrix.
+ *
+ * Order is (x, y, z, qx, qy, qz)


The covariance/information matrix is only 2x2. The order should be (x, y) or (u, v), correct?
Same with the covariance() method below.

Correct, this case is the "simple" covariance (i.e. pixel error)

svwilliams · 2023-12-19T22:14:45Z

fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_constraint.h

+ /**
+ * @brief Read-only access to the observation Matrix (Nx2).
+ *
+ * Order is (x, y)
+ */


These are 3D points, correct? Shouldn't the size should be (Nx3) and they should have a Z?

Yep. My copy pasting has bit me in the butt again.

svwilliams · 2023-12-19T22:31:56Z

fuse_constraints/include/fuse_constraints/reprojection_error_cost_functor.h

+ * 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.


Suggested change

* and x is the 2D ovservations.

* and x is the 2D observations.

svwilliams · 2023-12-19T22:32:24Z

fuse_constraints/include/fuse_constraints/reprojection_error_cost_functor.h

+ *
+ * @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


Suggested change

* @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)

svwilliams

I have some questions about how camera UUIDs should work. And I don't remember if there was anything of consequence from my previous review.

svwilliams · 2023-12-28T21:17:42Z

fuse_constraints/test/test_bal_problem.cpp

+// BALProblem adapted from:
+// https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/simple_bundle_adjuster.cc


Since this is using external code as a template, I'm holding all of my comments about raw pointers, etc. Just know that I am still thinking them in my head. 🙂

Hide the pain, push it down. Its what i did. :). I can leave a to-do note to rewrite this properly sometime in the future?

svwilliams · 2023-12-28T21:25:39Z

fuse_constraints/test/test_bal_problem.cpp

+ 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.


Suggested change

// 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.

svwilliams · 2023-12-28T21:38:00Z

fuse_constraints/test/test_bal_problem.cpp

+ // Define Observation Covariance
+ fuse_core::Matrix2d cov;
+ cov << 1e-5, 0.0, // NOLINT
+ 0.0, 1e-5; // NOLINT


The really small observation covariance was implemented because inside the reference implementation (bal_problem_ceres), the observations are assumed to be perfect?

I think so? At least the residuals are not being scaled here: https://github.com/ceres-solver/ceres-solver/blob/master/examples/snavely_reprojection_error.h and there is no-where that the pixel covariance comes into play.

svwilliams · 2023-12-28T21:48:03Z

fuse_variables/include/fuse_variables/base_camera.h

+ /**
+ * @brief Construct a pinhole camera variable given a camera id and intrinsic parameters
+ *
+ * @param[in] camera_id The id associated to a camera


The constructor has a uuid input as well. Doxygen is out of date.

svwilliams · 2023-12-28T22:13:57Z

fuse_variables/include/fuse_variables/base_camera.h

+ explicit BaseCamera(const fuse_core::UUID& uuid, const uint64_t& camera_id):
+ FixedSizeVariable<N>(uuid), id_(camera_id) {}


In the Point2DLandmark class, I have this constructor hidden from the public. The UUID is supposed to be generated by some function of the class name and the landmark ID. Maybe "hash" is a better description than "uuid"? But the idea was that every Landmark ID generated a unique UUID for fuse bookkeeping purposes. And the same landmark UUID should be generated regardless of which robot observed the landmark. For example, an Apriltag has an ID embedded in it. So regradless of whether Tag1 is observed by RobotA or RobotB, the same landmark variable should be used in both cases.

Whereas the Orientation2DStamped accepts a timestamp and an optional "device_id". Here the intent is that a device (robot is what I was thinking, but however someone wants to use the device) may move around over time. A device UUID is not sufficient to identify a variable uniquely in the fuse/Ceres system. But a device at a specific point in time should be unique. I.e. (DeviceA, Time=1.0) generates a different variable UUID than (DeviceA, Time=2.0) or (DeviceB, Time=1.0). This allows different devices/robots to have orientations are the same time without generating clashing UUIDs.

I'm not sure whether a BaseCamera is closer to a Landmark? Or closer to an Orientation?

The Camera variable contains the calibration values. We do not expect the calibration values to be time-varying. So we would want one Camera variable for every physical camera. E.g. (RobotA, CenterCamera) is one variable, (RobotA, RearCamera) is a second variable, and (RobotB, CenterCamera) is a third variable. But there is no timestamp component.

So you could does something like the Orientation, but instead of a timestamp to differentiate different variables from the same robot, you would have some sort of camera ID or camera position?

explicit Orientation2DStamped(const uint64_t& id, const fuse_core::UUID& device_id = fuse_core::uuid::NIL) : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), id, device_id)),

or

explicit Orientation2DStamped(const std::string& camera_name, const fuse_core::UUID& device_id = fuse_core::uuid::NIL) : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), camera_name, device_id)),

This is something that's been playing in the back of my mind. In a "standard" bundle adjustment, each cameras would also have a pose associated with it, which presumably varies over time in the case of SfM, so it would make sense for the camera object to include details of the pose and therefore time. However, this seems a bit inelegant. The graph would have a new camera constraint for every frame when the robot is moving. Instead, I chose to have the camera only represent intrinsics. That way, we can represent the pose of the robot as one variable, the cam->bot extrinsics by another (probably fixed) variable and the intrinsics by this. So basically, yes, I agree. I will change it to use the "orientation" with an ID instead of a stamp.

svwilliams · 2023-12-28T22:14:45Z

fuse_variables/include/fuse_variables/pinhole_camera.h

@@ -137,7 +138,7 @@ class PinholeCamera : public FixedSizeVariable<4>
 /**
 * @brief Read-only access to the id
 */
- const uint64_t& id() const { return id_; }
+ // const uint64_t& id() const { return id_; }


Remove commented code if it is not needed?

svwilliams · 2023-12-28T22:17:24Z

fuse_variables/include/fuse_variables/pinhole_camera_simple.h

+ * construction and dependent on a user input database id. As such, the database id cannot be altered after
+ * construction.
+ */
+class PinholeCameraSimple : public BaseCamera<3>


Is this a SnavellyCamera? Or RadialDistortionCamera?
Just trying to think of a better name than "Simple".

Both, I think...

Strictly speaking, its a radial distortion model. Snavelly make some assumptions about the coordiate frame on the image plane, but it is still a radial distortion model. I'll call it radial.

svwilliams · 2023-12-28T22:17:58Z

fuse_variables/include/fuse_variables/pinhole_camera_simple.h

+ /**
+ * @brief Construct a pinhole camera variable given a camera id and intrinsic parameters
+ *
+ * @param[in] camera_id The id associated to a camera


Doxygen does not list all constructor inputs

svwilliams · 2023-12-28T22:18:26Z

fuse_variables/include/fuse_variables/pinhole_camera_simple.h

+private:
+ // Allow Boost Serialization access to private methods
+ friend class boost::serialization::access;
+ // uint64_t id_ { 0 };


Remove commented code

needphpsql · 2024-02-24T00:47:17Z

fuse_constraints/src/fixed_3d_landmark_simple_covariance_constraint.cpp

+ return new ceres::AutoDiffCostFunction<Fixed3DLandmarkSimpleCovarianceCostFunctor, 8, 3, 4, 4>(
+ new Fixed3DLandmarkSimpleCovarianceCostFunctor(sqrt_information_, mean_, observations_, pts3d_));
+}


A question about the numbers here, 8, 3, 4, 4>? The first parameter is the dimension of the residues. If we look into the implementation of the CostFunctor inside ' Fixed3DLandmarkSimpleCovarianceCostFunctor', it will generate 2*N residules, where N is the number of observations. Should it be something dynamic or it doesn't matter?

oscarmendezm added 3 commits December 11, 2023 14:59

Adding python scripts to generate test cases.

5b22af0

Minor PR Fixes.

3a2a561

Improved vision constraints. Includes Snavelly,

dcc0ff1

Simple Pinhole and Fixed Landmark with Jacobian.

oscarmendezm assigned ayrton04 and svwilliams Dec 15, 2023

svwilliams self-requested a review December 15, 2023 17:01

oscarmendezm added 6 commits December 15, 2023 17:11

Adding simple covariance version which uses

5e9737e

2x2 matrix

Linting fixes for CI.

a5e3c8c

Applying simple covariance correctly,

a55be22

and fixing documentation

Small fixes for linting and testing.

bd3119a

More linting for CI errors.

f216d41

Fixing base_camera.h spacing.

822ab4a

svwilliams reviewed Dec 19, 2023

View reviewed changes

svwilliams self-requested a review December 19, 2023 22:44

svwilliams requested changes Dec 28, 2023

View reviewed changes

PR Review.

3188b48

needphpsql reviewed Feb 24, 2024

View reviewed changes

oscarmendezm added 2 commits April 8, 2024 19:33

Changing fixed3d_landmark to use a variable number of residuals.

e17b717

Fixing bug with number of residual calculation.

119ccf6

vinnnyr mentioned this pull request Jun 27, 2024

Visual Odometry in fuse? #371

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Vision constraints #352

Vision constraints #352

oscarmendezm commented Dec 15, 2023

svwilliams left a comment

svwilliams Dec 19, 2023

svwilliams Dec 19, 2023

oscarmendezm Jan 4, 2024

svwilliams Dec 19, 2023

oscarmendezm Jan 4, 2024

svwilliams Dec 19, 2023

oscarmendezm Jan 4, 2024

svwilliams Dec 19, 2023

oscarmendezm Jan 4, 2024

svwilliams Dec 19, 2023

svwilliams Dec 19, 2023

svwilliams left a comment

svwilliams Dec 28, 2023

oscarmendezm Jan 4, 2024

svwilliams Dec 28, 2023

svwilliams Dec 28, 2023

oscarmendezm Jan 4, 2024

svwilliams Dec 28, 2023

svwilliams Dec 28, 2023

oscarmendezm Jan 4, 2024

svwilliams Dec 28, 2023

svwilliams Dec 28, 2023

oscarmendezm Jan 4, 2024

svwilliams Dec 28, 2023

svwilliams Dec 28, 2023

needphpsql Feb 24, 2024

	// Get the covariance weigthing to point losses from a pose uncertainty
	// Get the covariance weighting to point losses from a pose uncertainty

		// [ (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]

		J << fx/gz, T(0), -fx * (gx / gz2), -fxgxyz, fx(T(1)+(gxgx)/gz2), -fx gy/gz,
		T(0), fy/gz, -fy * (gy / gz2), -fy(T(1)+(gygy)/gz2), fygxyz, fy gx/gz;

	* and x is the 2D ovservations.
	* and x is the 2D observations.

	* @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)

		// BALProblem adapted from:
		// https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/simple_bundle_adjuster.cc

	// 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.

		explicit BaseCamera(const fuse_core::UUID& uuid, const uint64_t& camera_id):
		FixedSizeVariable<N>(uuid), id_(camera_id) {}

Vision constraints #352

Are you sure you want to change the base?

Vision constraints #352

Conversation

oscarmendezm commented Dec 15, 2023

svwilliams left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

svwilliams left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment