perf(linear): compute JacobianFactor::gradient() directly, skip HessianFactor conversion

[jashshah999]

Problem

JacobianFactor::gradient(Key, VectorValues) converts the entire factor to a HessianFactor to compute a single gradient entry (flagged by an existing TODO at line 881):

Vector JacobianFactor::gradient(Key key, const VectorValues& x) const {
  // TODO: optimize it for JacobianFactor without converting to a HessianFactor
  HessianFactor hessian(*this);
  return hessian.gradient(key, x);
}

The HessianFactor constructor computes A'A which is O(n*m^2) for an n-row, m-column Jacobian. This is unnecessary when only one key's gradient is needed.

Fix

Compute the gradient directly:

1. Evaluate unwhitened residual e = A*x - b by iterating over Jacobian blocks — O(n*m)
2. Apply noise model: e = Sigma^{-1} * e (double whiten)
3. Return A_k' * e for the requested key — O(n*d_k)

Total cost: O(nm) vs O(nm^2) for the old path. No heap allocation for the HessianFactor.

Tests

Two new tests in testJacobianFactor.cpp:

• gradient: isotropic noise model, verifies numerical equivalence with the HessianFactor path
• gradient_no_noise: no noise model (unit covariance), same verification

[Copilot]

Pull request overview

Note

Copilot was unable to run its full agentic suite in this review.

This PR optimizes JacobianFactor::gradient(Key, VectorValues) by computing a single-key gradient directly from Jacobian blocks, avoiding conversion to HessianFactor and its AᵀA construction cost.

Changes:

• Reimplemented JacobianFactor::gradient() to compute the residual and project onto a requested key’s Jacobian block.
• Added unit tests comparing the new path to the existing HessianFactor-based gradient for isotropic and no-noise cases.

Reviewed changes

Copilot reviewed 2 out of 2 changed files in this pull request and generated 4 comments.

File	Description
gtsam/linear/JacobianFactor.cpp	Replaces HessianFactor conversion with direct residual/gradient computation.
gtsam/linear/tests/testJacobianFactor.cpp	Adds tests validating gradient equivalence against HessianFactor.

[Copilot]

If key is not present in this factor, find(key) may return end(), making it - begin() equal to size() and causing out-of-bounds access on Ab_. The previous implementation delegated to HessianFactor::gradient, which likely had defined behavior for missing keys. Add an explicit check for it == end() and match the prior behavior (e.g., return an empty vector or throw the same exception type/message as before).

Suggested change

	const Factor::const_iterator it = find(key);
	const Factor::const_iterator it = find(key);
	if (it == end())
	throw std::out_of_range("JacobianFactor::gradient: requested key is not present in this factor");

[Copilot]

The added tests cover isotropic noise and the no-noise case, but they do not cover a non-diagonal Gaussian noise model where whitenInPlace() uses a non-symmetric sqrt-information factor. Since the new implementation’s correctness depends on applying RᵀR (not R²), add a test using a full covariance / general Gaussian noise model (e.g., a 2x2 non-diagonal covariance) and compare to the HessianFactor path; this will catch regressions in the noise-weighted gradient computation.

[Copilot]

The added tests cover isotropic noise and the no-noise case, but they do not cover a non-diagonal Gaussian noise model where whitenInPlace() uses a non-symmetric sqrt-information factor. Since the new implementation’s correctness depends on applying RᵀR (not R²), add a test using a full covariance / general Gaussian noise model (e.g., a 2x2 non-diagonal covariance) and compare to the HessianFactor path; this will catch regressions in the noise-weighted gradient computation.

Suggested change

	/* ************************************************************************* */
	/* ************************************************************************* */
	TEST(JacobianFactor, gradient_full_gaussian_noise)
	{
	// Test gradient with a full non-diagonal Gaussian noise model. This exercises
	// whitening with a non-symmetric sqrt-information factor, where correctness
	// depends on applying R'R rather than R^2.
	Matrix A1 = (Matrix(2, 2) << 1.0, 2.0, -0.5, 3.0).finished();
	Matrix A2 = (Matrix(2, 2) << -4.0, 0.25, 1.5, 2.0).finished();
	Vector b = Vector2(0.3, -1.2);
	Matrix covariance = (Matrix(2, 2) << 2.0, 0.3, 0.3, 1.0).finished();
	auto noise = noiseModel::Gaussian::Covariance(covariance);
	JacobianFactor lf(1, A1, 2, A2, b, noise);
	VectorValues x;
	x.insert(1, Vector2(0.5, -1.0));
	x.insert(2, Vector2(2.0, 1.5));
	HessianFactor hf(lf);
	EXPECT(assert_equal(hf.gradient(1, x), lf.gradient(1, x), 1e-9));
	EXPECT(assert_equal(hf.gradient(2, x), lf.gradient(2, x), 1e-9));
	}
	/* ************************************************************************* */

[jashshah999]

Pushed a fix for the noise model handling. The previous version applied whitenInPlace twice, computing R*R*e instead of R^T*R*e. These differ when R is non-symmetric (any non-diagonal covariance). Now whitens both A_k and e once: (R*A_k)^T * (R*e) = A_k^T * R^T*R * e.

Also added a bounds check for missing keys and a test with non-diagonal (Gaussian) covariance that would have caught this.