def setUpClass(cls):
"""
Equidistant fisheye projection
An equidistant fisheye projection with focal length f is defined
as the relation r/f = tan(theta), with r being the radius in the
image plane and theta the incident angle of the object point.
"""
x, y, z = 1.0, 0.0, 1.0
u, v = np.arctan2(x, z), 0.0
cls.obj_point = np.array([x, y, z])
cls.img_point = np.array([u, v])
p1 = [-1.0, 0.0, -1.0]
p2 = [1.0, 0.0, -1.0]
q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
pose1 = gtsam.Pose3(q1, p1)
pose2 = gtsam.Pose3(q2, p2)
camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
cls.origin = np.array([0.0, 0.0, 0.0])
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 setUpClass(cls):
"""
Stereographic fisheye projection
An equidistant fisheye projection with focal length f is defined
as the relation r/f = 2*tan(theta/2), with r being the radius in the
image plane and theta the incident angle of the object point.
"""
x, y, z = 1.0, 0.0, 1.0
r = np.linalg.norm([x, y, z])
u, v = 2*x/(z+r), 0.0
cls.obj_point = np.array([x, y, z])
cls.img_point = np.array([u, v])
fx, fy, s, u0, v0 = 2, 2, 0, 0, 0
k1, k2, p1, p2 = 0, 0, 0, 0
xi = 1
cls.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi)
p1 = [-1.0, 0.0, -1.0]
p2 = [ 1.0, 0.0, -1.0]
q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
pose1 = gtsam.Pose3(q1, p1)
pose2 = gtsam.Pose3(q2, p2)
camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic)
camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic)
cls.origin = np.array([0.0, 0.0, 0.0])
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 circlePose3(numPoses=8, radius=1.0, symbolChar='\0'):
"""
circlePose3 generates a set of poses in a circle. This function
returns those poses inside a gtsam.Values object, with sequential
keys starting from 0. An optional character may be provided, which
will be stored in the msb of each key (i.e. gtsam.Symbol).
We use aerospace/navlab convention, X forward, Y right, Z down
First pose will be at (R,0,0)
^y ^ X
| |
z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer)
Vehicle at p0 is looking towards y axis (X-axis points towards world y)
"""
values = gtsam.Values()
theta = 0.0
dtheta = 2 * pi / numPoses
gRo = gtsam.Rot3(
np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F'))
for i in range(numPoses):
key = gtsam.symbol(symbolChar, i)
gti = gtsam.Point3(radius * cos(theta), radius * sin(theta), 0)
oRi = gtsam.Rot3.Yaw(
-theta) # negative yaw goes counterclockwise, with Z down !
gTi = gtsam.Pose3(gRo.compose(oRi), gti)
values.insert(key, gTi)
theta = theta + dtheta
return values
def test_find(self):
# Check that optimizing for Karcher mean (which minimizes Between distance)
# gets correct result.
rotations = {R, R.inverse()}
expected = gtsam.Rot3()
actual = find_Karcher_mean_Rot3(rotations)
self.gtsamAssertEquals(expected, actual)
def sim3_transform_map(result, nrCameras, nrPoints):
# Similarity transform
s_poses = []
s_points = []
_sim3 = sim3.Similarity3()
for i in range(nrCameras):
pose_i = result.atPose3(symbol(ord('x'), i))
s_poses.append(pose_i)
for j in range(nrPoints):
point_j = result.atPoint3(symbol(ord('p'), j))
s_points.append(point_j)
theta = np.radians(30)
wRc = gtsam.Rot3(np.array([[-math.sin(theta), 0, math.cos(theta)],
[-math.cos(theta), 0, -math.sin(theta)],
[0, 1, 0]]))
d_pose1 = gtsam.Pose3(
wRc, gtsam.Point3(-math.sin(theta)*1.58, -math.cos(theta)*1.58, 1.2))
d_pose2 = gtsam.Pose3(
wRc, gtsam.Point3(-math.sin(theta)*1.58*3, -math.cos(theta)*1.58*3, 1.2))
pose_pairs = [[s_poses[2], d_pose2], [s_poses[0], d_pose1]]
_sim3.align_pose(pose_pairs)
print('R', _sim3._R)
print('t', _sim3._t)
print('s', _sim3._s)
s_map = (s_poses, s_points)
actual_poses, actual_points = _sim3.map_transform(s_map)
d_map = _sim3.map_transform(s_map)
return _sim3, d_map
def setUp(self):
self.testclouda = np.array([[1], [1], [1]])
self.testcloudb = np.array([[2, 10], [1, 1], [1, 1]])
self.testcloudc = np.array([[2], [1], [1]])
self.testbTa = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0))
self.testcloudd = np.array([[0, 20, 10], [0, 10, 20], [0, 0, 0]])
self.testcloude = np.array([[10, 30, 20], [10, 20, 30], [0, 0, 0]])
def create_past_poses(self, delta):
"""Create the set of ground-truth poses of the last time step."""
for i, y in enumerate([0, 2.5, 5, 7.5, 10]):
wRc = gtsam.Rot3(
np.array([[0 + delta, 1 + delta, 0 + delta],
[0 + delta, 0 - delta, -1 + delta],
[1 - delta, 0 + delta, 0 - delta]]).T)
self.past_poses.append(
gtsam.Pose3(wRc, gtsam.Point3(0, y - delta, 1.5)))
def ExampleValues():
T = []
T.append(gtsam.Point3(np.array([3.14, 1.59, 2.65])))
T.append(gtsam.Point3(np.array([-1.0590e+00, -3.6017e-02, -1.5720e+00])))
T.append(gtsam.Point3(np.array([8.5034e+00, 6.7499e+00, -3.6383e+00])))
data = gtsam.Values()
for i in range(len(T)):
data.insert(i, gtsam.Pose3(gtsam.Rot3(), T[i]))
return data
def find_Karcher_mean_Rot3(rotations):
"""Find the Karcher mean of given values."""
# Cost function C(R) = \sum PriorFactor(R_i)::error(R)
# No closed form solution.
graph = gtsam.NonlinearFactorGraph()
for R in rotations:
graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
initial = gtsam.Values()
initial.insert(KEY, gtsam.Rot3())
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
return result.atRot3(KEY)
def setUp(cls):
test_clouds = read_ply(*scans_fnames)[:6]
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
ICP_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
test_graph = gtsam.NonlinearFactorGraph()
test_initial_estimates = gtsam.Values()
initial_pose = gtsam.Pose3(gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955, -0.0221840, -0.0116519, 0.0228910, 0.9996701),
gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633))
test_graph.add(gtsam.PriorFactorPose3(0, initial_pose, PRIOR_NOISE))
test_initial_estimates.insert(0, initial_pose)
test_graph, test_initial_estimates = populate_factor_graph(test_graph, test_initial_estimates, initial_pose, test_clouds)
cls.graph = test_graph
cls.initial_estimates = test_initial_estimates
def test_insertProjectionFactors(self):
"""Test insertProjectionFactors."""
graph = gtsam.NonlinearFactorGraph()
gtsam.utilities.insertProjectionFactors(
graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]),
gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2())
self.assertEqual(graph.size(), 2)
graph = gtsam.NonlinearFactorGraph()
gtsam.utilities.insertProjectionFactors(
graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]),
gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2(),
gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0)))
self.assertEqual(graph.size(), 2)
def setUp(self):
"""Set up two constraints to test."""
aTb = gtsam.Pose3(gtsam.Rot3.Yaw(np.pi / 2), gtsam.Point3(1, 2, 3))
cov = gtsam.noiseModel.Isotropic.Variance(6, 1e-3).covariance()
counts = np.zeros((5, 5))
counts[0][0] = 200
self.a_constraint_b = Constraint(1, 2, aTb, cov, counts)
aTc = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.5, 0.5, 0.5))
variances = np.array([1e-4, 1e-5, 1e-6, 1e-1, 1e-2, 1e-3])
aCOVc = gtsam.noiseModel.Diagonal.Variances(variances).covariance()
counts = np.zeros((5, 5))
counts[1][2] = 100
self.a_constraint_c = Constraint(1, 3, aTc, aCOVc, counts)
def horn_align(trajectory1, trajectory2):
""" Aligns trajectory1 with trajectory2 using HORN """
xyz1 = np.matrix([
p.translation().vector() for p in trajectory1.data()
]).transpose()
xyz2 = np.matrix([
p.translation().vector() for p in trajectory2.data()
]).transpose()
R, T, trans_error = Evaluation._horn_align(xyz1, xyz2)
transform = gtsam.Pose3(gtsam.Rot3(R), gtsam.Point3(
np.array(T)[:, 0])) # T = [[x],[y],[z]] [:,0] = tranpose()[0]
warped_trajectory = trajectory1.applyTransform(transform)
# print('trans_error: ', np.sqrt(np.dot(trans_error,trans_error) / len(trans_error)))
return warped_trajectory
def bounds_3D(self, box3D, pose, calibration, color=(255, 0, 255)):
# convert 3D box to set of 8 points
b = box3D.vector()
points_3D = np.array([[x, y, z] for x in b[0:2] for y in b[2:4]
for z in b[4:6]])
# ensure points infront of camera
for point in points_3D:
if pose.between(gtsam.Pose3(
gtsam.Rot3(),
gtsam.Point3(point))).translation().vector()[-1] < 0:
return
# project 3D points to 2D
P = quadricslam.QuadricCamera.transformToImage(pose, calibration)
points_3DTH = np.concatenate(
(points_3D.T, np.ones((1, points_3D.shape[0]))))
points_2D = P.dot(points_3DTH)
points_2D = points_2D[0:2, :] / points_2D[2]
points_2D = points_2D.transpose()
# only draw if all points project correctly
if np.any(np.isinf(points_2D)) or np.any(np.isnan(points_2D)):
return
# draw points
for point in points_2D:
cv2.circle(self._image,
tuple(point.astype('int')),
1,
color=(0, 0, 255),
thickness=-1,
lineType=cv2.LINE_AA)
# draw lines
for index in [(0, 1), (1, 3), (3, 2), (2, 0), (4, 5), (5, 7), (7, 6),
(6, 4), (2, 6), (1, 5), (0, 4), (3, 7)]:
p1 = tuple(points_2D[index[0]].astype('int'))
p2 = tuple(points_2D[index[1]].astype('int'))
cv2.line(self._image,
p1,
p2,
color=color,
thickness=1,
lineType=cv2.LINE_AA)
def add_sfm_factors(graph, init_est, dataset_path, scale):
f = open(dataset_path + '/reconstruction.json', 'r')
data = json.load(f)
images = data[0]['shots']
for img in images.items():
#print('Adding sfm factor for ' + img[0])
img_num = int(img[0].split('.')[0])
pos = np.array(img[1]['translation']) / scale
rot_aa = np.array(img[1]['rotation']) #axis-angle
rot_gtsam = gtsam.Rot3((Rotation.from_rotvec(rot_aa)).as_dcm())
pos = (Rotation.from_rotvec(rot_aa)).inv().as_dcm() @ pos
pos[2] = -pos[2]
trans_gtsam = gtsam.Point3(pos)
pose_gtsam = gtsam.Pose3(rot_gtsam, trans_gtsam)
factor = gtsam.PriorFactorPose3(C(img_num), pose_gtsam, sfm_noise)
graph.push_back(factor)
init_est.insert(C(img_num), pose_gtsam)
def get_rotation_from_axis(axis, angle):
"""
Compute the matrix that rotates around `axis` by `angle` radians.
See https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle.
axis:
np.ndarray, shape == (3,)
angle:
float
return:
gtsam.Rot3
"""
axis = axis / np.linalg.norm(axis)
return gtsam.Rot3(
np.cos(angle) * np.eye(3) + \
np.sin(angle) * gtsam.Unit3(gtsam.Point3(axis)).skew() + \
(1 - np.cos(angle)) * np.outer(axis, axis)
)
def sequence1():
""" a manually generated dataset sequence """
points = []
points.append(gtsam.Point3(10, 0, 0))
points.append(gtsam.Point3(0, -10, 0))
points.append(gtsam.Point3(-10, 0, 0))
points.append(gtsam.Point3(0, 10, 0))
points.append(gtsam.Point3(10, 0, 0))
quadrics = []
quadrics.append(
quadricslam.ConstrainedDualQuadric(gtsam.Pose3(),
np.array([0.2, 0.3, 0.4])))
quadrics.append(
quadricslam.ConstrainedDualQuadric(
gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.2, 0.2, 0.2)),
np.array([0.2, 0.3, 0.4])))
sequence = SimulatedSequence(points, quadrics)
return sequence
def add_sfm_factors_gt(graph, init_est, dataset_path):
f = open(dataset_path + '/ep_data.pkl', 'rb')
data = pickle.load(f)
for ind in range(len(data['cam_pose'])):
pose_mat = np.eye(4)
quat = data['cam_pose'][ind][3:]
quat[0], quat[1], quat[2], quat[3] = quat[1], quat[2], quat[3], quat[0]
pose_mat[:3, :3] = (
Rotation.from_quat(quat) *
Rotation.from_euler('xyz', [0, np.pi, 0])).as_dcm()
pose_mat[:3, 3] = data['cam_pose'][ind][:3]
pos = pose_mat[:3, 3]
rot_gtsam = gtsam.Rot3(pose_mat[:3, :3])
trans_gtsam = gtsam.Point3(pos)
pose_gtsam = gtsam.Pose3(rot_gtsam, trans_gtsam)
factor = gtsam.PriorFactorPose3(C(ind), pose_gtsam, sfm_noise)
graph.push_back(factor)
init_est.insert(C(ind), pose_gtsam)
def add_pose_factors(graph, init_est, dataset_path):
f = open(dataset_path + '/obj_det_poses.pkl', 'rb')
data = pickle.load(f)
cnt = 0
for pose in data.items():
#print('Adding pose det factor for ' + pose[0])
num = int(pose[0].split('.')[0])
pos = gtsam.Point3(pose[1][:3])
quat = pose[1][3:]
#wxyz to xyzw
quat[0], quat[1], quat[2], quat[3] = quat[1], quat[2], quat[3], quat[0]
rot = gtsam.Rot3((Rotation.from_quat(quat)).as_dcm())
pose = gtsam.Pose3(rot, pos)
factor = gtsam.BetweenFactorPose3(C(num), O(0), pose, pose_noise)
print('new factor')
graph.push_back(factor)
cnt += 1
init_est.insert(O(0), gtsam.Pose3()) #identity
def test_PriorWeights(self):
lmParams = gtsam.LevenbergMarquardtParams.CeresDefaults()
params = ShonanAveragingParameters3(lmParams)
self.assertAlmostEqual(0, params.getAnchorWeight(), 1e-9)
self.assertAlmostEqual(1, params.getKarcherWeight(), 1e-9)
self.assertAlmostEqual(0, params.getGaugesWeight(), 1e-9)
alpha, beta, gamma = 100.0, 200.0, 300.0
params.setAnchorWeight(alpha)
params.setKarcherWeight(beta)
params.setGaugesWeight(gamma)
self.assertAlmostEqual(alpha, params.getAnchorWeight(), 1e-9)
self.assertAlmostEqual(beta, params.getKarcherWeight(), 1e-9)
self.assertAlmostEqual(gamma, params.getGaugesWeight(), 1e-9)
params.setKarcherWeight(0)
shonan = fromExampleName("Klaus3.g2o", params)
initial = gtsam.Values()
for i in range(3):
initial.insert(i, gtsam.Rot3())
self.assertAlmostEqual(3.0756, shonan.cost(initial), places=3)
result, _lambdaMin = shonan.run(initial, 3, 3)
self.assertAlmostEqual(0.0015, shonan.cost(result), places=3)
def sim3_transform_map(self, result, img_pose_id_list, landmark_id_list):
# Similarity transform
s_poses = []
s_points = []
_sim3 = sim3.Similarity3()
theta = np.radians(30)
wRc = gtsam.Rot3(
np.array([[-math.sin(theta), 0,
math.cos(theta)],
[-math.cos(theta), 0, -math.sin(theta)], [0, 1, 0]]))
pose_pairs = []
for i, idx in enumerate(img_pose_id_list):
pose_i = result.atPose3(symbol(ord('x'), idx))
s_poses.append(pose_i)
if i < 2:
d_pose = gtsam.Pose3(
wRc,
gtsam.Point3(-math.sin(theta) * 1.58 * idx,
-math.cos(theta) * 1.58 * idx, 1.2))
pose_pairs.append([s_poses[i], d_pose])
i += 1
for idx in landmark_id_list:
point_j = result.atPoint3(symbol(ord('p'), idx))
s_points.append(point_j)
_sim3.align_pose(pose_pairs)
print('R', _sim3._R)
print('t', _sim3._t)
print('s', _sim3._s)
s_map = (s_poses, s_points)
actual_poses, actual_points = _sim3.map_transform(s_map)
d_map = _sim3.map_transform(s_map)
return _sim3, d_map
Note: This cell runs your ICP implementation 180 times. If you've implemented your ICP similarly to the TAs, expect this cell to take 2 minutes. If you're missing the `initial_transform` argument for icp, it may take ~1 hour.
"""
# load in all clouds in our dataset
clouds = read_ply(*scans_fnames)
# Setting up our factor graph
graph = gtsam.NonlinearFactorGraph()
initial_estimates = gtsam.Values()
# We get the initial pose of the car from Argo AI's dataset, and we add it to the graph as such
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(
np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
initial_pose = gtsam.Pose3(
gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955,
-0.0221840, -0.0116519, 0.0228910, 0.9996701),
gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633))
graph.add(gtsam.PriorFactorPose3(0, initial_pose, PRIOR_NOISE))
initial_estimates.insert(0, initial_pose)
# We'll use your function to populate the factor graph
graph, initial_estimates = populate_factor_graph(graph, initial_estimates,
initial_pose, clouds)
# Now optimize for the states
parameters = gtsam.GaussNewtonParams()
parameters.setRelativeErrorTol(1e-5)
parameters.setMaxIterations(100)
optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimates, parameters)
result = optimizer.optimize()
"""Let's plot these poses to see how our vechicle moves.
"""
This file is not a real python unittest. It contains small experiments
to test the wrapper with gtsam_test, a short version of gtsam.h.
Its name convention is different from other tests so it won't be discovered.
"""
import gtsam
import numpy as np
r = gtsam.Rot3()
print(r)
print(r.pitch())
r2 = gtsam.Rot3()
r3 = r.compose(r2)
print("r3 pitch:", r3.pitch())
v = np.array([1, 1, 1])
print("v = ", v)
r4 = r3.retract(v)
print("r4 pitch:", r4.pitch())
r4.print_(b'r4: ')
r3.print_(b"r3: ")
v = r3.localCoordinates(r4)
print("localCoordinates:", v)
Rmat = np.array([[0.990074, -0.0942928, 0.104218],
[0.104218, 0.990074, -0.0942928],
[-0.0942928, 0.104218, 0.990074]])
r5 = gtsam.Rot3(Rmat)
r5.print_(b"r5: ")
def test_StereoVOExample(self):
## Assumptions
# - For simplicity this example is in the camera's coordinate frame
# - X: right, Y: down, Z: forward
# - Pose x1 is at the origin, Pose 2 is 1 meter forward (along Z-axis)
# - x1 is fixed with a constraint, x2 is initialized with noisy values
# - No noise on measurements
## Create keys for variables
x1 = symbol('x', 1)
x2 = symbol('x', 2)
l1 = symbol('l', 1)
l2 = symbol('l', 2)
l3 = symbol('l', 3)
## Create graph container and add factors to it
graph = gtsam.NonlinearFactorGraph()
## add a constraint on the starting pose
first_pose = gtsam.Pose3()
graph.add(gtsam.NonlinearEqualityPose3(x1, first_pose))
## Create realistic calibration and measurement noise model
# format: fx fy skew cx cy baseline
K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)
stereo_model = gtsam.noiseModel.Diagonal.Sigmas(
np.array([1.0, 1.0, 1.0]))
## Add measurements
# pose 1
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(520, 480, 440),
stereo_model, x1, l1, K))
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(120, 80, 440),
stereo_model, x1, l2, K))
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 280, 140),
stereo_model, x1, l3, K))
#pose 2
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(570, 520, 490),
stereo_model, x2, l1, K))
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(70, 20, 490),
stereo_model, x2, l2, K))
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 270, 115),
stereo_model, x2, l3, K))
## Create initial estimate for camera poses and landmarks
initialEstimate = gtsam.Values()
initialEstimate.insert(x1, first_pose)
# noisy estimate for pose 2
initialEstimate.insert(
x2, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1, -.1, 1.1)))
expected_l1 = gtsam.Point3(1, 1, 5)
initialEstimate.insert(l1, expected_l1)
initialEstimate.insert(l2, gtsam.Point3(-1, 1, 5))
initialEstimate.insert(l3, gtsam.Point3(0, -.5, 5))
## optimize
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
result = optimizer.optimize()
## check equality for the first pose and point
pose_x1 = result.atPose3(x1)
self.gtsamAssertEquals(pose_x1, first_pose, 1e-4)
point_l1 = result.atPoint3(l1)
self.gtsamAssertEquals(point_l1, expected_l1, 1e-4)
def bundle_adjustment(self):
MIN_LANDMARK_SEEN = 3
# Initialize factor Graph
graph = gtsam.NonlinearFactorGraph()
initialEstimate = gtsam.Values()
# Add factors for all measurements
measurementNoiseSigma = 1.0
measurementNoise = gtsam.noiseModel_Isotropic.Sigma(
2, measurementNoiseSigma)
landmark_id_list = {}
img_pose_id_list = {}
for i, img_pose in enumerate(self.img_pose):
for k in range(len(img_pose.kp)):
if img_pose.kp_3d_exist(k):
landmark_id = img_pose.kp_3d(k)
if (self.landmark[landmark_id].seen >= MIN_LANDMARK_SEEN):
landmark_id_list[landmark_id] = True
img_pose_id_list[i] = True
graph.add(
gtsam.GenericProjectionFactorCal3_S2(
Point2(img_pose.kp[k][0],
img_pose.kp[k][1]), measurementNoise,
X(i), P(landmark_id), self.cal))
# Initialize estimate for poses
# Create priors
s = np.radians(60)
poseNoiseSigmas = np.array([s, s, s, 10, 10, 10])
# poseNoiseSigmas = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas)
# theta = np.radians(30)
# wRc = gtsam.Rot3(np.array([[-math.sin(theta), 0 , math.cos(theta)],
# [-math.cos(theta), 0 , -math.sin(theta)],
# [0, 1 , 0]]))
wRc = gtsam.Rot3(np.identity(3))
for i, idx in enumerate(img_pose_id_list):
initialEstimate.insert(X(idx), self.img_pose[idx].T)
if i < 2:
# graph.add(gtsam.PriorFactorPose3(X(idx), gtsam.Pose3(
# wRc, gtsam.Point3(-math.sin(theta)*1.58*idx, -math.cos(theta)*1.58*idx, 1.2)), posePriorNoise))
graph.add(
gtsam.PriorFactorPose3(
X(idx),
gtsam.Pose3(wRc,
gtsam.Point3(1.38 * idx, 0, -0.29 * idx)),
posePriorNoise))
# i+=1
# Initialize estimate for landmarks
for idx in landmark_id_list:
p = self.landmark[idx].point
initialEstimate.insert(P(idx), Point3(p[0], p[1], p[2]))
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
# sfm_result = optimizer.optimize()
for i in range(1000):
optimizer.iterate()
sfm_result = optimizer.values()
# print(sfm_result)
# Marginalization
marginals = gtsam.Marginals(graph, sfm_result)
for idx in img_pose_id_list:
marginals.marginalCovariance(X(idx))
for idx in landmark_id_list:
marginals.marginalCovariance(P(idx))
nrCamera = len(img_pose_id_list)
nrPoint = len(landmark_id_list)
return sfm_result, img_pose_id_list, landmark_id_list
gtsam.Point3(0, 0, 1)).pose())
poses.append(
gtsam.SimpleCamera.Lookat(gtsam.Point3(0, 10, 0), gtsam.Point3(),
gtsam.Point3(0, 0, 1)).pose())
poses.append(
gtsam.SimpleCamera.Lookat(gtsam.Point3(10, 0, 0), gtsam.Point3(),
gtsam.Point3(0, 0, 1)).pose())
# define quadrics
quadrics = []
quadrics.append(
quadricslam.ConstrainedDualQuadric(gtsam.Pose3(),
np.array([1., 2., 3.])))
quadrics.append(
quadricslam.ConstrainedDualQuadric(
gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1, 0.1, 0.1)),
np.array([1., 2., 3.])))
# add prior on first pose
prior_factor = gtsam.PriorFactorPose3(X(0), poses[0], prior_noise)
graph.add(prior_factor)
# add trajectory estimate
for i, pose in enumerate(poses):
# add a perturbation to initial pose estimates to simulate noise
perturbed_pose = poses[i].compose(
gtsam.Pose3(gtsam.Rot3.Rodrigues(0.1, 0.1, 0.1),
gtsam.Point3(0.1, 0.2, 0.3)))
initial_estimate.insert(X(i), perturbed_pose)
"""The real power of GTSAM will show here. In five lines, we'll setup a Gauss Newton nonlinear optimizer and optimize for the vehicle's poses in world coordinates.
Note: This cell runs your ICP implementation 180 times. If you've implemented your ICP similarly to the TAs, expect this cell to take 2 minutes. If you're missing the `initial_transform` argument for icp, it may take ~1 hour.
"""
# load in all clouds in our dataset
clouds = read_ply(*scans_fnames)
# Setting up our factor graph
graph = gtsam.NonlinearFactorGraph()
initial_estimates = gtsam.Values()
# We get the initial pose of the car from Argo AI's dataset, and we add it to the graph as such
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
initial_pose = gtsam.Pose3(gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955, -0.0221840, -0.0116519, 0.0228910, 0.9996701),
gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633))
graph.add(gtsam.PriorFactorPose3(0, initial_pose, PRIOR_NOISE))
initial_estimates.insert(0, initial_pose)
# We'll use your function to populate the factor graph
graph, initial_estimates = populate_factor_graph(graph, initial_estimates, initial_pose, clouds)
# Now optimize for the states
parameters = gtsam.GaussNewtonParams()
parameters.setRelativeErrorTol(1e-5)
parameters.setMaxIterations(100)
optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimates, parameters)
result = optimizer.optimize()
"""Let's plot these poses to see how our vechicle moves.
def optimize(gps_measurements: List[GpsMeasurement],
imu_measurements: List[ImuMeasurement],
sigma_init_x: gtsam.noiseModel.Diagonal,
sigma_init_v: gtsam.noiseModel.Diagonal,
sigma_init_b: gtsam.noiseModel.Diagonal,
noise_model_gps: gtsam.noiseModel.Diagonal,
kitti_calibration: KittiCalibration, first_gps_pose: int,
gps_skip: int) -> gtsam.ISAM2:
"""Run ISAM2 optimization on the measurements."""
# Set initial conditions for the estimated trajectory
# initial pose is the reference frame (navigation frame)
current_pose_global = Pose3(gtsam.Rot3(),
gps_measurements[first_gps_pose].position)
# the vehicle is stationary at the beginning at position 0,0,0
current_velocity_global = np.zeros(3)
current_bias = gtsam.imuBias.ConstantBias() # init with zero bias
imu_params = getImuParams(kitti_calibration)
# Set ISAM2 parameters and create ISAM2 solver object
isam_params = gtsam.ISAM2Params()
isam_params.setFactorization("CHOLESKY")
isam_params.relinearizeSkip = 10
isam = gtsam.ISAM2(isam_params)
# Create the factor graph and values object that will store new factors and
# values to add to the incremental graph
new_factors = gtsam.NonlinearFactorGraph()
# values storing the initial estimates of new nodes in the factor graph
new_values = gtsam.Values()
# Main loop:
# (1) we read the measurements
# (2) we create the corresponding factors in the graph
# (3) we solve the graph to obtain and optimal estimate of robot trajectory
print("-- Starting main loop: inference is performed at each time step, "
"but we plot trajectory every 10 steps")
j = 0
included_imu_measurement_count = 0
for i in range(first_gps_pose, len(gps_measurements)):
# At each non=IMU measurement we initialize a new node in the graph
current_pose_key = X(i)
current_vel_key = V(i)
current_bias_key = B(i)
t = gps_measurements[i].time
if i == first_gps_pose:
# Create initial estimate and prior on initial pose, velocity, and biases
new_values.insert(current_pose_key, current_pose_global)
new_values.insert(current_vel_key, current_velocity_global)
new_values.insert(current_bias_key, current_bias)
new_factors.addPriorPose3(current_pose_key, current_pose_global,
sigma_init_x)
new_factors.addPriorVector(current_vel_key,
current_velocity_global, sigma_init_v)
new_factors.addPriorConstantBias(current_bias_key, current_bias,
sigma_init_b)
else:
t_previous = gps_measurements[i - 1].time
# Summarize IMU data between the previous GPS measurement and now
current_summarized_measurement = gtsam.PreintegratedImuMeasurements(
imu_params, current_bias)
while (j < len(imu_measurements)
and imu_measurements[j].time <= t):
if imu_measurements[j].time >= t_previous:
current_summarized_measurement.integrateMeasurement(
imu_measurements[j].accelerometer,
imu_measurements[j].gyroscope, imu_measurements[j].dt)
included_imu_measurement_count += 1
j += 1
# Create IMU factor
previous_pose_key = X(i - 1)
previous_vel_key = V(i - 1)
previous_bias_key = B(i - 1)
new_factors.push_back(
gtsam.ImuFactor(previous_pose_key, previous_vel_key,
current_pose_key, current_vel_key,
previous_bias_key,
current_summarized_measurement))
# Bias evolution as given in the IMU metadata
sigma_between_b = gtsam.noiseModel.Diagonal.Sigmas(
np.asarray([
np.sqrt(included_imu_measurement_count) *
kitti_calibration.accelerometer_bias_sigma
] * 3 + [
np.sqrt(included_imu_measurement_count) *
kitti_calibration.gyroscope_bias_sigma
] * 3))
new_factors.push_back(
gtsam.BetweenFactorConstantBias(previous_bias_key,
current_bias_key,
gtsam.imuBias.ConstantBias(),
sigma_between_b))
# Create GPS factor
gps_pose = Pose3(current_pose_global.rotation(),
gps_measurements[i].position)
if (i % gps_skip) == 0:
new_factors.addPriorPose3(current_pose_key, gps_pose,
noise_model_gps)
new_values.insert(current_pose_key, gps_pose)
print(f"############ POSE INCLUDED AT TIME {t} ############")
print(gps_pose.translation(), "\n")
else:
new_values.insert(current_pose_key, current_pose_global)
# Add initial values for velocity and bias based on the previous
# estimates
new_values.insert(current_vel_key, current_velocity_global)
new_values.insert(current_bias_key, current_bias)
# Update solver
# =======================================================================
# We accumulate 2*GPSskip GPS measurements before updating the solver at
# first so that the heading becomes observable.
if i > (first_gps_pose + 2 * gps_skip):
print(f"############ NEW FACTORS AT TIME {t:.6f} ############")
new_factors.print()
isam.update(new_factors, new_values)
# Reset the newFactors and newValues list
new_factors.resize(0)
new_values.clear()
# Extract the result/current estimates
result = isam.calculateEstimate()
current_pose_global = result.atPose3(current_pose_key)
current_velocity_global = result.atVector(current_vel_key)
current_bias = result.atConstantBias(current_bias_key)
print(f"############ POSE AT TIME {t} ############")
current_pose_global.print()
print("\n")
return isam
import cv2
import gtsam
import numpy as np
# load images to test with
img_l = cv2.imread("imagesL/tag_L.jpg")
img_c = cv2.imread("imagesL/tag_C.jpg")
img_r = cv2.imread("imagesL/tag_R.jpg")
# set up camera and import intrinsic parameters
paramsl = np.load('params/left_params.npz')
paramsr = np.load('params/right_params.npz')
left = [
"L", img_l, paramsl['mtx_l'], paramsl['dist_l'],
gtsam.Pose3(gtsam.Rot3(paramsl['R_L']), paramsl['T_L'])
]
center = ["C", img_c, paramsl['mtx_c'], paramsl['dist_c'], gtsam.Pose3()]
right = [
"R", img_r, paramsr['mtx_r'], paramsr['dist_r'],
gtsam.Pose3(gtsam.Rot3(paramsr['R_R']), paramsr['T_R']).inverse()
]
all_cameras = [left, center, right]
# setup aruco finder
dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_APRILTAG_36h11)
aruco_params = cv2.aruco.DetectorParameters_create()
size = 2.6 #inches
aruco_params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX
aruco_params.cornerRefinementWinSize = 5
aruco_params.cornerRefinementMaxIterations = 30
aruco_params.cornerRefinementMinAccuracy = 1e-5
def icp(clouda, cloudb, initial_transform=gtsam.Pose3(), max_iterations=25):
"""Runs ICP on two clouds by calling
all five steps implemented above.
Iterates until close enough or max
iterations.
Returns a series of intermediate clouds
for visualization purposes.
Args:
clouda (ndarray): point cloud A
cloudb (ndarray): point cloud B
initial_transform (gtsam.Pose3): the initial estimate of transform between clouda and cloudb (step 1 of icp)
max_iterations (int): maximum iters of ICP to run before breaking
Ret:
bTa (gtsam.Pose3): the final transform
icp_series (list): visualized icp for debugging
"""
icp_series = []
bTa = initial_transform
i = 0
temp = True
while i < max_iterations and temp:
newClTr = transform_cloud(bTa,clouda)
newCloudb = assign_closest_pairs(newClTr, cloudb)
transform = estimate_transform(newClTr,newCloudb)
if transform.equals(gtsam.Pose3.identity(),tol=1e-2:
temp = False
else:
bTa = gtsam.Pose3(np.matmul(bTa.matrix(),transform.matrix()))
i += 1
icp_series.append([newClTr, cloudb])
return bTa, icp_series
"""The animation shows how clouda has moved after each iteration of ICP. You should see stationary landmarks, like walls and parked cars, converge onto each other."""
aTb, icp_series = icp(clouda, cloudb)
visualize_clouds_animation(icp_series, speed=400, show_grid_lines=True)
"""ICP is a computationally intense algorithm and we plan to run it between each cloud pair in our 180 clouds dataset. Use the python profiler to identify the computationally expensive subroutines in your algorithm and use numpy to reduce your runtime. The TAs get ~6.5 seconds."""
import cProfile
cProfile.run('icp(clouda, cloudb)')
"""These unit tests will verify the basic functionality of the functions you've implemented in this section. Keep in mind that these are not exhaustive."""
import unittest
class TestICP(unittest.TestCase):
def setUp(self):
self.testclouda = np.array([[1], [1], [1]])
self.testcloudb = np.array([[2, 10], [1, 1], [1, 1]])
self.testcloudc = np.array([[2], [1], [1]])
self.testbTa = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0))
self.testcloudd = np.array([[0, 20, 10], [0, 10, 20], [0, 0, 0]])
self.testcloude = np.array([[10, 30, 20], [10, 20, 30], [0, 0, 0]])
def test_assign_closest_pairs1(self):
expected = (3, 1)
actual = assign_closest_pairs(self.testclouda, self.testcloudb).shape
self.assertEqual(expected, actual)
def test_assign_closest_pairs2(self):
expected = 2
actual = assign_closest_pairs(self.testclouda, self.testcloudb)[0][0]
self.assertEqual(expected, actual)
def test_estimate_transform1(self):
expected = 1
actual = estimate_transform(self.testclouda, self.testcloudc).x()
self.assertEqual(expected, actual)
def test_estimate_transform2(self):
expected = 10
actual = estimate_transform(self.testcloudd, self.testcloude).x()
self.assertAlmostEqual(expected, actual, places=7)
actua2 = estimate_transform(self.testcloudd, self.testcloude).y()
self.assertAlmostEqual(expected, actua2, places=7)
def test_transform_cloud1(self):
expected = 2
actual = transform_cloud(self.testbTa, self.testclouda)[0][0]
self.assertEqual(expected, actual)
def test_icp1(self):
ret = icp(self.testclouda, self.testcloudb)
expected1 = type(gtsam.Pose3())
actual1 = type(ret[0])
self.assertEqual(expected1, actual1)
expected2 = type([])
actual2 = type(ret[1])
self.assertEqual(expected2, actual2)
expected3 = type([])
actual3 = type(ret[1][0])
self.assertEqual(expected3, actual3)
def test_icp2(self):
expected = 1
actual = icp(self.testclouda, self.testcloudb)[0].x()
self.assertEqual(expected, actual)
def test_icp3(self):
expected = 1
actual = icp(self.testclouda, self.testcloudc)[0].x()
self.assertEqual(expected, actual)
if __name__ == "__main__":
unittest.main(argv=['first-arg-is-ignored'], exit=False)
"""# Factor Graph
In this section, we'll build a factor graph to estimate the pose of our vechicle using the transforms our ICP algorithm gives us between frames. These ICP transforms are the factors that tie the pose variables together.
We will be using GTSAM to construct the factor graph as well as perform a optimization for the pose of the car as it travels down the street. Let's start with a simple example first. Recall from PoseSLAM describe in the LIDAR slides how we could add a factor (aka constraint) between two state variables. When we revisited a state, we could add a loop closure. Since the car in our dataset never revisits a previous pose, there is not loop closure. Here is that example from the slides copied here. Note how the graph is initialized and how factors are added.
"""
# # Factor graph example
# Helper function to create a pose
def vector3(x, y, z):
"""Create 3d double numpy array."""
return np.array([x, y, z], dtype=np.float)
# Create noise model
priorNoise = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.3, 0.3, 0.1))
model = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.2, 0.2, 0.1))
# Instantiate the factor graph
example_graph = gtsam.NonlinearFactorGraph()
# Adding a prior on the first pose
example_graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), priorNoise))
# Create odometry (Between) factors between consecutive poses
example_graph.add(gtsam.BetweenFactorPose2( 1, 2, gtsam.Pose2(2, 0, 0), model))
example_graph.add(gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2), model))
example_graph.add(gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2), model))
example_graph.add(gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2), model))
# Add the loop closure constraint
example_graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2), model))
# Create the initial estimate
example_initial_estimate = gtsam.Values()
example_initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
example_initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
example_initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2))
example_initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi))
example_initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2))
# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
ex_parameters = gtsam.GaussNewtonParams()
ex_parameters.setRelativeErrorTol(1e-5)
ex_parameters.setMaxIterations(100)
ex_optimizer = gtsam.GaussNewtonOptimizer(example_graph, example_initial_estimate, ex_parameters)
ex_result = ex_optimizer.optimize()
print("Final Result:\n{}".format(ex_result))
# Plot your graph
marginals = gtsam.Marginals(example_graph, ex_result)
fig = plt.figure(0)
for i in range(1, 6):
gtsam_plot.plot_pose2(0, ex_result.atPose2(i), 0.5, marginals.marginalCovariance(i))
plt.axis('equal')
plt.show()
"""**TODO** [25 points]
You will be using your ICP implementation here to find the transform between two subsequent clouds. These transforms become the factors between pose variables in the graph. So, you will need to go through all the point clouds and run icp pair-wise to find the relative movement of the car. With these transformation, create a factor representing the transform between the pose variables.
We talked about how loop closure helps us consolidate conflicting data into a better global estimate. Unfortunately, our car does not perform a loop closure. So, our graph would just be a long series of poses connected by icp-returned transforms. However, our lidar scans are noisy, which means that our icp-returned transforms are not perfect either. This ultimately results in incorrect vehicle poses and overall map. One way that we can augment our graph is through "skipping". We simply run ICP between every other cloud, and add these skip connections into the graph. You can basically perform ICP between two non-consecutive point clouds and add that transform as a factor in the factor graph.
"""
def populate_factor_graph(graph, initial_estimates, initial_pose, clouds):
"""Populates a gtsam.NonlinearFactorGraph with
factors between state variables. Populates
initial_estimates for state variables as well.
Args:
graph (gtsam.NonlinearFactorGraph): the factor graph populated with ICP constraints
initial_estimates (gtsam.Values): the populated estimates for vehicle poses
initial_pose (gtsam.Pose3): the starting pose for the estimates in world coordinates
clouds (np.ndarray): the numpy array with all our point clouds
"""
ICP_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
factor_pose = initial_pose
# Add ICP Factors between each pair of clouds
prev_T = gtsam.Pose3()
for i in range(len(clouds) - 1):
# TODO: Run ICP between clouds (hint: use inital_tranform argument)
bta, icp_series = icp(clouds[i], clouds[i+1], initial_transform=prev_T)
#T = initial_transform(initial_pose, clouds)
# TODO: Set T to its inverse: use `gtsam.Pose3.inverse()`
T = bta.inverse()
# TODO: Add a `gtsam.BetweenFactorPose3()` to the graph
graph.add(gtsam.BetweenFactorPose3(i, i+1, T, ICP_NOISE))
factor_pose = factor_pose.compose(T)
initial_estimates.insert(i+1, factor_pose)
print(".", end="")
# Add skip connections between every other frame
prev_T = gtsam.Pose3()
for i in range(0, len(clouds) - 2, 2):
# TODO: Run ICP between clouds (hint: use inital_tranform argument)
bta, icp_series = icp(clouds[i], clouds[i+2],initial_transform=prev_T)
# TODO: Set T to its inverse: use `gtsam.Pose3.inverse()`
T = bta.inverse()
# TODO: Add a `gtsam.BetweenFactorPose3()` to the graph
graph.add(gtsam.BetweenFactorPose3(i, i+2, T, ICP_NOISE))
print(".", end="")
return graph, initial_estimates
"""The real power of GTSAM will show here. In five lines, we'll setup a Gauss Newton nonlinear optimizer and optimize for the vehicle's poses in world coordinates.
Note: This cell runs your ICP implementation 180 times. If you've implemented your ICP similarly to the TAs, expect this cell to take 2 minutes. If you're missing the `initial_transform` argument for icp, it may take ~1 hour.
"""
# load in all clouds in our dataset
clouds = read_ply(*scans_fnames)
# Setting up our factor graph
graph = gtsam.NonlinearFactorGraph()
initial_estimates = gtsam.Values()
# We get the initial pose of the car from Argo AI's dataset, and we add it to the graph as such
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
initial_pose = gtsam.Pose3(gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955, -0.0221840, -0.0116519, 0.0228910, 0.9996701),
gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633))
graph.add(gtsam.PriorFactorPose3(0, initial_pose, PRIOR_NOISE))
initial_estimates.insert(0, initial_pose)
# We'll use your function to populate the factor graph
graph, initial_estimates = populate_factor_graph(graph, initial_estimates, initial_pose, clouds)
# Now optimize for the states
parameters = gtsam.GaussNewtonParams()
parameters.setRelativeErrorTol(1e-5)
parameters.setMaxIterations(100)
optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimates, parameters)
result = optimizer.optimize()
"""Let's plot these poses to see how our vechicle moves.
Screenshot this for your reflection.
"""
poses_cloud = np.array([[], [], []])
for i in range(len(clouds)):
poses_cloud = np.hstack([poses_cloud, np.array([[result.atPose3(i).x()], [result.atPose3(i).y()], [result.atPose3(i).z()]])])
init_car_pose = gtsam.Pose3(gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955, -0.0221840, -0.0116519, 0.0228910, 0.9996701),
gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633))
visualize_clouds([poses_cloud, transform_cloud(init_car_pose, clouds[0])], show_grid_lines=True)
"""These unit tests will verify the basic functionality of the function you've implemented in this section. Keep in mind that these are not exhaustive."""
import unittest
class TestFactorGraph(unittest.TestCase):
def setUp(cls):
test_clouds = read_ply(*scans_fnames)[:6]
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
ICP_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
test_graph = gtsam.NonlinearFactorGraph()
test_initial_estimates = gtsam.Values()
initial_pose = gtsam.Pose3(gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955, -0.0221840, -0.0116519, 0.0228910, 0.9996701),
gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633))
test_graph.add(gtsam.PriorFactorPose3(0, initial_pose, PRIOR_NOISE))
test_initial_estimates.insert(0, initial_pose)
test_graph, test_initial_estimates = populate_factor_graph(test_graph, test_initial_estimates, initial_pose, test_clouds)
cls.graph = test_graph
cls.initial_estimates = test_initial_estimates
def test_graph_params(self):
self.assertTrue(type(self.graph) == gtsam.NonlinearFactorGraph)
def test_initial_estimates_params(self):
self.assertTrue(type(self.initial_estimates) == gtsam.Values)
def suite():
functions = ['test_graph_params', 'test_initial_estimates_params']
suite = unittest.TestSuite()
for func in functions:
suite.addTest(TestFactorGraph(func))
return suite
if __name__ == "__main__":
runner = unittest.TextTestRunner()
runner.run(suite())
"""# Mapping
In this section, we'll tackle the mapping component of SLAM (Simulataneous Localization and Mapping). The previous section used a factor graph to localize our vehicle's poses in world coordinates. We'll now use those poses to form a map of the street from the point clouds.
Given the poses and the clouds, this task is easy. We'll use your `transform_cloud` method from the ICP section to transform every other cloud in our dataset to be centered at the corresponding pose where the cloud was captured. Visualizing all of these clouds yields the complete map. We don't use every cloud in our dataset to reduce the amount of noise in our map while retaining plenty of detail.
Screenshot this for your reflection.
"""
cloud_map = []
for i in range(0, len(clouds), 2):
cloud_map.append(transform_cloud(result.atPose3(i), clouds[i-1]))
visualize_clouds(cloud_map, show_grid_lines=True, single_color="#C6C6C6")
"""# Reflection
