Fixed print_ -> print
parent
7adc9898ad
commit
f04d8125ef
|
@ -31,7 +31,7 @@ def main():
|
||||||
- A measurement model with the correct dimensionality for the factor
|
- A measurement model with the correct dimensionality for the factor
|
||||||
"""
|
"""
|
||||||
prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
|
prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
|
||||||
prior.print_('goal angle')
|
prior.print('goal angle')
|
||||||
model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
|
model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
|
||||||
key = X(1)
|
key = X(1)
|
||||||
factor = gtsam.PriorFactorRot2(key, prior, model)
|
factor = gtsam.PriorFactorRot2(key, prior, model)
|
||||||
|
@ -48,7 +48,7 @@ def main():
|
||||||
"""
|
"""
|
||||||
graph = gtsam.NonlinearFactorGraph()
|
graph = gtsam.NonlinearFactorGraph()
|
||||||
graph.push_back(factor)
|
graph.push_back(factor)
|
||||||
graph.print_('full graph')
|
graph.print('full graph')
|
||||||
|
|
||||||
"""
|
"""
|
||||||
Step 3: Create an initial estimate
|
Step 3: Create an initial estimate
|
||||||
|
@ -65,7 +65,7 @@ def main():
|
||||||
"""
|
"""
|
||||||
initial = gtsam.Values()
|
initial = gtsam.Values()
|
||||||
initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20)))
|
initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20)))
|
||||||
initial.print_('initial estimate')
|
initial.print('initial estimate')
|
||||||
|
|
||||||
"""
|
"""
|
||||||
Step 4: Optimize
|
Step 4: Optimize
|
||||||
|
@ -77,7 +77,7 @@ def main():
|
||||||
with the final state of the optimization.
|
with the final state of the optimization.
|
||||||
"""
|
"""
|
||||||
result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
|
result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
|
||||||
result.print_('final result')
|
result.print('final result')
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
|
|
|
@ -10,8 +10,6 @@ A visualSLAM example for the structure-from-motion problem on a simulated datase
|
||||||
This version uses iSAM to solve the problem incrementally
|
This version uses iSAM to solve the problem incrementally
|
||||||
"""
|
"""
|
||||||
|
|
||||||
from __future__ import print_function
|
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import gtsam
|
import gtsam
|
||||||
from gtsam.examples import SFMdata
|
from gtsam.examples import SFMdata
|
||||||
|
@ -94,7 +92,7 @@ def main():
|
||||||
current_estimate = isam.estimate()
|
current_estimate = isam.estimate()
|
||||||
print('*' * 50)
|
print('*' * 50)
|
||||||
print('Frame {}:'.format(i))
|
print('Frame {}:'.format(i))
|
||||||
current_estimate.print_('Current estimate: ')
|
current_estimate.print('Current estimate: ')
|
||||||
|
|
||||||
# Clear the factor graph and values for the next iteration
|
# Clear the factor graph and values for the next iteration
|
||||||
graph.resize(0)
|
graph.resize(0)
|
||||||
|
|
|
@ -36,7 +36,7 @@ class GroundTruth:
|
||||||
self.cameras = [Pose3()] * nrCameras
|
self.cameras = [Pose3()] * nrCameras
|
||||||
self.points = [Point3(0, 0, 0)] * nrPoints
|
self.points = [Point3(0, 0, 0)] * nrPoints
|
||||||
|
|
||||||
def print_(self, s="") -> None:
|
def print(self, s="") -> None:
|
||||||
print(s)
|
print(s)
|
||||||
print("K = ", self.K)
|
print("K = ", self.K)
|
||||||
print("Cameras: ", len(self.cameras))
|
print("Cameras: ", len(self.cameras))
|
||||||
|
|
Loading…
Reference in New Issue