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

sort imports in python examples

release/4.3a0
Varun Agrawal 2020-07-27 21:25:44 -05:00
parent 95d3582c2e
commit 95b77f7634
4 changed files with 20 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
python/gtsam_py/python/examples/ImuFactorExample.py
View File

@ -15,14 +15,12 @@ from __future__ import print_function
import argparse import argparse
import math import math
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import gtsam import gtsam
import matplotlib.pyplot as plt
import numpy as np
from gtsam.gtsam.symbol_shorthand import B, V, X from gtsam.gtsam.symbol_shorthand import B, V, X
from gtsam.utils.plot import plot_pose3 from gtsam.utils.plot import plot_pose3
from mpl_toolkits.mplot3d import Axes3D
from PreintegrationExample import POSES_FIG, PreintegrationExample from PreintegrationExample import POSES_FIG, PreintegrationExample

7
python/gtsam_py/python/examples/ImuFactorISAM2Example.py
View File

@ -8,11 +8,9 @@ from __future__ import print_function
import math import math
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import numpy as np
import gtsam import gtsam
import matplotlib.pyplot as plt
import numpy as np
from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
ConstantTwistScenario, ImuFactor, NonlinearFactorGraph, ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
PinholeCameraCal3_S2, Point3, Pose3, PinholeCameraCal3_S2, Point3, Pose3,
@ -20,6 +18,7 @@ from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
PriorFactorVector, Rot3, Values) PriorFactorVector, Rot3, Values)
from gtsam.gtsam.symbol_shorthand import B, V, X from gtsam.gtsam.symbol_shorthand import B, V, X
from gtsam.utils import plot from gtsam.utils import plot
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
def vector3(x, y, z): def vector3(x, y, z):

5
python/gtsam_py/python/examples/PreintegrationExample.py
View File

@ -10,12 +10,11 @@ A script validating the Preintegration of IMU measurements
import math import math
import gtsam
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import numpy as np import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import gtsam
from gtsam.utils.plot import plot_pose3 from gtsam.utils.plot import plot_pose3
from mpl_toolkits.mplot3d import Axes3D
IMU_FIG = 1 IMU_FIG = 1
POSES_FIG = 2 POSES_FIG = 2

20
python/gtsam_py/python/examples/VisualISAMExample.py
View File

@ -16,9 +16,9 @@ import numpy as np
import gtsam import gtsam
from gtsam.examples import SFMdata from gtsam.examples import SFMdata
from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
NonlinearFactorGraph, NonlinearISAM, Pose3, NonlinearFactorGraph, NonlinearISAM, Pose3,
PriorFactorPoint3, PriorFactorPose3, Rot3, PriorFactorPoint3, PriorFactorPose3, Rot3,
SimpleCamera, Values, Point3) PinholeCameraCal3_S2, Values, Point3)
def symbol(name: str, index: int) -> int: def symbol(name: str, index: int) -> int:
@ -37,7 +37,8 @@ def main():
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model # Define the camera observation noise model
camera_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v camera_noise = gtsam.noiseModel.Isotropic.Sigma(
2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks # Create the set of ground-truth landmarks
points = SFMdata.createPoints() points = SFMdata.createPoints()
@ -54,15 +55,17 @@ def main():
# Loop over the different poses, adding the observations to iSAM incrementally # Loop over the different poses, adding the observations to iSAM incrementally
for i, pose in enumerate(poses): for i, pose in enumerate(poses):
camera = SimpleCamera(pose, K) camera = PinholeCameraCal3_S2(pose, K)
# Add factors for each landmark observation # Add factors for each landmark observation
for j, point in enumerate(points): for j, point in enumerate(points):
measurement = camera.project(point) measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, symbol('x', i), symbol('l', j), K) factor = GenericProjectionFactorCal3_S2(
measurement, camera_noise, symbol('x', i), symbol('l', j), K)
graph.push_back(factor) graph.push_back(factor)
# Intentionally initialize the variables off from the ground truth # Intentionally initialize the variables off from the ground truth
noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), t=Point3(0.05, -0.10, 0.20)) noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25),
t=Point3(0.05, -0.10, 0.20))
initial_xi = pose.compose(noise) initial_xi = pose.compose(noise)
# Add an initial guess for the current pose # Add an initial guess for the current pose
@ -74,7 +77,8 @@ def main():
# adding it to iSAM. # adding it to iSAM.
if i == 0: if i == 0:
# Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z # Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise) factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise)
graph.push_back(factor) graph.push_back(factor)