def add_point(self, pointsInitial, measurements, octave):
if pointsInitial[-1] > self.depth_threshold:
information = self.inv_lvl_sigma2[octave] * np.identity(2)
stereo_model = gt.noiseModel_Diagonal.Information(information)
huber = gt.noiseModel_mEstimator_Huber.Create(self.deltaMono)
robust_model = gt.noiseModel_Robust(huber, stereo_model)
factor = gt.GenericProjectionFactorCal3_S2(
gt.Point2(measurements[0], measurements[2]), robust_model,
X(1), L(self.counter), self.K_mono)
self.is_stereo.append(False)
else:
information = self.inv_lvl_sigma2[octave] * np.identity(3)
stereo_model = gt.noiseModel_Diagonal.Information(information)
huber = gt.noiseModel_mEstimator_Huber.Create(self.deltaStereo)
robust_model = gt.noiseModel_Robust(huber, stereo_model)
factor = gt.GenericStereoFactor3D(
gt.StereoPoint2(*tuple(measurements)), robust_model, X(1),
L(self.counter), self.K_stereo)
self.is_stereo.append(True)
self.graph.add(
gt.NonlinearEqualityPoint3(L(self.counter),
gt.Point3(pointsInitial)))
self.initialEstimate.insert(L(self.counter), gt.Point3(pointsInitial))
self.graph.add(factor)
self.octave.append(octave)
self.counter += 1
def test_point3(self):
"""Test for Point3 version."""
factor = gtsam.NonlinearEquality2Point3(0, 1)
error = factor.evaluateError(gtsam.Point3(0, 0, 0),
gtsam.Point3(0, 0, 0))
np.testing.assert_allclose(error, np.zeros(3))
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 n2g(numpy_arr, obj):
if obj == "Quaternion":
x, y, z, w = numpy_arr
return gtsam.Rot3.Quaternion(w, x, y, z)
elif obj == "Euler":
roll, pitch, yaw = numpy_arr
return gtsam.Rot3.Ypr(yaw, pitch, roll)
elif obj == "Point2":
x, y = numpy_arr
return gtsam.Point2(x, y)
elif obj == "Pose2":
x, y, yaw = numpy_arr
return gtsam.Pose2(x, y, yaw)
elif obj == "Point3":
x, y, z = numpy_arr
return gtsam.Point3(x, y, z)
elif obj == "Pose3":
x, y, z, roll, pitch, yaw = numpy_arr
return gtsam.Pose3(gtsam.Rot3.Ypr(yaw, pitch, roll),
gtsam.Point3(x, y, z))
elif obj == "imuBiasConstantBias":
imu_bias = numpy_arr
return gtsam.imuBias_ConstantBias(np.array(imu_bias[:3]),
np.array(imu_bias[3:]))
elif obj == "Vector":
return np.array(numpy_arr)
else:
raise NotImplementedError("Not implemented from numpy to " + obj)
def estimate_transform(clouda, cloudb):
"""Estimate the transform from clouda to
cloudb. Returns a gtsam.Pose3 object.
Args:
clouda (ndarray): point cloud A
cloudb (ndarray): point cloud B
"""
if clouda.shape != cloudb.shape and clouda.shape[1] < 3:
return None
centroida = np.average(clouda, axis=1)
centroidb = np.average(cloudb, axis=1)
clouda_prime = clouda - centroida[:, np.newaxis]
cloudb_prime = cloudb - centroidb[:, np.newaxis]
H = np.sum(clouda_prime.T[:, :, None] * cloudb_prime.T[:, None], axis=0)
aRb = gtsam.Rot3.ClosestTo(H)
rot_centroidb = aRb.rotate(gtsam.Point3(centroidb))
atb = gtsam.Point3(
centroida -
np.array([rot_centroidb.x(),
rot_centroidb.y(),
rot_centroidb.z()]))
return gtsam.Pose3(aRb, atb).inverse()
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 createPoses():
# Create the set of ground-truth poses
radius = 30.0
angles = np.linspace(0, 2 * np.pi, 8, endpoint=False)
up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0)
poses = []
for theta in angles:
position = gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta),
0.0)
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up)
poses.append(camera.pose())
return poses
def createPoses(K: Cal3_S2) -> List[Pose3]:
"""Generate a set of ground-truth camera poses arranged in a circle about the origin."""
radius = 40.0
height = 10.0
angles = np.linspace(0, 2 * np.pi, 8, endpoint=False)
up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0)
poses = []
for theta in angles:
position = gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta),
height)
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
poses.append(camera.pose())
return poses
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 motion_sample(self, action, action_noise_matrix, ML=False):
# Taking the path length for using the latest pose
try:
self.optimize()
except:
pass
path_length = len(self.daRealization)
new_pose = self.result.atPose3(self.X(path_length)).compose(action)
new_pose_vector = [
new_pose.rotation().rpy()[0],
new_pose.rotation().rpy()[1],
new_pose.rotation().rpy()[2],
new_pose.x(),
new_pose.y(),
new_pose.z()
]
# Sample unless ML assumption is true
if ML is False:
sampled_new_pose_vector = np.random.multivariate_normal(
new_pose_vector, action_noise_matrix)
return gtsam.Pose3(
gtsam.Rot3.Ypr(sampled_new_pose_vector[2],
sampled_new_pose_vector[1],
sampled_new_pose_vector[0]),
gtsam.Point3(sampled_new_pose_vector[3],
sampled_new_pose_vector[4],
sampled_new_pose_vector[5]))
else:
return new_pose
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 main():
"""Main runner."""
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
# Add a prior on the first point, setting it to the origin
# A prior factor consists of a mean and a noise model (covariance matrix)
priorMean = gtsam.Pose3() # prior at origin
graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE))
# Add GPS factors
gps = gtsam.Point3(lat0, lon0, h0)
graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE))
print("\nFactor Graph:\n{}".format(graph))
# Create the data structure to hold the initialEstimate estimate to the solution
# For illustrative purposes, these have been deliberately set to incorrect values
initial = gtsam.Values()
initial.insert(1, gtsam.Pose3())
print("\nInitial Estimate:\n{}".format(initial))
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
print("\nFinal Result:\n{}".format(result))
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 __init__(self, points, quadrics, n_interpolate=50):
"""
:param - points: list of camera positions (Point3)
:param - quadrics: list of ConstrainedDualQuadrics
:param - n_interpolate: number of points to be generated between 'points'.
"""
# generate camera poses looking at a quadric
target = quadrics[0].pose().translation()
poses = [
gtsam.SimpleCamera.Lookat(point, target, gtsam.Point3(0, 0,
1)).pose()
for point in points
]
# interpolate poses into trajectory
self.true_trajectory = self.interpolate_poses(poses, n_interpolate)
# create quadrics from list
self.true_quadrics = Quadrics(dict(zip(range(len(quadrics)),
quadrics)))
# create box measurements
self.true_boxes = self.reproject_quadrics(self.true_quadrics,
self.true_trajectory)
def test_extractPoint3(self):
"""Test extractPoint3."""
initial = gtsam.Values()
point3 = gtsam.Point3(0.0, 0.1, 0.0)
initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1))
initial.insert(2, point3)
np.testing.assert_equal(gtsam.utilities.extractPoint3(initial),
point3.reshape(-1, 3))
def test_perturbPoint3(self):
"""Test perturbPoint3."""
values = gtsam.Values()
point3 = gtsam.Point3(0, 0, 0)
values.insert(0, gtsam.Pose2())
values.insert(1, point3)
gtsam.utilities.perturbPoint3(values, 1)
self.assertTrue(not np.allclose(values.atPoint3(1), point3))
def add_mr_object_measurement(self,
stack,
class_realization,
new_input_object_prior=None,
new_input_object_covariance=None):
graph_add = gtsam.NonlinearFactorGraph()
initial_add = gtsam.Values()
idx = 0
for obj in stack:
cov_noise_model = stack[obj]['covariance']
exp_gtsam = gtsam.Pose3(
gtsam.Rot3.Ypr(stack[obj]['expectation'][2],
stack[obj]['expectation'][1],
stack[obj]['expectation'][0]),
gtsam.Point3(stack[obj]['expectation'][3],
stack[obj]['expectation'][4],
stack[obj]['expectation'][5]))
pose_rotation_matrix = exp_gtsam.rotation().matrix()
cov_noise_model_rotated = self.rotate_cov_6x6(
cov_noise_model, np.transpose(pose_rotation_matrix))
cov_noise_model_rotated = gtsam.noiseModel.Gaussian.Covariance(
cov_noise_model_rotated)
#updating the graph
self.graph.add(
gtsam.PriorFactorPose3(self.XO(obj), exp_gtsam,
cov_noise_model_rotated))
graph_add.add(
gtsam.PriorFactorPose3(self.XO(obj), exp_gtsam,
cov_noise_model_rotated))
self.prop_belief.add(
gtsam.PriorFactorPose3(self.XO(obj), exp_gtsam,
cov_noise_model_rotated))
if self.prop_initial.exists(self.XO(obj)) is False:
self.prop_initial.insert(
self.XO(obj), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0)))
# if obj in new_input_object_prior and obj not in self.classRealization:
# self.graph.add(gtsam.PriorFactorPose3(self.XO(obj), new_input_object_prior[obj],
# new_input_object_covariance[obj]))
# self.prop_belief.add(gtsam.PriorFactorPose3(self.XO(obj), new_input_object_prior[obj],
# new_input_object_covariance[obj]))
self.classRealization[obj] = class_realization[obj]
if self.isam.value_exists(self.XO(obj)) is False:
initial_add.insert(self.XO(obj),
gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0)))
idx += 1
if stack[obj]['weight'] > -322:
self.logWeight += stack[obj]['weight'] # + 150
else:
self.logWeight = -math.inf
def test_allPose3s(self):
"""Test allPose3s."""
initial = gtsam.Values()
initial.insert(0, gtsam.Pose3())
initial.insert(2, gtsam.Point2(1, 1))
initial.insert(1, gtsam.Pose3())
initial.insert(3, gtsam.Point3(1, 2, 3))
result = gtsam.utilities.allPose3s(initial)
self.assertEqual(result.size(), 2)
def test_insertBackprojections(self):
"""Test insertBackprojections."""
values = gtsam.Values()
cam = gtsam.PinholeCameraCal3_S2()
gtsam.utilities.insertBackprojections(
values, cam, [0, 1, 2], np.asarray([[20, 30, 40], [20, 30, 40]]),
10)
np.testing.assert_allclose(values.atPoint3(0),
gtsam.Point3(200, 200, 10))
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 test_extractPose2(self):
"""Test extractPose2."""
initial = gtsam.Values()
pose2 = np.asarray((0.0, 0.1, 0.1))
initial.insert(1, gtsam.Pose2(*pose2))
initial.insert(2, gtsam.Point3(0.0, 0.1, 0.0))
np.testing.assert_allclose(gtsam.utilities.extractPose2(initial),
pose2.reshape(-1, 3))
def add_LandmarkEstimate(self, L_id, Pt_estimate):
if Pt_estimate.ndim == 1:
raise ValueError(
"Supplied point is 1-dimensional, required Nx3 array")
if Pt_estimate.shape[1] != 3:
raise ValueError(
"2nd dimension on supplied point is not 3, required Nx3 array")
for l, p_est in zip(L_id, Pt_estimate):
self.initial_estimate.insert(iSAM2Wrapper.get_key('l', l),
gtsam.Point3(*p_est))
def createPoints():
# Create the set of ground-truth landmarks
points = [
gtsam.Point3(10.0, 10.0, 10.0),
gtsam.Point3(-10.0, 10.0, 10.0),
gtsam.Point3(-10.0, -10.0, 10.0),
gtsam.Point3(10.0, -10.0, 10.0),
gtsam.Point3(10.0, 10.0, -10.0),
gtsam.Point3(-10.0, 10.0, -10.0),
gtsam.Point3(-10.0, -10.0, -10.0),
gtsam.Point3(10.0, -10.0, -10.0)
]
return points
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 __init__(self, body_ptx: float, body_pty: float, body_ptz: float,
body_prx: float, body_pry: float, body_prz: float,
accelerometer_sigma: float, gyroscope_sigma: float,
integration_sigma: float, accelerometer_bias_sigma: float,
gyroscope_bias_sigma: float, average_delta_t: float):
self.bodyTimu = Pose3(gtsam.Rot3.RzRyRx(body_prx, body_pry, body_prz),
gtsam.Point3(body_ptx, body_pty, body_ptz))
self.accelerometer_sigma = accelerometer_sigma
self.gyroscope_sigma = gyroscope_sigma
self.integration_sigma = integration_sigma
self.accelerometer_bias_sigma = accelerometer_bias_sigma
self.gyroscope_bias_sigma = gyroscope_bias_sigma
self.average_delta_t = average_delta_t
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 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 plot_cameras_gtsam(transform_matrix_filename: str):
set_cams = np.load(
'/home/sushmitawarrier/clevr-dataset-gen/output/images/CLEVR_new000000/set_cams.npz'
)
with open(transform_matrix_filename) as f:
transforms = json.load(f)
figure_number = 0
fig = plt.figure(figure_number)
axes = fig.gca(projection='3d')
plt.cla()
# Plot cameras
idx = 0
for i in range(len(set_cams['set_rot'])):
print(set_cams['set_rot'][i])
print("i", i)
rot = set_cams['set_rot'][i]
t = set_cams['set_loc'][i]
rot_3 = gtsam.Rot3.RzRyRx(rot)
pose_i = gtsam.Pose3(rot_3, t)
if idx % 2 == 0:
gtsam_plot.plot_pose3(figure_number, pose_i, 1)
break
idx += 1
for i in transforms['frames']:
T = i['transform_matrix']
pose_i = gtsam.Pose3(T)
#print("T", T)
#print("pose_i",pose_i)
#print(set_cams['set_loc'][0])
#print(set_cams['set_rot'][0])
# Reference pose
# trying to look along x-axis, 0.2m above ground plane
# x forward, y left, z up (x-y is the ground plane)
upright = gtsam.Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2)
pose_j = gtsam.Pose3(upright, gtsam.Point3(0, 0, 0.2))
axis_length = 1
# plotting alternate poses
if idx % 2 == 0:
# gtsam_plot.plot_pose3(figure_number, pose_i, axis_length)
gtsam_plot.plot_pose3(figure_number, pose_j, axis_length)
idx += 1
x_axe, y_axe, z_axe = 10, 10, 10
# Draw
axes.set_xlim3d(-x_axe, x_axe)
axes.set_ylim3d(-y_axe, y_axe)
axes.set_zlim3d(0, z_axe)
plt.legend()
plt.show()
def test_sfm_factor2(self):
"""Evaluate residual with camera, pose and point as state variables"""
graph = gtsam.NonlinearFactorGraph()
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)
k = gtsam.Cal3Fisheye()
state.insert_cal3fisheye(camera_key, k)
state.insert_pose3(pose_key, gtsam.Pose3())
state.insert_point3(landmark_key, gtsam.Point3(self.obj_point))
factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noise_model,
pose_key, landmark_key,
camera_key)
graph.add(factor)
score = graph.error(state)
self.assertAlmostEqual(score, 0)
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)
