def test_console(self):
with patch.object(Console, "get_version", return_value="testing"):
Console.warn("This is a warning")
Console.error("This is an error message")
Console.info("This is an informative message")
Console.banner()
Console.get_username()
Console.get_hostname()
Console.get_date()
Console.get_version()
for i in range(1, 10):
Console.progress(i, 10)
def fit_and_save(self, cloud, processed_folder):
"""Fit mean plane and uncertainty bounding planes to point cloud
Parameters
----------
cloud : ndarray of shape (nx3)
Point cloud
processed_folder : Path
Path of the processed folder where outputs are written
Returns
-------
String
Plane parameters of the mean plane and a set of uncertainty
bounding planes of the point cloud in yaml-file format.
"""
total_no_points = len(cloud)
# Fit mean plane
Console.info("Fitting a plane to", total_no_points, "points...")
p = Plane([1, 0, 0, 1.5])
mean_plane, self.inliers_cloud_list = p.fit(cloud, self.mdt)
# p.plot(cloud=cloud)
filename = time.strftime("pointclouds_and_best_model_%Y%m%d_%H%M%S.html")
plot_pointcloud_and_planes(
[np.array(cloud), np.array(self.inliers_cloud_list)],
[np.array(mean_plane)],
str(processed_folder / filename),
)
scale = 1.0 / mean_plane[0]
mean_plane = np.array(mean_plane) * scale
mean_plane = mean_plane.tolist()
Console.info("Least squares found", len(self.inliers_cloud_list), "inliers")
if len(self.inliers_cloud_list) < 0.5 * len(cloud) * self.gip:
Console.warn("The number of inliers found are off from what you expected.")
Console.warn(" * Expected inliers:", len(cloud) * self.gip)
Console.warn(" * Found inliers:", len(self.inliers_cloud_list))
Console.warn(
"Check the output cloud to see if the found plane makes sense."
)
Console.warn("Try to increase your distance threshold.")
inliers_cloud = np.array(self.inliers_cloud_list)
mean_x = np.mean(inliers_cloud[:, 0])
mean_y = np.mean(inliers_cloud[:, 1])
mean_z = np.mean(inliers_cloud[:, 2])
mean_xyz = np.array([mean_x, mean_y, mean_z])
# Determine minimum distance between points as function of inlier
# point cloud size
std_y = np.std(inliers_cloud[:, 1])
std_z = np.std(inliers_cloud[:, 2])
# print("Min y: " + str(np.min(inliers_cloud[:, 1])))
# print("Max y: " + str(np.max(inliers_cloud[:, 1])))
# print("Std y: " + str(std_y))
# print("Min z: " + str(np.min(inliers_cloud[:, 2])))
# print("Max z: " + str(np.max(inliers_cloud[:, 2])))
# print("Std z: " + str(std_z))
min_dist = 2 * math.sqrt(std_y ** 2 + std_z ** 2)
Console.info("Minimum distance for poisson disc sampling: {}".format(min_dist))
min_sin_angle = 0.866 # = sin(60°)
# Append 1 to the points, so they can be multiplied (dot product) with
# plane paramters to find out if they are in front, behind or on a
# plane.
self.inliers_1 = np.concatenate(
[inliers_cloud, np.ones((inliers_cloud.shape[0], 1))], axis=1
)
planes_enclose_inliers = False
Console.info("Generating uncertainty planes...")
max_uncertainty_planes = 300
tries = 0
failed_distance = 0
failed_angle = 0
while (
planes_enclose_inliers is False
and len(self.uncertainty_planes) < max_uncertainty_planes
):
tries += 1
point_cloud_local = random.sample(self.inliers_cloud_list, 3)
# Check if the points are sufficiently far apart and not aligned
p0p1 = point_cloud_local[1][1:3] - point_cloud_local[0][1:3]
p0p2 = point_cloud_local[2][1:3] - point_cloud_local[0][1:3]
p1p2 = point_cloud_local[2][1:3] - point_cloud_local[1][1:3]
p0p1_norm = np.linalg.norm(p0p1)
p0p2_norm = np.linalg.norm(p0p2)
p1p2_norm = np.linalg.norm(p1p2)
# Poisson disc sampling: reject points that are too close together
if p0p1_norm < min_dist or p0p2_norm < min_dist or p1p2_norm < min_dist:
failed_distance += 1
if failed_distance % 100000 == 0:
Console.info(
"Combinations rejected due to distance criterion",
"(Poisson disk sampling):",
failed_distance,
"times,",
"due to angle criterion:",
failed_angle,
"times",
)
continue
# Reject points that are too closely aligned
if abs(np.cross(p0p1, p0p2)) / (p0p1_norm * p0p2_norm) < min_sin_angle:
failed_angle += 1
if failed_angle % 100000 == 0:
print(
"Combinations rejected due to distance criterion",
"(Poisson disk sampling):",
failed_distance,
"times,",
"due to angle criterion:",
failed_angle,
"times",
)
continue
# Compute plane through the 3 points and append to list
self.triples.append(np.array(point_cloud_local))
self.uncertainty_planes.append(plane_through_3_points(point_cloud_local))
Console.info(
"Number of planes: ",
len(self.uncertainty_planes),
", " "Number of tries so far: ",
tries,
".",
"Combinations rejected due to distance criterion",
"(Poisson disk sampling):",
failed_distance,
"times,",
"due to angle criterion:",
failed_angle,
"times",
)
planes_enclose_inliers = self._do_planes_enclose_inliers()
Console.info(
"... finished generating {} uncertainty planes.".format(
len(self.uncertainty_planes)
)
)
if len(self.uncertainty_planes) >= max_uncertainty_planes:
Console.warn("Stopped due to reaching max_uncertainty_planes")
filename = time.strftime(
"pointclouds_and_uncertainty_planes_all_" "%Y%m%d_%H%M%S.html"
)
plot_pointcloud_and_planes(
self.triples + [np.array(cloud), inliers_cloud],
self.uncertainty_planes,
str(processed_folder / filename),
)
# uncomment to save for debugging
# np.save('inliers_cloud.npy', inliers_cloud)
# for i, plane in enumerate(self.uncertainty_planes):
# np.save('plane' + str(i) + '.npy', plane)
# Console.info("Removing uncertainty planes that do not contribute...")
# self._remove_unnecessary_uncertainty_planes()
# Console.info("... finished removing non-contributing planes. Reduced"
# "number of uncertainty planes to {}."
# .format(len(self.uncertainty_planes)))
# assert self._check_if_planes_enclose_inliers() # Sanity check
filename = time.strftime(
"pointclouds_and_uncertainty_planes_%Y%m%d_" "%H%M%S.html"
)
plot_pointcloud_and_planes(
self.triples + [np.array(cloud), inliers_cloud],
self.uncertainty_planes,
str(processed_folder / filename),
)
yaml_msg = (
"mean_xyz_m: "
+ str(mean_xyz.tolist())
+ "\n"
+ "mean_plane: "
+ str(mean_plane)
+ "\n"
)
if len(self.uncertainty_planes) > 0:
uncertainty_planes_str = "uncertainty_planes:\n"
for i, up in enumerate(self.uncertainty_planes):
uncertainty_planes_str += " - " + str(up.tolist()) + "\n"
yaml_msg += uncertainty_planes_str
yaml_msg += (
'date: "'
+ Console.get_date()
+ '" \n'
+ 'user: "******" \n'
+ 'host: "'
+ Console.get_hostname()
+ '" \n'
+ 'version: "'
+ Console.get_version()
+ '" \n'
)
return yaml_msg
