def ransac_render(cloud):
pointCloud = vpc.VtkPointCloud()
model_p = pcl.SampleConsensusModelPlane(cloud)
ransac = pcl.RandomSampleConsensus(model_p)
ransac.set_DistanceThreshold(1)
ransac.computeModel()
plane = ransac.get_Inliers()
print(len(plane))
find_plane = 0 ## index of inlier inside cloud
cnt = 0
for i in range(0, cloud.size):
plane_index = plane[find_plane]
if plane_index == i:
if find_plane < len(plane) - 1:
find_plane += 1
else:
pointCloud.addPoint(cloud[i], -1)
cnt += 1
print(cnt)
print(cloud.size)
renderer = vtk.vtkRenderer()
renderWindow = vtk.vtkRenderWindow()
renderWindow.AddRenderer(renderer)
renderWindowInteractor = vtk.vtkRenderWindowInteractor()
renderWindowInteractor.SetRenderWindow(renderWindow)
renderer.AddActor(pointCloud.point_vtkActor)
renderer.SetBackground(0.0, 0.0, 0.0)
renderWindow.Render()
renderWindowInteractor.Initialize()
renderWindowInteractor.Start()
def ransac(cloud):
model_p = pcl.SampleConsensusModelPlane(cloud)
ransac = pcl.RandomSampleConsensus(model_p)
ransac.set_DistanceThreshold(0.1)
ransac.computeModel()
plane = ransac.get_Inliers()
final = []
find_plane = 0 ## index of inlier inside cloud
for i in range(0, cloud.size):
plane_index = plane[find_plane]
if plane_index == i:
if find_plane < len(plane) - 1:
find_plane += 1
else:
final.append(cloud[i])
return pcl.PointCloud(final)
def test_ModelPlane(self):
model_p = pcl.SampleConsensusModelPlane(self.p)
ransac = pcl.RandomSampleConsensus(model_p)
ransac.set_DistanceThreshold(.01)
ransac.computeModel()
inliers = ransac.get_Inliers()
# print(str(len(inliers)))
self.assertNotEqual(len(inliers), 0)
final = pcl.PointCloud()
if len(inliers) != 0:
finalpoints = np.zeros((len(inliers), 3), dtype=np.float32)
for i in range(0, len(inliers)):
finalpoints[i][0] = self.p[inliers[i]][0]
finalpoints[i][1] = self.p[inliers[i]][1]
finalpoints[i][2] = self.p[inliers[i]][2]
final.from_array(finalpoints)
self.assertNotEqual(final.size, 0)
pass
def ModelPlane(cloud_filter, threshold=0.1, ext=True):
modle_p = pcl.SampleConsensusModelPlane(cloud_filter)
ransac = pcl.RandomSampleConsensus(modle_p)
ransac.set_DistanceThreshold(threshold)
ransac.computeModel()
# ransac.setNegative(True)
inliers = ransac.get_Inliers()
# final.extract()
final = pcl.PointCloud()
finalpoints = np.zeros((len(inliers), 3), dtype=np.float32)
for i in range(0, len(inliers)):
finalpoints[i][0] = cloud_filter[inliers[i]][0]
finalpoints[i][1] = cloud_filter[inliers[i]][1]
finalpoints[i][2] = cloud_filter[inliers[i]][2]
# finalpoints[1][0] = cloud_filter[inliers[1]][0]
# finalpoints[1][1] = cloud_filter[inliers[1]][1]
# finalpoints[1][2] = cloud_filter[inliers[1]][2]
final = pcl.PointCloud()
final.from_array(finalpoints)
if ext == True:
return cloud_filter.extract(inliers, True)
else:
return final
def extract_points_3D(self, pointcloud_msg, now, proc_results, calibrator_instance):
# print("lidar_points:", len(lidar_points), ":", [x.shape for x in lidar_points])
# Log PID
rospy.loginfo('3D Picker PID: [%d]' % os.getpid())
# Extract points data
points = ros_numpy.point_cloud2.pointcloud2_to_array(pointcloud_msg)
points = np.asarray(points.tolist())
if (len(points.shape)>2):
points = points.reshape(-1, points.shape[-1])
points = points[~np.isnan(points).any(axis=1), :]
points2pcd(points, os.path.join(PKG_PATH, CALIB_PATH, self.camera_name+"_pcd", str(self.lidar_points_num)+".pcd"))
if (self.args.transform_pointcloud):
points = (self.lidar_rotation_matrix.dot(points[:,:3].T) + self.lidar_translation_vector.reshape(-1,1)).T
# Select points within chessboard range
inrange = np.where((np.abs(points[:, 0]) < 7) &
(points[:, 1] < 25) &
(points[:, 1] > 0) &
# (points[:, 2] < 4) &
(points[:, 2] > 0.2))
points = points[inrange[0]]
if points.shape[0] < 10:
rospy.logwarn('Too few PCL points available in range')
return
# pptk viewer
viewer = pptk.viewer(points[:, :3])
viewer.set(lookat=(0,0,4))
viewer.set(phi=-1.57)
viewer.set(theta=0.4)
viewer.set(r=4)
viewer.set(floor_level=0)
viewer.set(point_size=0.02)
rospy.loginfo("Press [Ctrl + LeftMouseClick] to select a chessboard point. Press [Enter] on viewer to finish selection.")
pcl_pointcloud = pcl.PointCloud(points[:,:3].astype(np.float32))
region_growing = pcl_pointcloud.make_RegionGrowing(searchRadius=self.chessboard_diagonal/5.0)
indices = []
viewer.wait()
indices = viewer.get('selected')
if (len(indices) < 1):
rospy.logwarn("No point selected!")
# self.terminate_all()
viewer.close()
return
rg_indices = region_growing.get_SegmentFromPoint(indices[0])
points_color = np.zeros(len(points))
points_color[rg_indices] = 1
viewer.attributes(points_color)
# viewer.wait()
chessboard_pointcloud = pcl_pointcloud.extract(rg_indices)
plane_model = pcl.SampleConsensusModelPlane(chessboard_pointcloud)
pcl_RANSAC = pcl.RandomSampleConsensus(plane_model)
pcl_RANSAC.set_DistanceThreshold(0.01)
pcl_RANSAC.computeModel()
inliers = pcl_RANSAC.get_Inliers()
# random select a fixed number of lidar points to reduce computation time
inliers = np.random.choice(inliers, self.args.lidar_points)
chessboard_points = np.asarray(chessboard_pointcloud)
proc_results.put([np.asarray(chessboard_points[inliers, :3])])
points_color = np.zeros(len(chessboard_points))
points_color[inliers] = 1
viewer.load(chessboard_points[:,:3])
viewer.set(lookat=(0,0,4))
viewer.set(phi=-1.57)
viewer.set(theta=0.4)
viewer.set(r=2)
viewer.set(floor_level=0)
viewer.set(point_size=0.02)
viewer.attributes(points_color)
viewer.attributes(points_color)
rospy.loginfo("Check the chessboard segmentation in the viewer.")
viewer.wait()
viewer.close()
# pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
# ransac.setDistanceThreshold (.01);
# ransac.computeModel();
# ransac.getInliers(inliers);
# }
# else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 )
# {
# pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
# ransac.setDistanceThreshold (.01);
# ransac.computeModel();
# ransac.getInliers(inliers);
# }
###
# inliers = vector[int]
model_s = pcl.SampleConsensusModelSphere(cloud)
model_p = pcl.SampleConsensusModelPlane(cloud)
if argc > 1:
if argvs == "-f":
ransac = pcl.RandomSampleConsensus(model_p)
ransac.set_DistanceThreshold(.01)
ransac.computeModel()
inliers = ransac.get_Inliers()
elif argvs == "-sf":
ransac = pcl.RandomSampleConsensus(model_s)
ransac.set_DistanceThreshold(.01)
ransac.computeModel()
inliers = ransac.get_Inliers()
else:
inliers = []
else:
inliers = []
def extract_points_3D(self, pcd_file, proc_results):
# print("lidar_points:", len(lidar_points), ":", [x.shape for x in lidar_points])
# Log PID
rospy.loginfo('3D Picker PID: [%d] file name: %s' %
(os.getpid(), os.path.basename(pcd_file)))
# Extract points data
points = pcl.load(pcd_file).to_array()
# Select points within chessboard range
inrange = np.where((np.abs(points[:, 0]) < 8) & (points[:, 1] < 25)
& (points[:, 1] > 0) &
# (points[:, 2] < 4) &
(points[:, 2] > 0.2))
points = points[inrange[0]]
if points.shape[0] < 10:
rospy.logwarn('Too few PCL points available in range')
return
# pptk viewer
viewer = pptk.viewer(points[:, :3])
viewer.set(lookat=(0, 0, 4))
viewer.set(phi=-1.57)
viewer.set(theta=0.4)
viewer.set(r=4)
viewer.set(floor_level=0)
viewer.set(point_size=0.02)
rospy.loginfo(
"Press [Ctrl + LeftMouseClick] to select a chessboard point. Press [Enter] on viewer to finish selection."
)
pcl_pointcloud = pcl.PointCloud(points[:, :3].astype(np.float32))
region_growing = pcl_pointcloud.make_RegionGrowing(
searchRadius=self.chessboard_diagonal / 5.0)
indices = []
viewer.wait()
indices = viewer.get('selected')
if (len(indices) < 1):
rospy.logwarn("No point selected! Skip this frame.")
viewer.close()
return
rg_indices = region_growing.get_SegmentFromPoint(indices[0])
points_color = np.zeros(len(points))
points_color[rg_indices] = 1
viewer.attributes(points_color)
# viewer.wait()
chessboard_pointcloud = pcl_pointcloud.extract(rg_indices)
plane_model = pcl.SampleConsensusModelPlane(chessboard_pointcloud)
pcl_RANSAC = pcl.RandomSampleConsensus(plane_model)
pcl_RANSAC.set_DistanceThreshold(0.01)
pcl_RANSAC.computeModel()
inliers = pcl_RANSAC.get_Inliers()
inliers = np.random.choice(inliers, self.args.lidar_points)
chessboard_points = np.asarray(chessboard_pointcloud)
proc_results.put([np.asarray(chessboard_points[inliers, :3])])
points_color = np.zeros(len(chessboard_points))
points_color[inliers] = 1
viewer.load(chessboard_points[:, :3])
viewer.set(lookat=(0, 0, 4))
viewer.set(phi=-1.57)
viewer.set(theta=0.4)
viewer.set(r=2)
viewer.set(floor_level=0)
viewer.set(point_size=0.02)
viewer.attributes(points_color)
rospy.loginfo("Check the chessboard segmentation in the viewer.")
viewer.wait()
viewer.close()
def calibrate(points3D=None):
# Load corresponding points
folder = os.path.join(PKG_PATH, CALIB_PATH)
if points3D is None and os.path.exists(
os.path.join(folder, 'lidar_points.txt')):
points3D = np.loadtxt(os.path.join(folder, 'lidar_points.txt'))
# print("points3D:", points3D.shape)
# Estimate extrinsics
# best-fit linear plane
# method 1: PCA method
# pca = PCA()
# pca.fit(points3D[:,:3])
# normal_vector = pca.components_[2]
# method 2: lstsq method
# A = np.c_[points3D[:,0], points3D[:,1], np.ones(points3D.shape[0])]
# C,_,_,_ = scipy.linalg.lstsq(A, points3D[:,2]) # coefficients
# # evaluate it on grid
# # Z = C[0]*X + C[1]*Y + C[2]
# # or expressed using matrix/vector product
# #Z = np.dot(np.c_[XX, YY, np.ones(XX.shape)], C).reshape(X.shape)
# # normal vector
# # print("C:", C)
# normal_vector = np.array([C[0], C[1], -1])
# normal_vector = np.array(normal_vector) / np.linalg.norm(normal_vector)
# translation_vector = np.array([[0],[0],[C[2]]]) # shpae: (3,1)
# method 3: PCL RANSAC fit plane
pcl_pointcloud = pcl.PointCloud(points3D[:, :3].astype(np.float32))
plane_model = pcl.SampleConsensusModelPlane(pcl_pointcloud)
pcl_RANSAC = pcl.RandomSampleConsensus(plane_model)
pcl_RANSAC.set_DistanceThreshold(0.03)
pcl_RANSAC.computeModel()
inliers = pcl_RANSAC.get_Inliers()
points3D = points3D[inliers, :3]
pca = PCA()
pca.fit(points3D)
normal_vector = pca.components_[2]
translation_vector = np.zeros((3, 1))
if (normal_vector[2] < 0): normal_vector = -normal_vector
# print("normal vector:", normal_vector)
# rotation_angle = np.arccos(normal_vector[2])
# rotation_vector_temp = np.cross(normal_vector, np.array([0,0,1]))
# rotation_vector = rotation_angle * rotation_vector_temp / np.linalg.norm(rotation_vector_temp)
# Convert rotation vector
rotation_matrix = rotation_matrix_from_vectors(normal_vector,
np.array([0, 0, 1]))
euler = euler_from_matrix(rotation_matrix)
# use reverse transform because of issues in the BIT-IVRC sensor_driver package
reverse_euler_angles = np.zeros((1, 3))
reverse_rotation_matrix = rotation_matrix_from_vectors(
np.array([0, 0, 1]), normal_vector)
reverse_euler_angles = euler_from_matrix(reverse_rotation_matrix)
alfa, beta, gama = reverse_euler_angles
alfa = -alfa
# x_offset, y_offset, z_offset = C
transform_matrix_calibration = np.zeros((3, 3))
transform_matrix_calibration[0, 0] = np.cos(beta) * np.cos(gama) - np.sin(
beta) * np.sin(alfa) * np.sin(gama)
transform_matrix_calibration[1, 0] = np.cos(beta) * np.sin(gama) + np.sin(
beta) * np.sin(alfa) * np.cos(gama)
transform_matrix_calibration[2, 0] = -np.cos(alfa) * np.sin(beta)
transform_matrix_calibration[0, 1] = -np.cos(alfa) * np.sin(gama)
transform_matrix_calibration[1, 1] = np.cos(alfa) * np.cos(gama)
transform_matrix_calibration[2, 1] = np.sin(alfa)
transform_matrix_calibration[0, 2] = np.sin(beta) * np.cos(gama) + np.cos(
beta) * np.sin(alfa) * np.sin(gama)
transform_matrix_calibration[1, 2] = np.sin(beta) * np.sin(gama) - np.cos(
beta) * np.sin(alfa) * np.cos(gama)
transform_matrix_calibration[2, 2] = np.cos(alfa) * np.cos(beta)
# points3D = np.matmul(points3D[:,:3], transform_matrix_calibration)
points3D = transform_matrix_calibration.dot(points3D[:, :3].T).T
z_mean = np.mean(points3D[:, 2])
# print("Z statitics:", z_mean, np.std(points3D[:,2]))
# viewer = pptk.viewer(points3D[:,:3])
# viewer.wait()
translation_vector[2] = -z_mean
rospy.loginfo(
"The following values are used for tf static_transform_publisher")
# print("rotation_matrix * normal_vector:", np.dot(rotation_matrix, normal_vector))
# print("rotation_matrix * [0,0,1]:", np.dot(rotation_matrix, np.array([0,0,1])))
# Display results
print('Euler angles (RPY):', euler)
print('Rotation Matrix:', rotation_matrix)
print('Translation Offsets:', translation_vector)
# Save extrinsics
if (not os.path.exists(folder)):
os.makedirs(folder)
np.savez(os.path.join(folder, args.lidar_name + '_extrinsics.npz'),
euler=euler,
R=rotation_matrix,
T=translation_vector)
# with open(os.path.join(folder, args.lidar_name + '_extrinsics.yaml'),'w') as f:
# yaml.dump({'euler':list(euler), 'R':rotation_matrix.tolist(), 'T':translation_vector.tolist()},f)
calibration_string = lidarCalibrationYamlBuf(rotation_matrix,
translation_vector,
euler,
name=args.lidar_name)
if (not os.path.exists(folder)):
os.mkdir(folder)
with open(os.path.join(folder, args.lidar_name + '_extrinsics.yaml'),
'w') as f:
f.write(calibration_string)
reverse_euler_angles = np.array(reverse_euler_angles)
reverse_euler_angles *= 180. / np.pi
rospy.loginfo(
"Calibration finished. Copy the following offsets and angles to the lua file in sensor_driver package."
)
rospy.loginfo(
"x_offset: %f, y_offset: %f, z_offset: %f" %
(translation_vector[0], translation_vector[1], translation_vector[2]))
rospy.loginfo("x_angle: %f, y_angle: %f, z_angle: %f" %
(-reverse_euler_angles[0], reverse_euler_angles[1],
reverse_euler_angles[2]))
return reverse_rotation_matrix, transform_matrix_calibration, translation_vector
