def main():
filenum = 1
lidar_pitch = 9.5/180*np.pi
filename = "/home/henry/Documents/projects/avl/Detection/ground/data/points_data_"+repr(filenum)+".txt"
planes, pcds = read_points_data(filename)
tf = Rotation(roll=0.0, pitch=lidar_pitch, yaw=0.0)
for plane in planes:
plane.rotate_around_axis(axis="y", angle=-lidar_pitch)
for pcd in pcds:
pcd.rotate(tf)
pcd = pcds[0]
plane = planes[1]
# print(np.min(pcd.data[0,:]), np.max(pcd.data[0,:]))
vis(plane, pcd, dim_2d=True)
# the onlyfloor points
# pcd = pcds[2]
# plane = planes[2]
# the reestimated plane
sac = RANSAC(Plane3D, min_sample_num=3, threshold=0.1, iteration=50, method="MSAC")
plane2, _, _, _ = sac.ransac(pcd.data.T)
vis(plane2, pcd, dim_2d=True)
plt.show()
def __init__(self):
#Subscribers
self.image_sub = rospy.Subscriber('/ardrone/bottom/image_raw', Image,
self.image_callback)
self.sub_vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre',
TransformStamped,
self.update_quadrotor_state)
#Global variables
self.image = Image
self.bridge = CvBridge()
self.quad_state = TransformStamped()
self.T_body2vicon = np.identity(4)
#wait for msg for initiating processing
rospy.wait_for_message('/ardrone/bottom/image_raw', Image)
#time flags for detecting when bag file is done publishing
self.current_time = rospy.Time.now().to_sec()
self.last_msg_time = rospy.Time.now().to_sec()
#Define landmark locations
self.landmarks = np.array([[4.32, 8.05], [0.874, 5.49], [7.68, 4.24],
[4.27, 1.23]])
#list of
self.landmark_prev_save_rad = [
float('inf'),
float('inf'),
float('inf'),
float('inf')
]
self.save_radius = 0.5
#pose graph
self.G = {}
#load images for classification
img1 = cv2.imread('cn_tower.png')
img2 = cv2.imread('casa_loma.png')
img3 = cv2.imread('nathan_philips_square.png')
img4 = cv2.imread('princes_gates.png')
self.landmark_ref_images = [img1, img2, img3, img4]
self.landmark_names = [
'cn_tower', 'casa_loma', 'nathan_philips_square', 'princes_gates'
]
#Initialize ransac
self.ransac = RANSAC()
def main(im1loc, im2loc, output):
correspondences = flipxy(get_matches(im1loc, im2loc))
model = RANSAC(correspondences)
im1, im2 = Image.open(im1loc).convert('L'), Image.open(im2loc).convert('L')
w1, h1 = im1.size
w2, h2 = im2.size
panorama = im2.transform((w1 + w2, h1), Image.PERSPECTIVE, model,
Image.BILINEAR)
panorama.save("transformed.jpg")
im1_arr, panorama_arr = np.asarray(im1), np.asarray(panorama)
final = np.copy(panorama_arr)
for i in range(len(panorama_arr)):
for j in range(len(panorama_arr[i])):
if j < len(im1_arr[i]):
pixel1 = im1_arr[i, j]
pixel2 = panorama_arr[i, j]
if pixel2 == 0 and pixel1 != 0: final[i, j] = pixel1
if pixel2 == 0 and pixel1 == 0: final[i, j] = 0
if pixel2 != 0 and pixel1 != 0:
final[i, j] = pixel2 #(int(pixel1) + int(pixel2))/2
if pixel2 != 0 and pixel1 == 0: final[i, j] = pixel2
test = Image.fromarray(np.uint8(final)).convert('L')
test.save(output)
def __init__(self):
# specify topic and data type
self.sub_bbox_1 = rospy.Subscriber("camera1/detection/vision_objects",
DetectedObjectArray,
self.bbox_array_callback)
self.sub_bbox_6 = rospy.Subscriber("camera6/detection/vision_objects",
DetectedObjectArray,
self.bbox_array_callback)
self.sub_pcd = rospy.Subscriber("/points_raw", PointCloud2,
self.pcd_callback)
# self.sub_pose = rospy.Subscriber("/current_pose", PoseStamped, self.pose_callback)
# Publisher
self.pub_intersect_markers = rospy.Publisher(
"/vision_objects_position_rviz", MarkerArray, queue_size=10)
self.pub_plane_markers = rospy.Publisher("/estimated_plane_rviz",
MarkerArray,
queue_size=10)
self.pub_plane = rospy.Publisher("/estimated_plane",
Plane,
queue_size=10)
self.pub_pcd_inlier = rospy.Publisher("/points_inlier",
PointCloud2,
queue_size=10)
self.pub_pcd_outlier = rospy.Publisher("/points_outlier",
PointCloud2,
queue_size=10)
self.cam1 = camera_setup_1()
self.cam6 = camera_setup_6()
self.plane = Plane3D(-0.157, 0, 0.988, 1.9)
self.plane_world = None
self.plane_tracker = None
self.sac = RANSAC(Plane3D,
min_sample_num=3,
threshold=0.22,
iteration=200,
method="MSAC")
self.tf_listener = TransformListener()
self.tfmr = Transformer()
self.tf_ros = TransformerROS()
def test_ransac(self):
"""
Test ransac by following steps:
1. make random homography
2. make inliers with noise
3. make outliers
4. RUN
"""
N_TOTAL = 1000
N_INLIER = 500
N_ITER = trial_count(N_INLIER / N_TOTAL, 4)
N_OUTLIER = N_TOTAL - N_INLIER
NOISE_SIZE = 0
# step 1
gt = np.random.random((3, 3))
gt[2, :] *= 100
gt[2, 2] = 1
# step 2
hom = Homography(gt)
x = np.random.random((N_INLIER, 2))
y = hom.transform(x)
x += np.random.random((N_INLIER, 2)) * NOISE_SIZE
y += np.random.random((N_INLIER, 2)) * NOISE_SIZE
inliers = np.concatenate((x, y), axis=1)
# step 3
outliers = np.random.random((N_OUTLIER, 4))
data = np.concatenate((inliers, outliers), axis=0)
# step 4
from ransac import RANSAC
ransac_inst = RANSAC(
n_sample=4,
n_iter=N_ITER,
err_thres=0.1,
)
model = ProjectionModel()
best_func, best_inliers = ransac_inst(data, model)
np.testing.assert_almost_equal(best_func.M, gt)
def PlaneDetect(points, normals, epsilon, alpha):
# 平面検出
# index: pointsからフィットした点のインデックス
plane, index, num = RANSAC(points, normals, epsilon=epsilon, alpha=alpha)
selectIndex = np.array(
[True if i in index else False for i in range(points.shape[0])])
# フィット点を平面射影
# plane_points: 射影後の3d座標点群
# UVvector: 射影後の2d座標点群
plane_points, UVvector, u, v, O = Plane2DProjection(points[index], plane)
# # プロット準備
# fig = plt.figure()
# ax = Axes3D(fig)
# ax.set_xlabel("X")
# ax.set_ylabel("Y")
# ax.set_zlabel("Z")
# # 点群描画
# X, Y, Z = Disassemble(points)
# MX, MY, MZ = X[index], Y[index], Z[index]
# PX, PY, PZ = Disassemble(plane_points)
# ax.plot(X, Y, Z, marker="o", linestyle='None', color="white")
# ax.plot(MX, MY, MZ, marker=".", linestyle='None', color="red")
# ax.plot(PX, PY, PZ, marker=".", linestyle='None', color="blue")
# # 平面描画
# plot_implicit(ax, plane.f_rep, points, AABB_size=1, contourNum=15)
# plt.show()
# plt.close()
# # 射影2d点群描画
# UX, UY = Disassemble2d(UVvector)
# plt.plot(UX, UY, marker="o",linestyle="None",color="red")
# plt.show()
# plt.close()
return UVvector, plane, u, v, O, selectIndex
# Compute CCL image
ccl = computeCCL(filteredLabelNum, label)
################## RANSAC PROGRAM ##################
i = 1 # Counter
# Run RANSAC on each connected component
for labelNum in filteredLabelNum :
logging.debug(f'COMPONENT : {i}')
# Compiling the coordinates of the pixels in the contour into data points
dataPoints = compileDataPoints(label, labelNum)
# Initialize an ellispe class
ellipse = Ellipse()
# Run RANSAC on the data points
model = RANSAC(
ellipse,
dataPoints,
args.sample_size,
args.iteration,
args.threshold,
args.tolerance
)
# Draw ellipse
if (model != []) :
drawEllipse(out, model, args.crop)
logging.info(f'Component {i} process complete.')
i = i + 1
####################################################
if args.crop :
out = cv.bitwise_and(out, img)
out = makeTransparent(out)
out = cv.resize(out, (w, h))
# Save images
kp2, ds2 = matcher_obj.getFeatures(img2)
matches = matcher_obj.match(ds1, ds2)
print("Number of matches: ", len(matches))
print('Keypoint detection and matching done in {:.6f}s'.format(time.time() -
t0))
src_orig = np.float32([kp1[m.queryIdx].pt for m in matches])
dst_orig = np.float32([kp2[m.trainIdx].pt for m in matches])
src_orig = np.vstack((src_orig.T, np.ones((1, len(matches)))))
dst_orig = np.vstack((dst_orig.T, np.ones((1, len(matches)))))
##################
# Outlier removal.
##################
t1 = time.time()
ransac = RANSAC(config.M, config.thr)
src_fine, dst_fine = ransac(img1, img2, src_orig, dst_orig)
print("Number of inliers: ", src_fine.shape[1])
print('RANSAC sampling done in {:.6f}s'.format(time.time() - t1))
########################
# Global homography (H).
########################
print("\nDLT (projective transform) on inliers")
# Refine homography using DLT on inliers.
Hg = homography_fit(src_fine, dst_fine)
#############################################
# Image stitching with global homography (H).
#############################################
# Obtaining size of canvas (using global Homography)
def DetectViewer2(path):
#点群,法線,OBBの対角線の長さ 取得
#points, X, Y, Z, normals, length = PreProcess(path)
#自作の点群を扱いたいときはこちら
#points, X, Y, Z, normals, length = PreProcess2()
# PLYデータを扱いたいときはこちら
points, X, Y, Z, normals, length = ViewPLY(path)
#元の点群データを保存しておく
ori_points = points[:, :]
#ori_normals = normals[:, :]
# 検知した図形のリスト
fitting_figures = []
print("points:{}".format(points.shape[0]))
###グラフ初期化###
ax = ViewerInit(points, X, Y, Z, normals)
while points.shape[0] >= ori_points.shape[0] * 0.05:
print("points:{}".format(points.shape[0]))
scores = []
paras = []
indices = []
###最適化###
for fig_type in [0,1,2,3]:
###グラフ初期化##
#ax = ViewerInit(points, X, Y, Z, normals)
#図形フィッティング
#result = figOptimize(points, normals, length, fig_type)
#result = figOptimize2(X, Y, Z, normals, length, fig_type)
result, MX, MY, MZ, num, index = RANSAC(fig_type, points, normals, X, Y, Z, length)
print(result)
#fig_typeに応じた図形を選択
if fig_type==0:
figure = F.sphere(result)
elif fig_type==1:
figure = F.plane(result)
elif fig_type==2:
figure = F.cylinder(result)
elif fig_type==3:
figure = F.cone(result)
#図形描画
#plot_implicit(ax, figure.f_rep, points, AABB_size=1, contourNum=50)
#図形に対して"条件"を満たす点群を数える、これをスコアとする
#MX, MY, MZ, num, index = CountPoints(figure, points, X, Y, Z, normals, epsilon=0.08*length, alpha=np.pi/9)
#print("AFTER_num:{}".format(num))
#条件を満たす点群, 最適化された図形描画
#ax.plot(MX,MY,MZ,marker=".",linestyle='None',color="orange")
#最後に.show()を書いてグラフ表示
#plt.show()
#スコアとパラメータ,インデックスを保存
scores.append(num)
paras.append(result)
indices.append(index)
print("="*100)
if max(scores) <= ori_points.shape[0] * 0.05:
print("おわり！")
break
###グラフ初期化###
ax = ViewerInit(points, X, Y, Z, normals)
# スコアが最大の図形を描画
best_fig = scores.index(max(scores))
# スコアが最大の図形を保存
fitting_figures.append([best_fig, paras[best_fig]])
if best_fig==0:
figure = F.sphere(paras[best_fig])
print("球の勝ち")
elif best_fig==1:
figure = F.plane(paras[best_fig])
print("平面の勝ち")
elif best_fig==2:
figure = F.cylinder(paras[best_fig])
print("円柱の勝ち")
elif best_fig==3:
figure = F.cone(paras[best_fig])
print("円錐の勝ち")
# フィット点描画
ax.plot(X[indices[best_fig]],Y[indices[best_fig]],Z[indices[best_fig]],\
marker=".",linestyle='None',color="orange")
# 図形描画
plot_implicit(ax, figure.f_rep, points, AABB_size=1, contourNum=15)
plt.show()
#フィットした点群を削除
points = np.delete(points, indices[best_fig], axis=0)
normals = np.delete(normals, indices[best_fig], axis=0)
X, Y, Z = Disassemble(points)
###グラフ初期化###
#ax = ViewerInit(points, X, Y, Z, normals)
#plt.show()
##################
print("="*100)
print(len(fitting_figures), fitting_figures)
plt.show()
def OptiViewer(path, fig_type):
#点群,法線,OBBの対角線の長さ 取得
#points, X, Y, Z, normals, length = PreProcess(path)
#自作の点群を扱いたいときはこちら
#points, X, Y, Z, normals, length = PreProcess2()
# PLYデータを扱いたいときはこちら
points, X, Y, Z, normals, length = ViewPLY(path)
#U, V, W = Disassemble(normals)
#法線を描画
#ax.quiver(X, Y, Z, U, V, W, length=0.1, normalize=True)
while True:
print("points:{}".format(points.shape[0]))
#グラフの枠を作っていく
fig = plt.figure()
ax = Axes3D(fig)
#軸にラベルを付けたいときは書く
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
#点群を描画
ax.plot(X,Y,Z,marker="o",linestyle='None',color="white")
#OBBを描画
OBBViewer(ax, points)
###最適化###
#result = figOptimize(points, normals, length, fig_type)
#result = figOptimize2(X, Y, Z, normals, length, fig_type)
result, MX, MY, MZ, num, index = RANSAC(fig_type, points, normals, X, Y, Z, length)
print(result)
#fig_typeに応じた図形を選択
if fig_type==0:
figure = F.sphere(result)
elif fig_type==1:
figure = F.plane(result)
elif fig_type==2:
figure = F.cylinder(result)
else:
figure = F.cone(result)
#最適化された図形を描画
plot_implicit(ax, figure.f_rep, points, AABB_size=1, contourNum=15)
#S_optを検出
#MX, MY, MZ, num, index = CountPoints(figure, points, X, Y, Z, normals, epsilon=0.08*length, alpha=np.pi/9)
print("num:{}".format(num))
ax.plot(MX,MY,MZ,marker=".",linestyle='None',color="red")
# グラフ表示
plt.show()
# フィットした点群を削除
points = np.delete(points, index, axis=0)
normals = np.delete(normals, index, axis=0)
X, Y, Z = Disassemble(points)
class RoadEstimation:
def __init__(self):
# specify topic and data type
self.sub_bbox_1 = rospy.Subscriber("camera1/detection/vision_objects",
DetectedObjectArray,
self.bbox_array_callback)
self.sub_bbox_6 = rospy.Subscriber("camera6/detection/vision_objects",
DetectedObjectArray,
self.bbox_array_callback)
self.sub_pcd = rospy.Subscriber("/points_raw", PointCloud2,
self.pcd_callback)
# self.sub_pose = rospy.Subscriber("/current_pose", PoseStamped, self.pose_callback)
# Publisher
self.pub_intersect_markers = rospy.Publisher(
"/vision_objects_position_rviz", MarkerArray, queue_size=10)
self.pub_plane_markers = rospy.Publisher("/estimated_plane_rviz",
MarkerArray,
queue_size=10)
self.pub_plane = rospy.Publisher("/estimated_plane",
Plane,
queue_size=10)
self.pub_pcd_inlier = rospy.Publisher("/points_inlier",
PointCloud2,
queue_size=10)
self.pub_pcd_outlier = rospy.Publisher("/points_outlier",
PointCloud2,
queue_size=10)
self.cam1 = camera_setup_1()
self.cam6 = camera_setup_6()
self.plane = Plane3D(-0.157, 0, 0.988, 1.9)
self.plane_world = None
self.plane_tracker = None
self.sac = RANSAC(Plane3D,
min_sample_num=3,
threshold=0.22,
iteration=200,
method="MSAC")
self.tf_listener = TransformListener()
self.tfmr = Transformer()
self.tf_ros = TransformerROS()
def display_bboxes_in_world(self, cam, bboxes):
vis_array = MarkerArray()
xlist, ylist = [], []
for box_id, bbox in enumerate(bboxes):
d, C = cam.bounding_box_to_ray(bbox)
intersection = self.plane.plane_ray_intersection(d, C)
# print(intersection[0,0],'\t', intersection[1,0],'\t', intersection[2,0])
marker = visualize_marker(intersection,
mkr_id=box_id,
scale=0.5,
frame_id="velodyne",
mkr_color=[0.0, 0.8, 0.0, 1.0])
vis_array.markers.append(marker)
xlist.append(intersection[0, 0])
ylist.append(intersection[1, 0])
self.pub_intersect_markers.publish(vis_array)
# plt.pause(0.001)
def bbox_array_callback(self, msg):
if msg.header.frame_id == "camera1":
cam = self.cam1
elif msg.header.frame_id == "camera6":
cam = self.cam6
else:
rospy.logwarn(
"unrecognized frame id {}, bounding box callback in road estimation fail",
msg.header.frame_id)
return
# rospy.loginfo("camera {:d} message received!!".format(camera.id))
bboxes = []
for obj in msg.objects:
# rospy.loginfo("{}".format(obj.label))
# rospy.loginfo("x:{} y:{} width:{} height:{} angle:{}".format(obj.x, obj.y, obj.width, obj.height, obj.angle))
bbox = BoundingBox(obj.x,
obj.y,
obj.width,
obj.height,
obj.angle,
label=obj.label)
bboxes.append(bbox)
self.display_bboxes_in_world(cam, bboxes)
def estimate_plane(self, pcd):
# threshold_z = [2.0, -0.5]
# pcd_z = clip_pcd_by_distance_plane(pcd, self.plane, threshold_z, in_only=True)
vec1 = np.array([1, 0, 0])
vec2 = np.array([0, 0, 1])
pt1 = np.array([0, 0, 0])
threshold = [-3.0, 6.0]
plane_from_vec = Plane3D.create_plane_from_vectors_and_point(
vec1, vec2, pt1)
pcd_close = clip_pcd_by_distance_plane(pcd,
plane_from_vec,
threshold,
in_only=True)
pcd_close = pcd_close.extract_low()
seed = 0
np.random.seed(seed)
plane1, _, _, _ = self.sac.ransac(pcd_close.data.T,
constraint=self.plane.param,
constraint_threshold=0.5)
# vis(plane1, pcd, dim_2d=True)
# normal = vec1.reshape([-1,1]) / np.linalg.norm(vec1)
# depth = np.matmul(pcd_close.data.T , normal).reshape([-1])
distance = plane1.distance_to_plane(pcd_close.data.T)
# threshold_outer = 0.3
# threshold_inner = 0.1
# mask_outer = distance < threshold_outer
# mask_inner = distance < threshold_inner
# bin_dist = 5.0
# depth_min = np.min(depth)
# bin_num = int((np.max(depth) - depth_min)/ bin_dist) + 1
# for i in range(bin_num):
# depth_thred_min, depth_thred_max = i*bin_dist+depth_min, (i+1)*bin_dist+depth_min
# mask_depth = np.logical_and(depth > depth_thred_min, depth < depth_thred_max)
# part_inner = np.logical_and(mask_depth, mask_inner)
# part_outer = np.logical_and(mask_depth, mask_outer)
# sum_outer = np.sum(part_outer)
# sum_inner = np.sum(part_inner)
# if sum_outer == 0:
# ratio = 1
# else:
# ratio = float(sum_inner) / sum_outer
# if not ratio == 1:
# print(i, "{:.1f}".format(depth_thred_min), "{:.1f}".format(depth_thred_max), sum_inner, sum_outer, "{:.4f}".format(ratio))
print('Plane params:', plane1.param.T)
threshold_inlier = 0.15
pcd_inlier = pcd_close.data[:, distance <= threshold_inlier]
pcd_outlier = pcd_close.data[:, distance > threshold_inlier]
return plane1, pcd_inlier, pcd_outlier
def create_and_publish_plane_markers(self,
plane,
frame_id='velodyne',
center=None,
normal=None):
if normal is None:
v1 = np.array([[0, 0, 1.0]]).T
else:
v1 = normal
v2 = np.array([[plane.a, plane.b, plane.c]]).T
q_self = Quaternion_self.create_quaternion_from_vector_to_vector(
v1, v2)
q = Quaternion(q_self.x, q_self.y, q_self.z, q_self.w)
euler = np.array(euler_from_quaternion(
(q.x, q.y, q.z, q.w))) * 180 / np.pi
print("Euler: ", euler)
marker_array = MarkerArray()
if center is None:
center = [10, 0, (-plane.a * 10 - plane.d) / plane.c]
marker = visualize_marker(center,
frame_id=frame_id,
mkr_type='cube',
orientation=q,
scale=[20, 10, 0.05],
lifetime=30,
mkr_color=[0.0, 0.8, 0.0, 0.8])
marker_array.markers.append(marker)
return marker_array
# @profile # profiling for analysis
def pcd_callback(self, msg):
rospy.logwarn("Getting pcd at: %d.%09ds, (%d,%d)",
msg.header.stamp.secs, msg.header.stamp.nsecs,
msg.height, msg.width)
pcd_original = pointcloud2_to_xyz_array(msg)
pcd = PointCloud(pcd_original.T)
self.plane, pcd_inlier, pcd_outlier = self.estimate_plane(pcd)
transform_matrix, trans, rot, euler = get_transformation(
frame_from='/velodyne',
frame_to='/world',
time_from=msg.header.stamp,
time_to=msg.header.stamp,
static_frame='/world',
tf_listener=self.tf_listener,
tf_ros=self.tf_ros)
if not transform_matrix is None:
plane_world_param = np.matmul(
np.linalg.inv(transform_matrix).T,
np.array(
[[self.plane.a, self.plane.b, self.plane.c,
self.plane.d]]).T)
plane_world_param = plane_world_param / np.linalg.norm(
plane_world_param[0:3])
if self.plane_tracker is None:
self.plane_tracker = Tracker(msg.header.stamp,
plane_world_param)
else:
self.plane_tracker.predict(msg.header.stamp)
self.plane_tracker.update(plane_world_param)
print("plane_world:", plane_world_param.T)
print("plane_traker:", self.plane_tracker.filter.x_post.T)
# self.plane_world = Plane3D(plane_world_param[0,0], plane_world_param[1,0], plane_world_param[2,0], plane_world_param[3,0])
self.plane_world = Plane3D(self.plane_tracker.filter.x_post[0, 0],
self.plane_tracker.filter.x_post[1, 0],
self.plane_tracker.filter.x_post[2, 0],
self.plane_tracker.filter.x_post[3, 0])
center_pos = np.matmul(
transform_matrix,
np.array([[
10, 0, (-self.plane.a * 10 - self.plane.d) / self.plane.c,
1
]]).T)
center_pos = center_pos[0:3].flatten()
# normal = np.matmul( transform_matrix, np.array([[0., 0., 1., 0.]]).T)
# normal = normal[0:3]
normal = None
marker_array = self.create_and_publish_plane_markers(
self.plane_world,
frame_id='world',
center=center_pos,
normal=normal)
self.pub_plane_markers.publish(marker_array)
plane_msg = Plane()
plane_msg.coef[0], plane_msg.coef[1], plane_msg.coef[
2], plane_msg.coef[
3] = self.plane.a, self.plane.b, self.plane.c, self.plane.d
self.pub_plane.publish(plane_msg)
# pcd_msg_inlier = create_point_cloud(pcd_inlier.T, frame_id='velodyne')
# pcd_msg_outlier = create_point_cloud(pcd_outlier.T, frame_id='velodyne')
pcd_msg_inlier = xyz_array_to_pointcloud2(pcd_inlier.T,
stamp=msg.header.stamp,
frame_id='velodyne')
pcd_msg_outlier = xyz_array_to_pointcloud2(pcd_outlier.T,
stamp=msg.header.stamp,
frame_id='velodyne')
self.pub_pcd_inlier.publish(pcd_msg_inlier)
self.pub_pcd_outlier.publish(pcd_msg_outlier)
rospy.logwarn("Finished plane estimation")
def pose_callback(self, msg):
rospy.logdebug("Getting pose at: %d.%09ds", msg.header.stamp.secs,
msg.header.stamp.nsecs)
def main():
# Parse input arguments
Parser = argparse.ArgumentParser()
Parser.add_argument(
'--Path',
default="../Oxford_dataset/stereo/centre",
help='Path to dataset, Default:../Oxford_dataset/stereo/centre')
Parser.add_argument(
'--ransacEpsilonThreshold',
default=0.01,
help='Threshold used for deciding inlier during RANSAC, Default:0.01')
Parser.add_argument(
'--inlierRatioThreshold',
default=0.8,
help='Threshold to consider a fundamental matrix as valid, Default:0.85'
)
Args = Parser.parse_args()
path = Args.Path
epsilonThresh = Args.ransacEpsilonThreshold
inlierRatioThresh = Args.inlierRatioThreshold
# pre-process data to get undistorted images
dataPrep.undistortImage(path_to_model='./model', path_to_images=path)
# extract calibration matrix from
K = dataPrep.extractCalibrationMatrix(path_to_model='./model')
# extract images from undistort
new_path = './undistort'
filesnumber = sorted(glob.glob(new_path + "/frame*.png"))
# extract calibration matrix
K = dataPrep.extractCalibrationMatrix(path_to_model='./model')
T = []
T_own = []
R = []
H = np.identity(4)
H_own = np.identity(4)
for imageIndex in range(50, len(filesnumber) - 60):
print('Image number:', imageIndex)
# bgrImages, vizImages = extractImages(new_path, 20)
# ------------Process pair of images -------------------------------------
img1 = cv2.imread(new_path + "/frame" + str(imageIndex) + ".png", -1)
# histogram equalization of the image
equ1 = cv2.equalizeHist(cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY))
# blur the image
img1_gray = cv2.GaussianBlur(equ1, (3, 3), 0)
# second image
img2 = cv2.imread(new_path + "/frame" + str(imageIndex + 1) + ".png",
-1)
# histogram equalization of the image
equ2 = cv2.equalizeHist(cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY))
# blur the image
img2_gray = cv2.GaussianBlur(equ2, (3, 3), 0)
# extract images from the input array
pixelsImg1, pixelsImg2 = features.extractSIFTFeatures(
img1_gray, img2_gray)
# vizMatches(img1, img2, pixelsImg1, pixelsImg2) # visualize the feature matches before RANSAC
F, inlierImg1Pixels, inlierImg2Pixels, _, _ = RANSAC(
pixelsImg1, pixelsImg2, epsilonThresh, inlierRatioThresh)
# vizMatches(img1, img2, inlierImg1Pixels, inlierImg2Pixels) # visualize after RANSAC
E, Cset, Rset = extractPose.extractPose(F, K)
# take points in image frame before checking chirality condition
points1new = np.hstack(
(np.array(inlierImg1Pixels), np.ones(
(len(inlierImg1Pixels), 1)))).T
points2new = np.hstack(
(np.array(inlierImg2Pixels), np.ones(
(len(inlierImg2Pixels), 1)))).T
points1k = np.linalg.inv(K).dot(points1new)
points1 = points1k.T
points2k = np.linalg.inv(K).dot(points2new)
points2 = points2k.T
# check chirality and obtain the true pose
newR, newT, X = checkChirality(Rset, Cset, points1, points2)
# perform non-liner triangulation
# X = triangulation.nonLinearTriangulation(inlierImg1Pixels, inlierImg2Pixels, H_own, X)
temp_H = np.hstack((newR, newT.reshape(3, 1)))
temp_H = np.vstack((temp_H, [0, 0, 0, 1]))
H_own = np.matmul(H_own, temp_H)
print('-----------------------own', H_own[0:3, 3])
T_own.append(H_own[0:3, 3])
#---------Inbuilt------------------
# E_cv2, mask1 = cv2.findFundamentalMat(np.array(pixelsImg1), np.array(pixelsImg2), method=cv2.RANSAC,
# focal=964.828979, pp=(643.788025, 484.40799), prob=0.85, threshold=0.5)
E_cv2, mask1 = cv2.findEssentialMat(np.array(pixelsImg1),
np.array(pixelsImg2),
method=cv2.RANSAC,
focal=964.828979,
pp=(643.788025, 484.40799),
prob=0.85,
threshold=3.0)
points, r, t, mask = cv2.recoverPose(E_cv2,
np.array(pixelsImg1),
np.array(pixelsImg2),
focal=964.828979,
pp=(643.788025, 484.40799),
mask=mask1)
newH = np.hstack((r.T, -r.T.dot(t.reshape(3, 1))))
newH = np.vstack((newH, [0, 0, 0, 1]))
H = np.matmul(H, newH)
T.append(H[0:3, 3])
print(H[0:3, 3])
print('--------------------------')
#----------------------------------
# visualize
vizCameraPose(T_own, T)
plt.legend()
plt.show()
class LandmarkClassifier(object):
"""Class for classifying landmarks
"""
def __init__(self):
#Subscribers
self.image_sub = rospy.Subscriber('/ardrone/bottom/image_raw', Image,
self.image_callback)
self.sub_vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre',
TransformStamped,
self.update_quadrotor_state)
#Global variables
self.image = Image
self.bridge = CvBridge()
self.quad_state = TransformStamped()
self.T_body2vicon = np.identity(4)
#wait for msg for initiating processing
rospy.wait_for_message('/ardrone/bottom/image_raw', Image)
#time flags for detecting when bag file is done publishing
self.current_time = rospy.Time.now().to_sec()
self.last_msg_time = rospy.Time.now().to_sec()
#Define landmark locations
self.landmarks = np.array([[4.32, 8.05], [0.874, 5.49], [7.68, 4.24],
[4.27, 1.23]])
#list of
self.landmark_prev_save_rad = [
float('inf'),
float('inf'),
float('inf'),
float('inf')
]
self.save_radius = 0.5
#pose graph
self.G = {}
#load images for classification
img1 = cv2.imread('cn_tower.png')
img2 = cv2.imread('casa_loma.png')
img3 = cv2.imread('nathan_philips_square.png')
img4 = cv2.imread('princes_gates.png')
self.landmark_ref_images = [img1, img2, img3, img4]
self.landmark_names = [
'cn_tower', 'casa_loma', 'nathan_philips_square', 'princes_gates'
]
#Initialize ransac
self.ransac = RANSAC()
def image_callback(self, msg):
"""Callback for image topic
"""
#store the current pose which will be attached to the current
#camera frame
current_quad_state = copy.deepcopy(self.quad_state)
self.image = msg
self.T_body2vicon[0, 3] = current_quad_state.transform.translation.x
self.T_body2vicon[1, 3] = current_quad_state.transform.translation.y
self.T_body2vicon[2, 3] = current_quad_state.transform.translation.z
q = [
current_quad_state.transform.rotation.x,
current_quad_state.transform.rotation.y,
current_quad_state.transform.rotation.z,
current_quad_state.transform.rotation.w
]
R = quaternion_matrix(q)
self.T_body2vicon[0:3, 0:3] = R[0:3, 0:3]
self.last_msg_time = rospy.Time.now().to_sec()
def update_quadrotor_state(self, msg):
"""Callback for vicon topic
"""
self.quad_state = msg
def save_image(self, image, image_name):
"""Function for saving images to file
"""
im_pil = IM.fromarray(image)
b, g, r = im_pil.split()
im_pil = IM.merge("RGB", (r, g, b))
im_pil.save(image_name + '.png')
def save_landmark_images(self):
"""Function that saves landmark images from project.bag, images
that minimizes the distance between the drone and landmarks are
saved
"""
#run ros loop to save images
while not rospy.is_shutdown():
#Exit rosloop if project.bag is done publishing
self.current_time = rospy.Time.now().to_sec()
if (self.current_time - self.last_msg_time) > 2:
break
#Store the current pose and image
T_b2vic = copy.deepcopy(self.T_body2vicon)
img = copy.deepcopy(self.image)
#convert image to opencv format
cv2_img = self.bridge.imgmsg_to_cv2(img, "bgr8")
#detect obstacle pixels
pos = T_b2vic[0:2, 3]
for i in range(self.landmarks.shape[0]):
#landmark position
l_pos = self.landmarks[i, :]
rad = np.linalg.norm(pos - l_pos)
#save the landmark if it will reduce the distance between drone and landmark
if rad < self.save_radius and rad < self.landmark_prev_save_rad[
i]:
self.save_image(cv2_img, str(i))
self.G[i] = T_b2vic #save pose for that landmark
self.landmark_prev_save_rad[i] = rad
def classify_landmarks(self):
"""Function that classifies the landmarks based on the largest
set of inliers from ransac
"""
#Classify Landmarks
for i in range(self.landmarks.shape[0]):
#load saved images
imgq = cv2.imread(str(i) + '.png')
max_inliers = 0
for j in range(len(self.landmark_ref_images)):
#load reference image
imgt = self.landmark_ref_images[j]
#perform ransac outlier rejection
inlier_count, avg_angle = self.ransac.ransac(imgq, imgt)
#largest set of inliers record
if inlier_count > max_inliers:
best_count = inlier_count
best_angle = avg_angle
max_inliers = inlier_count
best_idx = j
#calculate landmark orientation
R = self.G[i][0:3, 0:3]
(phi, theta, psi) = euler_from_matrix(R, 'rxyz')
landmark_orientation = psi + best_angle
#print results
print('Image :' + str(i) + '.png')
print('location : ', self.landmarks[i])
print('Landmark :' + self.landmark_names[best_idx])
print('Landmark Orientation :' + str(landmark_orientation))
print('inlier_count :' + str(best_count))