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

Modernize/format

release/4.3a0
Frank Dellaert 2024-10-26 10:19:13 -07:00
parent 5eb858b729
commit a8a229c10c
1 changed files with 21 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
python/gtsam/examples/SFMExample.py
View File

@ -9,20 +9,22 @@ See LICENSE for the license information
A structure-from-motion problem on a simulated dataset A structure-from-motion problem on a simulated dataset
""" """
import gtsam
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import numpy as np import numpy as np
import gtsam
from gtsam import symbol_shorthand from gtsam import symbol_shorthand
L = symbol_shorthand.L L = symbol_shorthand.L
X = symbol_shorthand.X X = symbol_shorthand.X
from gtsam.examples import SFMdata from gtsam.examples import SFMdata
from gtsam import (Cal3_S2, DoglegOptimizer,
GenericProjectionFactorCal3_S2, Marginals,
NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values)
from gtsam.utils import plot from gtsam.utils import plot
from gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2,
Marginals, NonlinearFactorGraph, PinholeCameraCal3_S2,
PriorFactorPoint3, PriorFactorPose3, Values)
def main(): def main():
""" """
@ -43,7 +45,7 @@ def main():
Finally, once all of the factors have been added to our factor graph, we will want to Finally, once all of the factors have been added to our factor graph, we will want to
solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
GTSAM includes several nonlinear optimizers to perform this step. Here we will use a GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
trust-region method known as Powell's Degleg trust-region method known as Powell's Dogleg
The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
nonlinear functions around an initial linearization point, then solve the linear system nonlinear functions around an initial linearization point, then solve the linear system
@ -78,8 +80,7 @@ def main():
camera = PinholeCameraCal3_S2(pose, K) camera = PinholeCameraCal3_S2(pose, K)
for j, point in enumerate(points): for j, point in enumerate(points):
measurement = camera.project(point) measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2( factor = GenericProjectionFactorCal3_S2(measurement, measurement_noise, X(i), L(j), K)
measurement, measurement_noise, X(i), L(j), K)
graph.push_back(factor) graph.push_back(factor)
# Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
@ -88,28 +89,29 @@ def main():
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise) factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor) graph.push_back(factor)
graph.print('Factor Graph:\n') graph.print("Factor Graph:\n")
# Create the data structure to hold the initial estimate to the solution # Create the data structure to hold the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth # Intentionally initialize the variables off from the ground truth
initial_estimate = Values() initial_estimate = Values()
rng = np.random.default_rng()
for i, pose in enumerate(poses): for i, pose in enumerate(poses):
transformed_pose = pose.retract(0.1*np.random.randn(6,1)) transformed_pose = pose.retract(0.1 * rng.standard_normal(6).reshape(6, 1))
initial_estimate.insert(X(i), transformed_pose) initial_estimate.insert(X(i), transformed_pose)
for j, point in enumerate(points): for j, point in enumerate(points):
transformed_point = point + 0.1*np.random.randn(3) transformed_point = point + 0.1 * rng.standard_normal(3)
initial_estimate.insert(L(j), transformed_point) initial_estimate.insert(L(j), transformed_point)
initial_estimate.print('Initial Estimates:\n') initial_estimate.print("Initial Estimates:\n")
# Optimize the graph and print results # Optimize the graph and print results
params = gtsam.DoglegParams() params = gtsam.DoglegParams()
params.setVerbosity('TERMINATION') params.setVerbosity("TERMINATION")
optimizer = DoglegOptimizer(graph, initial_estimate, params) optimizer = DoglegOptimizer(graph, initial_estimate, params)
print('Optimizing:') print("Optimizing:")
result = optimizer.optimize() result = optimizer.optimize()
result.print('Final results:\n') result.print("Final results:\n")
print('initial error = {}'.format(graph.error(initial_estimate))) print("initial error = {}".format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result))) print("final error = {}".format(graph.error(result)))
marginals = Marginals(graph, result) marginals = Marginals(graph, result)
plot.plot_3d_points(1, result, marginals=marginals) plot.plot_3d_points(1, result, marginals=marginals)
@ -117,5 +119,6 @@ def main():
plot.set_axes_equal(1) plot.set_axes_equal(1)
plt.show() plt.show()
if __name__ == '__main__':
if __name__ == "__main__":
main() main()