undo changes to tests to ensure backwards compatibility
Varun Agrawal
parent
b3635cc6ce
commit
55ce145bf7
|
|
@ -11,11 +11,12 @@ Refactored: Roderick Koehle
|
|||
"""
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam.symbol_shorthand import K, L, P
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
import gtsam
|
||||
|
||||
|
||||
def ulp(ftype=np.float64):
|
||||
"""
|
||||
|
|
@ -26,7 +27,7 @@ def ulp(ftype=np.float64):
|
|||
|
||||
|
||||
class TestCal3Fisheye(GtsamTestCase):
|
||||
|
||||
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
"""
|
||||
|
|
@ -50,10 +51,11 @@ class TestCal3Fisheye(GtsamTestCase):
|
|||
camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
|
||||
camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
|
||||
cls.origin = np.array([0.0, 0.0, 0.0])
|
||||
cls.poses = [pose1, pose2]
|
||||
cls.cameras = [camera1, camera2]
|
||||
cls.measurements = [k.project(cls.origin) for k in cls.cameras]
|
||||
|
||||
cls.poses = gtsam.Pose3Vector([pose1, pose2])
|
||||
cls.cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2])
|
||||
cls.measurements = gtsam.Point2Vector(
|
||||
[k.project(cls.origin) for k in cls.cameras])
|
||||
|
||||
def test_Cal3Fisheye(self):
|
||||
K = gtsam.Cal3Fisheye()
|
||||
self.assertEqual(K.fx(), 1.)
|
||||
|
|
@ -62,7 +64,7 @@ class TestCal3Fisheye(GtsamTestCase):
|
|||
def test_distortion(self):
|
||||
"""Fisheye distortion and rectification"""
|
||||
equidistant = gtsam.Cal3Fisheye()
|
||||
perspective_pt = self.obj_point[0:2]/self.obj_point[2]
|
||||
perspective_pt = self.obj_point[0:2] / self.obj_point[2]
|
||||
distorted_pt = equidistant.uncalibrate(perspective_pt)
|
||||
rectified_pt = equidistant.calibrate(distorted_pt)
|
||||
self.gtsamAssertEquals(distorted_pt, self.img_point)
|
||||
|
|
@ -166,7 +168,7 @@ class TestCal3Fisheye(GtsamTestCase):
|
|||
pose = gtsam.Pose3()
|
||||
camera = gtsam.Cal3Fisheye()
|
||||
state = gtsam.Values()
|
||||
camera_key, pose_key, landmark_key = K(0), P(0), L(0)
|
||||
pose_key, landmark_key = P(0), L(0)
|
||||
state.insert_point3(landmark_key, obj_point)
|
||||
state.insert_pose3(pose_key, pose)
|
||||
g = gtsam.NonlinearFactorGraph()
|
||||
|
|
|
|||
|
|
@ -10,11 +10,12 @@ Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
|||
"""
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam.symbol_shorthand import K, L, P
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
import gtsam
|
||||
|
||||
|
||||
class TestCal3Unified(GtsamTestCase):
|
||||
|
||||
|
|
@ -47,9 +48,10 @@ class TestCal3Unified(GtsamTestCase):
|
|||
camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic)
|
||||
camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic)
|
||||
cls.origin = np.array([0.0, 0.0, 0.0])
|
||||
cls.poses = [pose1, pose2]
|
||||
cls.cameras = [camera1, camera2]
|
||||
cls.measurements = [k.project(cls.origin) for k in cls.cameras]
|
||||
cls.poses = gtsam.Pose3Vector([pose1, pose2])
|
||||
cls.cameras = gtsam.CameraSetCal3Unified([camera1, camera2])
|
||||
cls.measurements = gtsam.Point2Vector(
|
||||
[k.project(cls.origin) for k in cls.cameras])
|
||||
|
||||
def test_Cal3Unified(self):
|
||||
K = gtsam.Cal3Unified()
|
||||
|
|
@ -106,7 +108,7 @@ class TestCal3Unified(GtsamTestCase):
|
|||
state = gtsam.Values()
|
||||
measured = self.img_point
|
||||
noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
|
||||
camera_key, pose_key, landmark_key = K(0), P(0), L(0)
|
||||
camera_key, pose_key, landmark_key = K(0), P(0), L(0)
|
||||
k = self.stereographic
|
||||
state.insert_cal3unified(camera_key, k)
|
||||
state.insert_pose3(pose_key, gtsam.Pose3())
|
||||
|
|
@ -141,7 +143,7 @@ class TestCal3Unified(GtsamTestCase):
|
|||
Dcal = np.zeros((2, 10), order='F')
|
||||
Dp = np.zeros((2, 2), order='F')
|
||||
camera.calibrate(img_point, Dcal, Dp)
|
||||
|
||||
|
||||
self.gtsamAssertEquals(Dcal, np.array(
|
||||
[[ 0., 0., 0., -1., 0., 0., 0., 0., 0., 0.],
|
||||
[ 0., 0., 0., 0., -1., 0., 0., 0., 0., 0.]]))
|
||||
|
|
@ -157,7 +159,7 @@ class TestCal3Unified(GtsamTestCase):
|
|||
|
||||
def test_triangulation_rectify(self):
|
||||
"""Estimate spatial point from image measurements using rectification"""
|
||||
rectified = [k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]
|
||||
rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)])
|
||||
shared_cal = gtsam.Cal3_S2()
|
||||
triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
|
||||
self.gtsamAssertEquals(triangulated, self.origin)
|
||||
|
|
|
|||
|
|
@ -23,14 +23,14 @@ class TestDsfTrackGenerator(GtsamTestCase):
|
|||
kps_i1 = Keypoints(np.array([[50.0, 60], [70, 80], [90, 100]]))
|
||||
kps_i2 = Keypoints(np.array([[110.0, 120], [130, 140]]))
|
||||
|
||||
keypoints_list = []
|
||||
keypoints_list = gtsam.KeypointsVector()
|
||||
keypoints_list.append(kps_i0)
|
||||
keypoints_list.append(kps_i1)
|
||||
keypoints_list.append(kps_i2)
|
||||
|
||||
# For each image pair (i1,i2), we provide a (K,2) matrix
|
||||
# of corresponding image indices (k1,k2).
|
||||
matches_dict = {}
|
||||
matches_dict = gtsam.MatchIndicesMap()
|
||||
matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]])
|
||||
matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]])
|
||||
|
||||
|
|
@ -86,7 +86,7 @@ class TestSfmTrack2d(GtsamTestCase):
|
|||
|
||||
def test_sfm_track_2d_constructor(self) -> None:
|
||||
"""Test construction of 2D SfM track."""
|
||||
measurements = []
|
||||
measurements = gtsam.SfmMeasurementVector()
|
||||
measurements.append((0, Point2(10, 20)))
|
||||
track = SfmTrack2d(measurements=measurements)
|
||||
track.measurement(0)
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
|
|||
# that will be sent to the smoothers
|
||||
new_factors = gtsam.NonlinearFactorGraph()
|
||||
new_values = gtsam.Values()
|
||||
new_timestamps = {}
|
||||
new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap()
|
||||
|
||||
# Create a prior on the first pose, placing it at the origin
|
||||
prior_mean = gtsam.Pose2(0, 0, 0)
|
||||
|
|
@ -48,7 +48,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
|
|||
gtsam.PriorFactorPose2(
|
||||
X1, prior_mean, prior_noise))
|
||||
new_values.insert(X1, prior_mean)
|
||||
new_timestamps[X1] = 0.0
|
||||
new_timestamps.insert((X1, 0.0))
|
||||
|
||||
delta_time = 0.25
|
||||
time = 0.25
|
||||
|
|
@ -78,7 +78,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
|
|||
current_key = int(1000 * time)
|
||||
|
||||
# assign current key to the current timestamp
|
||||
new_timestamps[current_key] = time
|
||||
new_timestamps.insert((current_key, time))
|
||||
|
||||
# Add a guess for this pose to the new values
|
||||
# Assume that the robot moves at 2 m/s. Position is time[s] *
|
||||
|
|
|
|||
|
|
@ -14,11 +14,12 @@ from __future__ import print_function
|
|||
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam.symbol_shorthand import X
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
import gtsam
|
||||
|
||||
|
||||
def create_graph():
|
||||
"""Create a basic linear factor graph for testing"""
|
||||
|
|
@ -98,7 +99,7 @@ class TestGaussianFactorGraph(GtsamTestCase):
|
|||
bn = gfg.eliminateSequential(ordering)
|
||||
self.assertEqual(bn.size(), 3)
|
||||
|
||||
keyVector = []
|
||||
keyVector = gtsam.KeyVector()
|
||||
keyVector.append(keys[2])
|
||||
#TODO(Varun) Below code causes segfault in Debug config
|
||||
# ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(gfg, keyVector)
|
||||
|
|
|
|||
|
|
@ -13,11 +13,12 @@ Author: Frank Dellaert
|
|||
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam import Rot3
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
import gtsam
|
||||
from gtsam import Rot3
|
||||
|
||||
KEY = 0
|
||||
MODEL = gtsam.noiseModel.Unit.Create(3)
|
||||
|
||||
|
|
@ -29,9 +30,11 @@ R = Rot3.Expmap(np.array([0.1, 0, 0]))
|
|||
class TestKarcherMean(GtsamTestCase):
|
||||
|
||||
def test_find(self):
|
||||
# Check that optimizing for Karcher mean (which minimizes Between distance)
|
||||
# gets correct result.
|
||||
rotations = [R, R.inverse()]
|
||||
"""
|
||||
Check that optimizing for Karcher mean (which minimizes Between distance)
|
||||
gets correct result.
|
||||
"""
|
||||
rotations = gtsam.Rot3Vector([R, R.inverse()])
|
||||
expected = Rot3()
|
||||
actual = gtsam.FindKarcherMean(rotations)
|
||||
self.gtsamAssertEquals(expected, actual)
|
||||
|
|
@ -42,7 +45,7 @@ class TestKarcherMean(GtsamTestCase):
|
|||
a2Rb2 = Rot3()
|
||||
a3Rb3 = Rot3()
|
||||
|
||||
aRb_list = [a1Rb1, a2Rb2, a3Rb3]
|
||||
aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3])
|
||||
aRb_expected = Rot3()
|
||||
|
||||
aRb = gtsam.FindKarcherMean(aRb_list)
|
||||
|
|
@ -58,7 +61,9 @@ class TestKarcherMean(GtsamTestCase):
|
|||
graph = gtsam.NonlinearFactorGraph()
|
||||
R12 = R.compose(R.compose(R))
|
||||
graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
|
||||
keys = [1, 2]
|
||||
keys = gtsam.KeyVector()
|
||||
keys.append(1)
|
||||
keys.append(2)
|
||||
graph.add(gtsam.KarcherMeanFactorRot3(keys))
|
||||
|
||||
initial = gtsam.Values()
|
||||
|
|
@ -67,7 +72,9 @@ class TestKarcherMean(GtsamTestCase):
|
|||
expected = Rot3()
|
||||
|
||||
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
|
||||
actual = gtsam.FindKarcherMean([result.atRot3(1), result.atRot3(2)])
|
||||
actual = gtsam.FindKarcherMean(
|
||||
gtsam.Rot3Vector([result.atRot3(1),
|
||||
result.atRot3(2)]))
|
||||
self.gtsamAssertEquals(expected, actual)
|
||||
self.gtsamAssertEquals(
|
||||
R12, result.atRot3(1).between(result.atRot3(2)))
|
||||
|
|
|
|||
|
|
@ -12,9 +12,10 @@ import math
|
|||
import unittest
|
||||
|
||||
import numpy as np
|
||||
from gtsam import Point2, Pose2
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
from gtsam import Point2, Point2Pairs, Pose2
|
||||
|
||||
|
||||
class TestPose2(GtsamTestCase):
|
||||
"""Test selected Pose2 methods."""
|
||||
|
|
@ -82,7 +83,7 @@ class TestPose2(GtsamTestCase):
|
|||
]
|
||||
|
||||
# fmt: on
|
||||
ab_pairs = list(zip(pts_a, pts_b))
|
||||
ab_pairs = Point2Pairs(list(zip(pts_a, pts_b)))
|
||||
aTb = Pose2.Align(ab_pairs)
|
||||
self.assertIsNotNone(aTb)
|
||||
|
||||
|
|
|
|||
|
|
@ -12,11 +12,12 @@ Author: Frank Dellaert, Duy Nguyen Ta
|
|||
import math
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam import Point3, Pose3, Rot3
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
import gtsam
|
||||
from gtsam import Point3, Pose3, Rot3
|
||||
|
||||
|
||||
def numerical_derivative_pose(pose, method, delta=1e-5):
|
||||
jacobian = np.zeros((6, 6))
|
||||
|
|
@ -222,7 +223,7 @@ class TestPose3(GtsamTestCase):
|
|||
sTt = Pose3(Rot3.Rodrigues(0, 0, -math.pi), Point3(2, 4, 0))
|
||||
transformed = sTt.transformTo(square)
|
||||
|
||||
st_pairs = []
|
||||
st_pairs = gtsam.Point3Pairs()
|
||||
for j in range(4):
|
||||
st_pairs.append((square[:,j], transformed[:,j]))
|
||||
|
||||
|
|
|
|||
|
|
@ -12,12 +12,13 @@ Author: Frank Dellaert
|
|||
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
import gtsam
|
||||
from gtsam import (BetweenFactorPose2, LevenbergMarquardtParams, Pose2, Rot2,
|
||||
ShonanAveraging2, ShonanAveraging3,
|
||||
ShonanAveragingParameters2, ShonanAveragingParameters3)
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
DEFAULT_PARAMS = ShonanAveragingParameters3(
|
||||
gtsam.LevenbergMarquardtParams.CeresDefaults()
|
||||
|
|
@ -176,7 +177,7 @@ class TestShonanAveraging(GtsamTestCase):
|
|||
shonan_params.setCertifyOptimality(True)
|
||||
|
||||
noise_model = gtsam.noiseModel.Unit.Create(3)
|
||||
between_factors = []
|
||||
between_factors = gtsam.BetweenFactorPose2s()
|
||||
for (i1, i2), i2Ri1 in i2Ri1_dict.items():
|
||||
i2Ti1 = Pose2(i2Ri1, np.zeros(2))
|
||||
between_factors.append(
|
||||
|
|
|
|||
|
|
@ -12,9 +12,10 @@ Author: John Lambert
|
|||
import unittest
|
||||
|
||||
import numpy as np
|
||||
from gtsam import Pose2, Rot2, Similarity2
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
from gtsam import Pose2, Pose2Pairs, Rot2, Similarity2
|
||||
|
||||
|
||||
class TestSim2(GtsamTestCase):
|
||||
"""Test selected Sim2 methods."""
|
||||
|
|
@ -46,7 +47,7 @@ class TestSim2(GtsamTestCase):
|
|||
|
||||
wToi_list = [wTo0, wTo1, wTo2]
|
||||
|
||||
we_pairs = list(zip(wToi_list, eToi_list))
|
||||
we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
|
||||
|
||||
# Recover the transformation wSe (i.e. world_S_egovehicle)
|
||||
wSe = Similarity2.Align(we_pairs)
|
||||
|
|
@ -81,7 +82,7 @@ class TestSim2(GtsamTestCase):
|
|||
|
||||
wToi_list = [wTo0, wTo1, wTo2]
|
||||
|
||||
we_pairs = list(zip(wToi_list, eToi_list))
|
||||
we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
|
||||
|
||||
# Recover the transformation wSe (i.e. world_S_egovehicle)
|
||||
wSe = Similarity2.Align(we_pairs)
|
||||
|
|
@ -119,7 +120,7 @@ class TestSim2(GtsamTestCase):
|
|||
|
||||
bTi_list = [bTi0, bTi1, bTi2, bTi3]
|
||||
|
||||
ab_pairs = list(zip(aTi_list, bTi_list))
|
||||
ab_pairs = Pose2Pairs(list(zip(aTi_list, bTi_list)))
|
||||
|
||||
# Recover the transformation wSe (i.e. world_S_egovehicle)
|
||||
aSb = Similarity2.Align(ab_pairs)
|
||||
|
|
|
|||
|
|
@ -12,11 +12,12 @@ Author: John Lambert
|
|||
import unittest
|
||||
from typing import List, Optional
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam import Point3, Pose3, Rot3, Similarity3
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
import gtsam
|
||||
from gtsam import Point3, Pose3, Rot3, Similarity3
|
||||
|
||||
|
||||
class TestSim3(GtsamTestCase):
|
||||
"""Test selected Sim3 methods."""
|
||||
|
|
@ -48,7 +49,7 @@ class TestSim3(GtsamTestCase):
|
|||
|
||||
wToi_list = [wTo0, wTo1, wTo2]
|
||||
|
||||
we_pairs = list(zip(wToi_list, eToi_list))
|
||||
we_pairs = gtsam.Pose3Pairs(list(zip(wToi_list, eToi_list)))
|
||||
|
||||
# Recover the transformation wSe (i.e. world_S_egovehicle)
|
||||
wSe = Similarity3.Align(we_pairs)
|
||||
|
|
@ -83,7 +84,7 @@ class TestSim3(GtsamTestCase):
|
|||
|
||||
wToi_list = [wTo0, wTo1, wTo2]
|
||||
|
||||
we_pairs = list(zip(wToi_list, eToi_list))
|
||||
we_pairs = gtsam.Pose3Pairs(list(zip(wToi_list, eToi_list)))
|
||||
|
||||
# Recover the transformation wSe (i.e. world_S_egovehicle)
|
||||
wSe = Similarity3.Align(we_pairs)
|
||||
|
|
@ -121,7 +122,7 @@ class TestSim3(GtsamTestCase):
|
|||
|
||||
bTi_list = [bTi0, bTi1, bTi2, bTi3]
|
||||
|
||||
ab_pairs = list(zip(aTi_list, bTi_list))
|
||||
ab_pairs = gtsam.Pose3Pairs(list(zip(aTi_list, bTi_list)))
|
||||
|
||||
# Recover the transformation wSe (i.e. world_S_egovehicle)
|
||||
aSb = Similarity3.Align(ab_pairs)
|
||||
|
|
@ -688,7 +689,7 @@ def align_poses_sim3(aTi_list: List[Pose3], bTi_list: List[Pose3]) -> Similarity
|
|||
assert len(aTi_list) == len(bTi_list)
|
||||
assert n_to_align >= 2, "SIM(3) alignment uses at least 2 frames"
|
||||
|
||||
ab_pairs = list(zip(aTi_list, bTi_list))
|
||||
ab_pairs = gtsam.Pose3Pairs(list(zip(aTi_list, bTi_list)))
|
||||
|
||||
aSb = Similarity3.Align(ab_pairs)
|
||||
|
||||
|
|
|
|||
|
|
@ -12,13 +12,14 @@ Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert
|
|||
import unittest
|
||||
from typing import Iterable, List, Optional, Tuple, Union
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
import gtsam
|
||||
from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2,
|
||||
CameraSetCal3Bundler, PinholeCameraCal3_S2,
|
||||
PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3,
|
||||
TriangulationParameters, TriangulationResult)
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2)
|
||||
|
||||
|
|
@ -34,7 +35,9 @@ class TestTriangulationExample(GtsamTestCase):
|
|||
# create second camera 1 meter to the right of first camera
|
||||
pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
|
||||
# twoPoses
|
||||
self.poses = [pose1, pose2]
|
||||
self.poses = gtsam.Pose3Vector()
|
||||
self.poses.append(pose1)
|
||||
self.poses.append(pose2)
|
||||
|
||||
# landmark ~5 meters infront of camera
|
||||
self.landmark = Point3(5, 0.5, 1.2)
|
||||
|
|
@ -64,7 +67,7 @@ class TestTriangulationExample(GtsamTestCase):
|
|||
cameras = camera_set()
|
||||
else:
|
||||
cameras = []
|
||||
measurements = []
|
||||
measurements = gtsam.Point2Vector()
|
||||
|
||||
for k, pose in zip(cal_params, self.poses):
|
||||
K = calibration(*k)
|
||||
|
|
@ -93,7 +96,7 @@ class TestTriangulationExample(GtsamTestCase):
|
|||
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
|
||||
|
||||
# Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
|
||||
measurements_noisy = []
|
||||
measurements_noisy = gtsam.Point2Vector()
|
||||
measurements_noisy.append(measurements[0] - np.array([0.1, 0.5]))
|
||||
measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3]))
|
||||
|
||||
|
|
@ -160,8 +163,8 @@ class TestTriangulationExample(GtsamTestCase):
|
|||
z2: Point2 = camera2.project(landmark)
|
||||
z3: Point2 = camera3.project(landmark)
|
||||
|
||||
poses = [pose1, pose2, pose3]
|
||||
measurements = [z1, z2, z3]
|
||||
poses = gtsam.Pose3Vector([pose1, pose2, pose3])
|
||||
measurements = gtsam.Point2Vector([z1, z2, z3])
|
||||
|
||||
# noise free, so should give exactly the landmark
|
||||
actual = gtsam.triangulatePoint3(poses,
|
||||
|
|
@ -226,7 +229,7 @@ class TestTriangulationExample(GtsamTestCase):
|
|||
cameras.append(camera1)
|
||||
cameras.append(camera2)
|
||||
|
||||
measurements = []
|
||||
measurements = gtsam.Point2Vector()
|
||||
measurements.append(z1)
|
||||
measurements.append(z2)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue