def error_point(measurement, calibration: gtsam.Cal3_S2,
this: gtsam.CustomFactor,
values: gtsam.Values,
jacobians: Optional[List[np.ndarray]]) -> np.ndarray:
x_key = this.keys()[0]
l_key = this.keys()[1]
pose = values.atPose3(x_key)
pw = values.atPoint3(l_key)
cam = PinholeCameraCal3_S2(pose, calibration)
q = cam.pose().transformTo(pw)
pn = cam.Project(q)
pi = cam.calibration().uncalibrate(pn)
error = (pi - measurement)
# print(pi, pi_line, error)
if jacobians is not None:
# print(gtsam.DefaultKeyFormatter(x_key), gtsam.DefaultKeyFormatter(l_key))
d = 1. / q[2]
Rt = cam.pose().rotation().transpose()
Dpose = cal_Dpose(pn, d)
Dpoint = cal_Dpoint(pn, d, Rt)
Dpi_pn = np.array([[320., 0], [0, 320.]], dtype=float)
Dpose = np.matmul(Dpi_pn, Dpose)
Dpoint = np.matmul(Dpi_pn, Dpoint)
jacobians[0] = Dpose
jacobians[1] = Dpoint
return error
def test_level2(self):
# Create a level camera, looking in Y-direction
pose2 = Pose2(0.4, 0.3, math.pi / 2.0)
camera = SimpleCamera.Level(K, pose2, 0.1)
# expected
x = Point3(1, 0, 0)
y = Point3(0, 0, -1)
z = Point3(0, 1, 0)
wRc = Rot3(x, y, z)
expected = Pose3(wRc, Point3(0.4, 0.3, 0.1))
self.gtsamAssertEquals(camera.pose(), expected, 1e-9)
def createPoses(K: Cal3_S2) -> List[Pose3]:
"""Generate a set of ground-truth camera poses arranged in a circle about the origin."""
radius = 4.0
height = -2.0
angles = np.linspace(0, 2 * np.pi, 8, endpoint=False)
up = Point3(0, 0, 1) # NED -> NORTH_EAST_DOWN ???
target = Point3(0, 0, 0)
poses = []
for theta in angles:
position = Point3(radius * np.cos(theta), radius * np.sin(theta), height)
camera = PinholeCameraCal3_S2.Lookat(position, target, up, K)
poses.append(camera.pose())
return poses
def test_pose_estimate(self):
"""Test pose estimate."""
wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name
estimated_pose = Pose3(wRc, Point3(0, 0, 1.2))
camera = PinholeCameraCal3_S2(estimated_pose, self.calibration)
landmark_points = [[-5, 10, 5], [5, 10, 5], [5, 10, -5], [-5, 10, -5]]
observations = []
for landmark in landmark_points:
# feature is gtsam.Point2 object
landmark_point = Point3(landmark[0], landmark[1], landmark[2])
feature_point = camera.project(landmark_point)
observations.append((feature_point, landmark_point))
current_pose = self.trajectory_estimator.pose_estimate(
observations, estimated_pose)
self.gtsamAssertEquals(current_pose, estimated_pose)
def generate_data(options) -> Tuple[Data, GroundTruth]:
""" Generate ground-truth and measurement data. """
K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
nrPoints = 3 if options.triangle else 8
truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints)
data = Data(K, nrCameras=options.nrCameras, nrPoints=nrPoints)
# Generate simulated data
if options.triangle: # Create a triangle target, just 3 points on a plane
r = 10
for j in range(len(truth.points)):
theta = j * 2 * pi / nrPoints
truth.points[j] = Point3(
r * math.cos(theta), r * math.sin(theta), 0)
else: # 3D landmarks as vertices of a cube
truth.points = [
Point3(10, 10, 10), Point3(-10, 10, 10),
Point3(-10, -10, 10), Point3(10, -10, 10),
Point3(10, 10, -10), Point3(-10, 10, -10),
Point3(-10, -10, -10), Point3(10, -10, -10)
]
# Create camera cameras on a circle around the triangle
height = 10
r = 40
for i in range(options.nrCameras):
theta = i * 2 * pi / options.nrCameras
t = Point3(r * math.cos(theta), r * math.sin(theta), height)
truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
Point3(0, 0, 0),
Point3(0, 0, 1),
truth.K)
# Create measurements
for j in range(nrPoints):
# All landmarks seen in every frame
data.Z[i][j] = truth.cameras[i].project(truth.points[j])
data.J[i][j] = j
# Calculate odometry between cameras
for i in range(1, options.nrCameras):
data.odometry[i] = truth.cameras[i - 1].pose().between(
truth.cameras[i].pose())
return data, truth
def cal_PointProject_Jacobians(cam: PinholeCameraCal3_S2, pw: Point3):
q = cam.pose().transformTo(pw)
pn = cam.Project(q)
pi = cam.calibration().uncalibrate(pn)
d = 1 / q[2]
Rt = cam.pose().rotation().transpose()
Dpose = cal_Dpose(pn, d)
Dpoint = cal_Dpoint(pn, d, Rt)
Dpi_pn = np.array([[cam.calibration().fx(), 0],
[0, cam.calibration().fy()]])
Dpose = np.matmul(Dpi_pn, Dpose) # - ?
Dpoint = np.matmul(Dpi_pn, Dpoint) # - ?
return pi, Dpose, Dpoint
def error_point_landmarks( measurement: np.ndarray,
calibration: gtsam.Cal3_S2,
sems: List,
this: gtsam.CustomFactor,
values: gtsam.Values,
jacobians: Optional[List[np.ndarray]]) -> np.ndarray:
x_key = this.keys()[0]
l_key = this.keys()[1]
# print(sems)
# s_key = this.keys()[2]
pose = values.atPose3(x_key)
pw = values.atPoint3(l_key)
# conf = values.atDouble(s_key)
cam = PinholeCameraCal3_S2(pose, calibration)
# pos = cam.project(point)
q = cam.pose().transformTo(pw)
pn = cam.Project(q)
pi = cam.calibration().uncalibrate(pn)
error = pi - measurement
if jacobians is not None:
# print(gtsam.DefaultKeyFormatter(x_key), gtsam.DefaultKeyFormatter(l_key))
s = sems[l_key-L0]
d = 1. / q[2]
Rt = cam.pose().rotation().transpose()
Dpose = cal_Dpose(pn, d)
Dpoint = cal_Dpoint(pn, d, Rt)
Dpi_pn = np.array([[320., 0], [0, 320.]], dtype=float)
Dpose = np.matmul(Dpi_pn, Dpose)
Dpoint = np.matmul(Dpi_pn, Dpoint)
jacobians[0] = Dpose
jacobians[1] = Dpoint
error*=s
# pos = cam.project(point)
return error
def main():
"""
Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
Each variable in the system (poses and landmarks) must be identified with a unique key.
We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
Here we will use Symbols
In GTSAM, measurement functions are represented as 'factors'. Several common factors
have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
Here we will use Projection factors to model the camera's landmark observations.
Also, we will initialize the robot at some location using a Prior factor.
When the factors are created, we will add them to a Factor Graph. As the factors we are using
are nonlinear factors, we will need a Nonlinear Factor Graph.
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.
GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
trust-region method known as Powell's Degleg
The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
nonlinear functions around an initial linearization point, then solve the linear system
to update the linearization point. This happens repeatedly until the solver converges
to a consistent set of variable values. This requires us to specify an initial guess
for each variable, held in a Values container.
"""
# Define the camera calibration parameters
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(
2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks
points = SFMdata.createPoints()
# Create the set of ground-truth poses
poses = SFMdata.createPoses(K)
# Create a factor graph
graph = NonlinearFactorGraph()
# Add a prior on pose x1. This indirectly specifies where the origin is.
# 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Simulated measurements from each camera pose, adding them to the factor graph
for i, pose in enumerate(poses):
camera = PinholeCameraCal3_S2(pose, K)
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(measurement,
measurement_noise, X(i),
L(j), K)
graph.push_back(factor)
# Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
# Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
# between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
graph.print_('Factor Graph:\n')
# Create the data structure to hold the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initial_estimate = Values()
for i, pose in enumerate(poses):
transformed_pose = pose.retract(0.1 * np.random.randn(6, 1))
initial_estimate.insert(X(i), transformed_pose)
for j, point in enumerate(points):
transformed_point = point + 0.1 * np.random.randn(3)
initial_estimate.insert(L(j), transformed_point)
initial_estimate.print_('Initial Estimates:\n')
# Optimize the graph and print results
params = gtsam.DoglegParams()
params.setVerbosity('TERMINATION')
optimizer = DoglegOptimizer(graph, initial_estimate, params)
print('Optimizing:')
result = optimizer.optimize()
result.print_('Final results:\n')
print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result)))
marginals = Marginals(graph, result)
plot.plot_3d_points(1, result, marginals=marginals)
plot.plot_trajectory(1, result, marginals=marginals, scale=8)
plot.set_axes_equal(1)
plt.show()
DEFAULT_NUM_TRACKS = 100
DEFAULT_TRACK_POINTS = [Point3(5, 5, float(i)) for i in range(DEFAULT_NUM_TRACKS)]
# set default camera intrinsics
DEFAULT_CAMERA_INTRINSICS = Cal3_S2(
fx=100.0,
fy=100.0,
s=1.0,
u0=DEFAULT_IMAGE_W // 2,
v0=DEFAULT_IMAGE_H // 2,
)
# set default camera poses as described in GTSAM example
DEFAULT_CAMERA_POSES = SFMdata.createPoses(DEFAULT_CAMERA_INTRINSICS)
# set default camera instances
DEFAULT_CAMERAS = [
PinholeCameraCal3_S2(DEFAULT_CAMERA_POSES[i], DEFAULT_CAMERA_INTRINSICS) for i in range(len(DEFAULT_CAMERA_POSES))
]
DEFAULT_NUM_CAMERAS = len(DEFAULT_CAMERAS)
# the number of valid images should be equal to the number of cameras (with estimated pose)
DEFAULT_NUM_IMAGES = DEFAULT_NUM_CAMERAS
# build dummy image dictionary with default image shape
DEFAULT_DUMMY_IMAGE_DICT = {
i: Image(value_array=np.zeros([DEFAULT_IMAGE_H, DEFAULT_IMAGE_W, DEFAULT_IMAGE_C], dtype=int))
for i in range(DEFAULT_NUM_IMAGES)
}
# set camera[1] to be selected in test_get_item
EXAMPLE_CAMERA_ID = 1
def cal_Point_Project_TartanCam(cam: PinholeCameraCal3_S2, pw: Point3):
q = cam.pose().transformTo(pw)[[1, 2, 0]] #
pn = cam.Project(q)
pi = cam.calibration().uncalibrate(pn)
# d = 1 / q[2]
return np.rint(pi).astype(np.int32)
viz = []
viz.extend(viz_base)
viz.extend(viz_cam)
# draw_geometries(viz, **config_viz)
# for pose in poses_mat44:
# viz_cam.append(getKeyframe(transform=pose,color=[1,0,0]))
# ----- GTSAM -----
# plt.imshow(frame0['color']);plt.show()
# plt.imshow(frame1['color']);plt.show()
K = Cal3_S2(320, 320, 0.0, 320, 240)
pose0 = Pose3(frame0['transform'])
cam0 = PinholeCameraCal3_S2(pose0, K)
pose1 = Pose3(frame1['transform'])
cam1 = PinholeCameraCal3_S2(pose1, K)
rgbd0 = RGBDImage.create_from_color_and_depth(color=Image(frame0['color']),
depth=Image(frame0['depth']),
depth_scale=1.0,
depth_trunc=np.inf,
convert_rgb_to_intensity=False)
cloud0 = PointCloud.create_from_rgbd_image(image=rgbd0,
intrinsic=camIntr,
extrinsic=tartan_camExtr)
cloud0.transform(frame0['transform'])
point3D = np.asarray(cloud0.points)
pl = createPointsLines()
poses = createPoses(K)
points = pl[0]
sems = pl[2]
# Create a factor graph
graph = NonlinearFactorGraph()
#Add Pose Prior
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Simulated measurements from each camera pose, adding them to the factor graph
for i, pose in enumerate(poses):
camera = PinholeCameraCal3_S2(pose, K)
for j, point in enumerate(points):
measurement = camera.project(point)
# print(measurement)
# factor = GenericProjectionFactorCal3_S2(measurement, measurement_noise, X(i), L(j), K)
factor = gtsam.CustomFactor(measurement_noise, [X(i),L(j)], partial(error_point_landmarks,measurement, K, sems))
graph.push_back(factor)
#Add Point Prior
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
# graph.print('Factor Graph:\n')
poses_noise = []
points_noise = []
def get_camera(radius):
up = Point3(0, 0, 1)
target = Point3(0, 0, 0)
position = Point3(radius, 0, 0)
camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
return camera
def test_constructor(self):
pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5))
camera = SimpleCamera(pose1, K)
self.gtsamAssertEquals(camera.calibration(), K, 1e-9)
self.gtsamAssertEquals(camera.pose(), pose1, 1e-9)
def test_triangulation_robust_three_poses(self) -> None:
"""Ensure triangulation with a robust model works."""
sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)
# landmark ~5 meters infront of camera
landmark = Point3(5, 0.5, 1.2)
pose1 = Pose3(UPRIGHT, Point3(0, 0, 1))
pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0))
pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -0.1))
camera1 = PinholeCameraCal3_S2(pose1, sharedCal)
camera2 = PinholeCameraCal3_S2(pose2, sharedCal)
camera3 = PinholeCameraCal3_S2(pose3, sharedCal)
z1: Point2 = camera1.project(landmark)
z2: Point2 = camera2.project(landmark)
z3: Point2 = camera3.project(landmark)
poses = gtsam.Pose3Vector([pose1, pose2, pose3])
measurements = Point2Vector([z1, z2, z3])
# noise free, so should give exactly the landmark
actual = gtsam.triangulatePoint3(poses,
sharedCal,
measurements,
rank_tol=1e-9,
optimize=False)
self.assertTrue(np.allclose(landmark, actual, atol=1e-2))
# Add outlier
measurements[0] += Point2(100, 120) # very large pixel noise!
# now estimate does not match landmark
actual2 = gtsam.triangulatePoint3(poses,
sharedCal,
measurements,
rank_tol=1e-9,
optimize=False)
# DLT is surprisingly robust, but still off (actual error is around 0.26m)
self.assertTrue(np.linalg.norm(landmark - actual2) >= 0.2)
self.assertTrue(np.linalg.norm(landmark - actual2) <= 0.5)
# Again with nonlinear optimization
actual3 = gtsam.triangulatePoint3(poses,
sharedCal,
measurements,
rank_tol=1e-9,
optimize=True)
# result from nonlinear (but non-robust optimization) is close to DLT and still off
self.assertTrue(np.allclose(actual2, actual3, atol=0.1))
# Again with nonlinear optimization, this time with robust loss
model = gtsam.noiseModel.Robust.Create(
gtsam.noiseModel.mEstimator.Huber.Create(1.345),
gtsam.noiseModel.Unit.Create(2))
actual4 = gtsam.triangulatePoint3(poses,
sharedCal,
measurements,
rank_tol=1e-9,
optimize=True,
model=model)
# using the Huber loss we now have a quite small error!! nice!
self.assertTrue(np.allclose(landmark, actual4, atol=0.05))
IMAGE_W = 400
IMAGE_H = 300
# set dummy camera intrinsics
CAMERA_INTRINSICS = Cal3_S2(
fx=100.0,
fy=100.0,
s=0.0,
u0=IMAGE_W // 2,
v0=IMAGE_H // 2,
)
# set dummy camera poses as described in GTSAM example
CAMERA_POSES = SFMdata.createPoses(CAMERA_INTRINSICS)
# set dummy camera instances
CAMERAS = [
PinholeCameraCal3_S2(CAMERA_POSES[i], CAMERA_INTRINSICS)
for i in range(len(CAMERA_POSES))
]
# set dummy sphere grid center
SPHERE_CENTER = np.array([-1.5, -1.5, -1.5])
# set dummy sphere grid radius
SPHERE_RADIUS = 0.2
UNIT_CUBE_CENTER = np.array([0.5, 0.5, 0.5])
class TestOverlapFrustums(unittest.TestCase):
"""Class containing all unit tests for overlap frustums utils."""
def test_calculate_overlap_frustums(self) -> None:
"""Test whether the overlap frustum area is correct"""
def main():
"""
A structure-from-motion example with landmarks
- The landmarks form a 10 meter cube
- The robot rotates around the landmarks, always facing towards the cube
"""
# Define the camera calibration parameters
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
camera_noise = gtsam.noiseModel.Isotropic.Sigma(
2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks
points = SFMdata.createPoints()
# Create the set of ground-truth poses
poses = SFMdata.createPoses(K)
# Create a NonlinearISAM object which will relinearize and reorder the variables
# every "reorderInterval" updates
isam = NonlinearISAM(reorderInterval=3)
# Create a Factor Graph and Values to hold the new data
graph = NonlinearFactorGraph()
initial_estimate = Values()
# Loop over the different poses, adding the observations to iSAM incrementally
for i, pose in enumerate(poses):
camera = PinholeCameraCal3_S2(pose, K)
# Add factors for each landmark observation
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(measurement, camera_noise,
X(i), L(j), K)
graph.push_back(factor)
# 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))
initial_xi = pose.compose(noise)
# Add an initial guess for the current pose
initial_estimate.insert(X(i), initial_xi)
# If this is the first iteration, add a prior on the first pose to set the coordinate frame
# and a prior on the first landmark to set the scale
# Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
# adding it to iSAM.
if i == 0:
# 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]))
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Add a prior on landmark l0
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
# Add initial guesses to all observed landmarks
noise = np.array([-0.25, 0.20, 0.15])
for j, point in enumerate(points):
# Intentionally initialize the variables off from the ground truth
initial_lj = points[j] + noise
initial_estimate.insert(L(j), initial_lj)
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)
current_estimate = isam.estimate()
print('*' * 50)
print('Frame {}:'.format(i))
current_estimate.print_('Current estimate: ')
# Clear the factor graph and values for the next iteration
graph.resize(0)
initial_estimate.clear()
def test_outliers_and_far_landmarks(self) -> None:
"""Check safe triangulation function."""
pose1, pose2 = self.poses
K1 = Cal3_S2(1500, 1200, 0, 640, 480)
# create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
camera1 = PinholeCameraCal3_S2(pose1, K1)
# create second camera 1 meter to the right of first camera
K2 = Cal3_S2(1600, 1300, 0, 650, 440)
camera2 = PinholeCameraCal3_S2(pose2, K2)
# 1. Project two landmarks into two cameras and triangulate
z1 = camera1.project(self.landmark)
z2 = camera2.project(self.landmark)
cameras = CameraSetCal3_S2()
measurements = Point2Vector()
cameras.append(camera1)
cameras.append(camera2)
measurements.append(z1)
measurements.append(z2)
landmarkDistanceThreshold = 10 # landmark is closer than that
# all default except landmarkDistanceThreshold:
params = TriangulationParameters(1.0, False, landmarkDistanceThreshold)
actual: TriangulationResult = gtsam.triangulateSafe(
cameras, measurements, params)
self.gtsamAssertEquals(actual.get(), self.landmark, 1e-2)
self.assertTrue(actual.valid())
landmarkDistanceThreshold = 4 # landmark is farther than that
params2 = TriangulationParameters(1.0, False,
landmarkDistanceThreshold)
actual = gtsam.triangulateSafe(cameras, measurements, params2)
self.assertTrue(actual.farPoint())
# 3. Add a slightly rotated third camera above with a wrong measurement
# (OUTLIER)
pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1))
K3 = Cal3_S2(700, 500, 0, 640, 480)
camera3 = PinholeCameraCal3_S2(pose3, K3)
z3 = camera3.project(self.landmark)
cameras.append(camera3)
measurements.append(z3 + Point2(10, -10))
landmarkDistanceThreshold = 10 # landmark is closer than that
outlierThreshold = 100 # loose, the outlier is going to pass
params3 = TriangulationParameters(1.0, False,
landmarkDistanceThreshold,
outlierThreshold)
actual = gtsam.triangulateSafe(cameras, measurements, params3)
self.assertTrue(actual.valid())
# now set stricter threshold for outlier rejection
outlierThreshold = 5 # tighter, the outlier is not going to pass
params4 = TriangulationParameters(1.0, False,
landmarkDistanceThreshold,
outlierThreshold)
actual = gtsam.triangulateSafe(cameras, measurements, params4)
self.assertTrue(actual.outlier())