12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add prior, use calibrations

release/4.3a0
Frank Dellaert 2024-10-28 15:58:48 -07:00
parent 60ce938e31
commit bbba497ca1
1 changed files with 29 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
python/gtsam/examples/ViewGraphComparison.py
View File

@ -71,7 +71,7 @@ def compute_ground_truth(method, poses, cal):
E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1]))
E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2]))
# Assert that E1.matrix and F1 are the same
# Assert that E1.matrix and F1 are the same, with known calibration
invK = np.linalg.inv(cal.K())
G1 = invK.transpose() @ E1.matrix() @ invK
G2 = invK.transpose() @ E2.matrix() @ invK
@ -94,6 +94,10 @@ def build_factor_graph(method, num_cameras, measurements):
FactorClass = gtsam.TransferFactorFundamentalMatrix
elif method == "EssentialMatrix":
FactorClass = gtsam.EssentialTransferFactorCal3f
# add priors on all calibrations:
for i in range(num_cameras):
model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
graph.addPriorCal3f(K(i), Cal3f(50.0, 50.0, 50.0), model)
else:
raise ValueError(f"Unknown method {method}")
@ -120,7 +124,7 @@ def build_factor_graph(method, num_cameras, measurements):
return graph
def get_initial_estimate(method, num_cameras, ground_truth, cal, std=1e-6):
def get_initial_estimate(method, num_cameras, ground_truth, cal):
"""get initial estimate for method"""
initialEstimate = Values()
total_dimension = 0
@ -130,22 +134,16 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal, std=1e-6):
for a in range(num_cameras):
b = (a + 1) % num_cameras # Next camera
c = (a + 2) % num_cameras # Camera after next
# Retract with delta drawn from zero-mean normal
F1_perturbed = F1.retract(np.random.normal(0, std, F1.dim()))
F2_perturbed = F2.retract(np.random.normal(0, 1e-4, F2.dim()))
initialEstimate.insert(EdgeKey(a, b).key(), F1_perturbed)
initialEstimate.insert(EdgeKey(a, c).key(), F2_perturbed)
initialEstimate.insert(EdgeKey(a, b).key(), F1)
initialEstimate.insert(EdgeKey(a, c).key(), F2)
total_dimension += F1.dim() + F2.dim()
elif method == "EssentialMatrix":
E1, E2 = ground_truth
for a in range(num_cameras):
b = (a + 1) % num_cameras # Next camera
c = (a + 2) % num_cameras # Camera after next
# Retract with delta drawn from zero-mean normal
E1_perturbed = E1.retract(np.random.normal(0, std, E1.dim()))
E2_perturbed = E2.retract(np.random.normal(0, 1e-4, E2.dim()))
initialEstimate.insert(EdgeKey(a, b).key(), E1_perturbed)
initialEstimate.insert(EdgeKey(a, c).key(), E2_perturbed)
initialEstimate.insert(EdgeKey(a, b).key(), E1)
initialEstimate.insert(EdgeKey(a, c).key(), E2)
total_dimension += E1.dim() + E2.dim()
# Insert initial calibrations
for i in range(num_cameras):
@ -158,14 +156,17 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal, std=1e-6):
return initialEstimate
def optimize(graph, initialEstimate):
def optimize(graph, initialEstimate, method):
"""optimize the graph"""
params = LevenbergMarquardtParams()
params.setlambdaInitial(1000.0) # Initialize lambda to a high value
params.setAbsoluteErrorTol(1.0)
params.setlambdaInitial(1e10) # Initialize lambda to a high value
params.setlambdaUpperBound(1e10)
# params.setAbsoluteErrorTol(0.1)
params.setVerbosityLM("SUMMARY")
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params)
result = optimizer.optimize()
# if method == "EssentialMatrix":
# result.print("Final results:\n", formatter)
return result
@ -195,9 +196,15 @@ def compute_distances(method, result, ground_truth, num_cameras, cal):
elif method == "EssentialMatrix":
E_est_ab = result.atEssentialMatrix(key_ab)
E_est_ac = result.atEssentialMatrix(key_ac)
# Retrieve correct calibrations from result:
cal_a = result.atCal3f(K(a))
cal_b = result.atCal3f(K(b))
cal_c = result.atCal3f(K(c))
# Convert estimated EssentialMatrices to FundamentalMatrices
F_est_ab = gtsam.FundamentalMatrix(cal.K(), E_est_ab, cal.K())
F_est_ac = gtsam.FundamentalMatrix(cal.K(), E_est_ac, cal.K())
F_est_ab = gtsam.FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K())
F_est_ac = gtsam.FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K())
# Compute local coordinates (geodesic distance on the F-manifold)
dist_ab = np.linalg.norm(F1.localCoordinates(F_est_ab))
@ -240,7 +247,7 @@ def main():
parser.add_argument("--num_cameras", type=int, default=4, help="Number of cameras (default: 4)")
parser.add_argument("--nr_trials", type=int, default=5, help="Number of trials (default: 5)")
parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)")
parser.add_argument("--noise_std", type=float, default=1.0, help="Standard deviation of noise (default: 1.0)")
parser.add_argument("--noise_std", type=float, default=0.5, help="Standard deviation of noise (default: 0.5)")
args = parser.parse_args()
# Initialize the random number generator
@ -273,17 +280,14 @@ def main():
graph = build_factor_graph(method, args.num_cameras, measurements)
# Assert that the initial error is the same for both methods:
if trial == 0 and method == methods[0]:
initial_error = graph.error(initial_estimate[method])
if method == methods[0]:
error0 = graph.error(initial_estimate[method])
else:
current_error = graph.error(initial_estimate[method])
if not np.allclose(initial_error, current_error):
print(f"Initial error for {method} ({current_error}) does not match ")
print(f"initial error for {methods[0]} ({initial_error})")
exit(1)
assert np.allclose(error0, current_error)
# Optimize the graph
result = optimize(graph, initial_estimate[method])
result = optimize(graph, initial_estimate[method], method)
# Compute distances from ground truth
distances = compute_distances(method, result, ground_truth[method], args.num_cameras, cal)