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

Switch to using Cal3f

release/4.3a0
Frank Dellaert 2024-10-26 23:53:20 -07:00
parent 0477d20695
commit b9dc9ae459
5 changed files with 41 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
examples/EssentialViewGraphExample.cpp
View File

@ -16,7 +16,7 @@
* @date October 2024
*/
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3f.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h>
@ -40,7 +40,7 @@ using namespace symbol_shorthand; // For K(symbol)
// Main function
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2 K_initial(50.0, 50.0, 0.0, 50.0, 50.0);
Cal3f cal(50.0, 50.0, 50.0);
// Create the set of 8 ground-truth landmarks
vector<Point3> points = createPoints();
@ -55,7 +55,7 @@ int main(int argc, char* argv[]) {
// Simulate measurements from each camera pose
std::array<std::array<Point2, 8>, 4> p;
for (size_t i = 0; i < 4; ++i) {
PinholeCamera<Cal3_S2> camera(poses[i], K_initial);
PinholeCamera<Cal3f> camera(poses[i], cal);
for (size_t j = 0; j < 8; ++j) {
p[i][j] = camera.project(points[j]);
}
@ -63,7 +63,7 @@ int main(int argc, char* argv[]) {
// Create the factor graph
NonlinearFactorGraph graph;
using Factor = EssentialTransferFactor<Cal3_S2>;
using Factor = EssentialTransferFactor<Cal3f>;
for (size_t a = 0; a < 4; ++a) {
size_t b = (a + 1) % 4; // Next camera
@ -113,7 +113,7 @@ int main(int argc, char* argv[]) {
// Insert initial calibrations (using K symbol)
for (size_t i = 0; i < 4; ++i) {
initialEstimate.insert(K(i), K_initial);
initialEstimate.insert(K(i), cal);
}
initialEstimate.print("Initial Estimates:\n", formatter);

12
python/gtsam/examples/EssentialViewGraphExample.py
View File

@ -18,10 +18,10 @@ import numpy as np
from gtsam.examples import SFMdata
import gtsam
from gtsam import Cal3_S2, EdgeKey, EssentialMatrix
from gtsam import EssentialTransferFactorCal3_S2 as Factor
from gtsam import Cal3f, EdgeKey, EssentialMatrix
from gtsam import EssentialTransferFactorCal3f as Factor
from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
NonlinearFactorGraph, PinholeCameraCal3_S2, Values)
NonlinearFactorGraph, PinholeCameraCal3f, Values)
# For symbol shorthand (e.g., X(0), L(1))
K = gtsam.symbol_shorthand.K
@ -39,7 +39,7 @@ def formatter(key):
def main():
# Define the camera calibration parameters
K_initial = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
cal = Cal3f(50.0, 50.0, 50.0)
# Create the set of 8 ground-truth landmarks
points = SFMdata.createPoints()
@ -54,7 +54,7 @@ def main():
# Simulate measurements from each camera pose
p = [[None for _ in range(8)] for _ in range(4)]
for i in range(4):
camera = PinholeCameraCal3_S2(poses[i], K_initial)
camera = PinholeCameraCal3f(poses[i], cal)
for j in range(8):
p[i][j] = camera.project(points[j])
@ -95,7 +95,7 @@ def main():
# Insert initial calibrations
for i in range(4):
initialEstimate.insert(K(i), K_initial)
initialEstimate.insert(K(i), cal)
# Optimize the graph and print results
params = LevenbergMarquardtParams()

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

@ -6,26 +6,19 @@
Date: October 2024
"""
import argparse
import matplotlib.pyplot as plt
import numpy as np
from gtsam.examples import SFMdata
import gtsam
import argparse
from gtsam import (
Cal3_S2,
EdgeKey,
EssentialMatrix,
FundamentalMatrix,
LevenbergMarquardtOptimizer,
LevenbergMarquardtParams,
NonlinearFactorGraph,
PinholeCameraCal3_S2,
Values,
)
from gtsam import (Cal3f, EdgeKey, EssentialMatrix, FundamentalMatrix,
LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
NonlinearFactorGraph, PinholeCameraCal3f, Values)
# For symbol shorthand (e.g., K(0), K(1))
K_sym = gtsam.symbol_shorthand.K
K = gtsam.symbol_shorthand.K
# Methods to compare
methods = ["FundamentalMatrix", "EssentialMatrix"]
@ -44,7 +37,7 @@ def formatter(key):
# Function to simulate data
def simulate_data(num_cameras):
# Define the camera calibration parameters
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
cal = Cal3f(50.0, 50.0, 50.0)
# Create the set of 8 ground-truth landmarks
points = SFMdata.createPoints()
@ -55,18 +48,18 @@ def simulate_data(num_cameras):
# Simulate measurements from each camera pose
measurements = [[None for _ in range(len(points))] for _ in range(num_cameras)]
for i in range(num_cameras):
camera = PinholeCameraCal3_S2(poses[i], K)
camera = PinholeCameraCal3f(poses[i], cal)
for j in range(len(points)):
measurements[i][j] = camera.project(points[j])
return points, poses, measurements, K
return points, poses, measurements, cal
# Function to compute ground truth matrices
def compute_ground_truth_matrices(method, poses, K):
def compute_ground_truth(method, poses, cal):
if method == "FundamentalMatrix":
F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K)
F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K)
F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K())
F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.K())
return F1, F2
elif method == "EssentialMatrix":
E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1]))
@ -84,8 +77,8 @@ def build_factor_graph(method, num_cameras, measurements):
# Use TransferFactorFundamentalMatrix
FactorClass = gtsam.TransferFactorFundamentalMatrix
elif method == "EssentialMatrix":
# Use EssentialTransferFactorCal3_S2
FactorClass = gtsam.EssentialTransferFactorCal3_S2
# Use EssentialTransferFactorCal3f
FactorClass = gtsam.EssentialTransferFactorCal3f
else:
raise ValueError(f"Unknown method {method}")
@ -113,7 +106,7 @@ def build_factor_graph(method, num_cameras, measurements):
# Function to get initial estimates
def get_initial_estimate(method, num_cameras, ground_truth, K):
def get_initial_estimate(method, num_cameras, ground_truth, cal):
initialEstimate = Values()
if method == "FundamentalMatrix":
@ -132,7 +125,7 @@ def get_initial_estimate(method, num_cameras, ground_truth, K):
initialEstimate.insert(EdgeKey(a, c).key(), E2)
# Insert initial calibrations
for i in range(num_cameras):
initialEstimate.insert(K_sym(i), K)
initialEstimate.insert(K(i), cal)
else:
raise ValueError(f"Unknown method {method}")
@ -150,7 +143,7 @@ def optimize(graph, initialEstimate):
# Function to compute distances from ground truth
def compute_distances(method, result, ground_truth, num_cameras, K):
def compute_distances(method, result, ground_truth, num_cameras, cal):
distances = []
if method == "FundamentalMatrix":
@ -158,8 +151,8 @@ def compute_distances(method, result, ground_truth, num_cameras, K):
elif method == "EssentialMatrix":
E1, E2 = ground_truth
# Convert ground truth EssentialMatrices to FundamentalMatrices using GTSAM method
F1 = gtsam.FundamentalMatrix(K, E1, K)
F2 = gtsam.FundamentalMatrix(K, E2, K)
F1 = gtsam.FundamentalMatrix(cal.K(), E1, cal.K())
F2 = gtsam.FundamentalMatrix(cal.K(), E2, cal.K())
else:
raise ValueError(f"Unknown method {method}")
@ -176,8 +169,8 @@ def compute_distances(method, result, ground_truth, num_cameras, K):
E_est_ab = result.atEssentialMatrix(key_ab)
E_est_ac = result.atEssentialMatrix(key_ac)
# Convert estimated EssentialMatrices to FundamentalMatrices using GTSAM method
F_est_ab = gtsam.FundamentalMatrix(K, E_est_ab, K)
F_est_ac = gtsam.FundamentalMatrix(K, E_est_ac, K)
F_est_ab = gtsam.FundamentalMatrix(cal.K(), E_est_ab, cal.K())
F_est_ac = gtsam.FundamentalMatrix(cal.K(), E_est_ac, cal.K())
else:
raise ValueError(f"Unknown method {method}")
@ -229,22 +222,22 @@ def main():
print(f"Running method: {method}")
# Simulate data
points, poses, measurements, K = simulate_data(args.num_cameras)
points, poses, measurements, cal = simulate_data(args.num_cameras)
# Compute ground truth matrices
ground_truth = compute_ground_truth_matrices(method, poses, K)
ground_truth = compute_ground_truth(method, poses, cal)
# Build the factor graph
graph = build_factor_graph(method, args.num_cameras, measurements)
# Get initial estimates
initialEstimate = get_initial_estimate(method, args.num_cameras, ground_truth, K)
initialEstimate = get_initial_estimate(method, args.num_cameras, ground_truth, cal)
# Optimize the graph
result = optimize(graph, initialEstimate)
# Compute distances from ground truth
distances = compute_distances(method, result, ground_truth, args.num_cameras, K)
distances = compute_distances(method, result, ground_truth, args.num_cameras, cal)
# Compute final error
final_error = graph.error(result)

8
python/gtsam/examples/ViewGraphExample.py
View File

@ -32,7 +32,7 @@ def formatter(key):
def main():
# Define the camera calibration parameters
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
cal = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Create the set of 8 ground-truth landmarks
points = SFMdata.createPoints()
@ -41,13 +41,13 @@ def main():
poses = SFMdata.posesOnCircle(4, 30)
# Calculate ground truth fundamental matrices, 1 and 2 poses apart
F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K)
F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K)
F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K())
F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.K())
# Simulate measurements from each camera pose
p = [[None for _ in range(8)] for _ in range(4)]
for i in range(4):
camera = PinholeCameraCal3_S2(poses[i], K)
camera = PinholeCameraCal3_S2(poses[i], cal)
for j in range(8):
p[i][j] = camera.project(points[j])