Develop Reference

Angle between 2 ArUco markers planes

I want to measure the deviation of the angle of an ArUco marker to a plane defined by a second reference ArUco marker.
A reference ArUco marker (M1) is fixed against a flat wall and a second ArUco marker (M2) is a few centimeters in front of that same wall. I want to know when the marker M2 is deviating more than 10 degrees from the xy plane of M1.
Here is an illustration of the configuration:
To do so, I thaught I should calculate the relative rotation between the pose rvec as explained in this post:
Relative rotation between pose (rvec)
that was proposing the following code:
def inversePerspective(rvec, tvec):
""" Applies perspective transform for given rvec and tvec. """
R, _ = cv2.Rodrigues(rvec)
R = np.matrix(R).T
invTvec = np.dot(R, np.matrix(-tvec))
invRvec, _ = cv2.Rodrigues(R)
return invRvec, invTvec
def relativePosition(rvec1, tvec1, rvec2, tvec2):
""" Get relative position for rvec2 & tvec2. Compose the returned rvec & tvec to use composeRT
with rvec2 & tvec2 """
rvec1, tvec1 = rvec1.reshape((3, 1)), tvec1.reshape((3, 1))
rvec2, tvec2 = rvec2.reshape((3, 1)), tvec2.reshape((3, 1))
# Inverse the second marker, the right one in the image
invRvec, invTvec = inversePerspective(rvec2, tvec2)
info = cv2.composeRT(rvec1, tvec1, invRvec, invTvec)
composedRvec, composedTvec = info[0], info[1]
composedRvec = composedRvec.reshape((3, 1))
composedTvec = composedTvec.reshape((3, 1))
return composedRvec, composedTvec
Computing the composedRvec, I get the following results :
With both ArUco markers in the same plane (composedRvec values in the top right corner) :
With both ArUco markers at a 90 degrees angle:
I do not really understand the results:
Ok for with the 0,0,0 composedRvec when markers in the same plane.
But why 0,1.78,0 in the second case?
What general condition should I have on the resulting composedRvec to tell me when the angle between the 2 markers is above 10 degrees?
Am I even following the right strategy with the composedRvec?
**** EDIT ***
Results of the 2 markers in the same xy plane with a 40° angle:
||composedRvec||= sqrt(0.619^2+0.529^2+0.711^2)=1.08 rad = 61.87°
**** EDIT 2 ***
By retaking measurements in the 40° angle configuration, I found out that the values are quite fluctuating even without modifying the set up or lightening. From time to time, I fall on the correct values:
||composedRvec||= sqrt(0.019^2+0.012^2+0.74^2)=0.74 rad = 42.4° which is quite accurate.
**** EDIT 3 ***
So here is my final code based on #Gilles-Philippe Paillé's edited answer:
import numpy as np
import cv2
import cv2.aruco as aruco
cap = cv2.VideoCapture(0, cv2.CAP_DSHOW) # Get the camera source
img_path='D:/your_path/'
# FILE_STORAGE_READ
cv_file = cv2.FileStorage(img_path+"camera.yml", cv2.FILE_STORAGE_READ)
matrix_coefficients = cv_file.getNode("K").mat()
distortion_coefficients = cv_file.getNode("D").mat()
nb_markers=2
def track(matrix_coefficients, distortion_coefficients):
while True:
ret, frame = cap.read()
# operations on the frame come here
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # Change grayscale
aruco_dict = aruco.custom_dictionary(nb_markers, 5)
parameters = aruco.DetectorParameters_create() # Marker detection parameters
# lists of ids and the corners beloning to each id
corners, ids, rejected_img_points = aruco.detectMarkers(gray,
aruco_dict,parameters=parameters,cameraMatrix=matrix_coefficients,distCoeff=distortion_coefficients)
# store rz1 and rz2
R_list=[]
if np.all(ids is not None): # If there are markers found by detector
for i in range(0, len(ids)): # Iterate in markers
# Estimate pose of each marker and return the values rvec and tvec---different from camera coefficients
rvec, tvec, markerPoints = aruco.estimatePoseSingleMarkers(corners[i], 0.02, matrix_coefficients,
distortion_coefficients)
(rvec - tvec).any() # get rid of that nasty numpy value array error
aruco.drawDetectedMarkers(frame, corners) # Draw A square around the markers
aruco.drawAxis(frame, matrix_coefficients, distortion_coefficients, rvec, tvec, 0.01) # Draw Axis
R, _ = cv2.Rodrigues(rvec)
# convert (np.matrix(R).T) matrix to array using np.squeeze(np.asarray()) to get rid off the ValueError: shapes (1,3) and (1,3) not aligned
R = np.squeeze(np.asarray(np.matrix(R).T))
R_list.append(R[2])
# Display the resulting frame
if len(R_list) == 2:
print('##############')
angle_radians = np.arccos(np.dot(R_list[0], R_list[1]))
angle_degrees=angle_radians*180/np.pi
print(angle_degrees)
cv2.imshow('frame', frame)
# Wait 3 milisecoonds for an interaction. Check the key and do the corresponding job.
key = cv2.waitKey(3000) & 0xFF
if key == ord('q'):
break
track(matrix_coefficients, distortion_coefficients)
And here are some results:
red -> real angle, white -> measured angle
This is out of the scope of the question but I find that the pose estimation is quite fluctuating. For example when the 2 markers are against the wall, the values easely jump from 9° to 37° without touching the system.

The result uses the Angle-axis representation, i.e., the norm of the vector is the angle of rotation (what you want), and the direction of the vector is the axis of rotation.
You are looking for θ = ||composedRvec||. Note that the result is in radians. The condition would be ||composedRvec|| > 10*π/180.
Edit: To only consider the angle between the Z-axis of both planes, convert the two rotation vectors rvec1 and rvec2 into matrices and extract the 3rd columns. The angle is then angle_radians = np.arccos(np.dot(rz1, rz2))

How to improve disparity map of stereo-pair images (python)

I captured the following 2 pictures from my mobile:Image1 , Image2
the camera was calibrated and I used this code to reconstruct 3D cloud point:
'''
Created by Omar Padierna "Para11ax" on Jan 1 2019
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
'''
import cv2
import numpy as np
import glob
from tqdm import tqdm
import PIL.ExifTags
import PIL.Image
from matplotlib import pyplot as plt
#=====================================
# Function declarations
#=====================================
#Function to create point cloud file
def create_output(vertices, colors, filename):
colors = colors.reshape(-1,3)
vertices = np.hstack([vertices.reshape(-1,3),colors])
ply_header = '''ply
format ascii 1.0
element vertex %(vert_num)d
property float x
property float y
property float z
property uchar red
property uchar green
property uchar blue
end_header
'''
with open(filename, 'w') as f:
f.write(ply_header %dict(vert_num=len(vertices)))
np.savetxt(f,vertices,'%f %f %f %d %d %d')
#Function that Downsamples image x number (reduce_factor) of times.
def downsample_image(image, reduce_factor):
for i in range(0,reduce_factor):
#Check if image is color or grayscale
if len(image.shape) > 2:
row,col = image.shape[:2]
else:
row,col = image.shape
image = cv2.pyrDown(image, dstsize= (col//2, row // 2))
return image
#=========================================================
# Stereo 3D reconstruction
#=========================================================
#Load camera parameters
ret = np.load('D:/Books/Pav Man/3DReconstruction-master/Reconstruction/camera_params/ret.npy')
K = np.load('D:/Books/Pav Man/3DReconstruction-master/Reconstruction/camera_params/K.npy')
dist = np.load('D:/Books/Pav Man/3DReconstruction-master/Reconstruction/camera_params/dist.npy')
#Specify image paths
img_path1 = 'D:/Books/Pav Man/3DReconstruction-master/Reconstruction/reconstruct_this/TestVi4.jpg'
img_path2 = 'D:/Books/Pav Man/3DReconstruction-master/Reconstruction/reconstruct_this/TestVi5.jpg'
#Load pictures
img_1 = cv2.imread(img_path1)
img_2 = cv2.imread(img_path2)
#Get height and width. Note: It assumes that both pictures are the same size. They HAVE to be same size and height.
h,w = img_2.shape[:2]
#Get optimal camera matrix for better undistortion
new_camera_matrix, roi = cv2.getOptimalNewCameraMatrix(K,dist,(w,h),1,(w,h))
#Undistort images
img_1_undistorted = cv2.undistort(img_1, K, dist, None, new_camera_matrix)
img_2_undistorted = cv2.undistort(img_2, K, dist, None, new_camera_matrix)
#Downsample each image 3 times (because they're too big)
img_1_downsampled = downsample_image(img_1_undistorted,3)
img_2_downsampled = downsample_image(img_2_undistorted,3)
#cv2.imwrite('undistorted_left.jpg', img_1_downsampled)
#cv2.imwrite('undistorted_right.jpg', img_2_downsampled)
#Set disparity parameters
#Note: disparity range is tuned according to specific parameters obtained through trial and error.
win_size = 1
min_disp = -1
max_disp = abs(min_disp) * 9 #min_disp * 9
num_disp = max_disp - min_disp # Needs to be divisible by 16
#Create Block matching object.
stereo = cv2.StereoSGBM_create(minDisparity= min_disp,
numDisparities = num_disp,
blockSize = 5,
uniquenessRatio = 5,
speckleWindowSize = 1,
speckleRange = 5,
disp12MaxDiff = 2,
P1 = 8*3*win_size**2,#8*3*win_size**2,
P2 =32*3*win_size**2) #32*3*win_size**2)
#Compute disparity map
print ("\nComputing the disparity map...")
disparity_map = stereo.compute(img_1_downsampled, img_2_downsampled)
#Show disparity map before generating 3D cloud to verify that point cloud will be usable.
plt.imshow(disparity_map,'gray')
plt.show()
#Generate point cloud.
print ("\nGenerating the 3D map...")
#Get new downsampled width and height
h,w = img_2_downsampled.shape[:2]
#Load focal length.
focal_length = np.load('D:/Books/Pav Man/3DReconstruction-master/Reconstruction/camera_params/FocalLength.npy')
#Perspective transformation matrix
#This transformation matrix is from the openCV documentation, didn't seem to work for me.
Q = np.float32([[1,0,0,-w/2.0],
[0,-1,0,h/2.0],
[0,0,0,-focal_length],
[0,0,1,0]])
#This transformation matrix is derived from Prof. Didier Stricker's power point presentation on computer vision.
#Link : https://ags.cs.uni-kl.de/fileadmin/inf_ags/3dcv-ws14-15/3DCV_lec01_camera.pdf
Q2 = np.float32([[1,0,0,0],
[0,-1,0,0],
[0,0,focal_length*0.05,0], #Focal length multiplication obtained experimentally.
[0,0,0,1]])
#Reproject points into 3D
points_3D = cv2.reprojectImageTo3D(disparity_map, Q2)
#Get color points
colors = cv2.cvtColor(img_1_downsampled, cv2.COLOR_BGR2RGB)
#Get rid of points with value 0 (i.e no depth)
mask_map = disparity_map > disparity_map.min()
#Mask colors and points.
output_points = points_3D[mask_map]
output_colors = colors[mask_map]
#Define name for output file
output_file = 'D:/Books/Pav Man/3DReconstruction-master/Reconstruction/reconstructed.ply'
#Generate point cloud
print ("\n Creating the output file... \n")
create_output(output_points, output_colors, output_file)
the following images are for disparity map and 3D model:
Disparity map
, Model
as you can see in the Model image, there are empty areas (red areas), how I can fill this area with points, and how to improve the disparity map.

The non-confident region(ie., algorithm not sure what this the correct disparity) is marked as black pixels in the disparity map. This is much expected behaviour.
You have to do some post processing to fill the map. Use Guided filter to complete map. If matlab provides any guided filter you can try once. In OpenCV "WLS" is the most common guided filter to get a filled map.

Projecting KITTI velodyne to image produces a narrow strip

I am trying to project the KITTI velodyne onto the left camera images. I followed the README in the KITTI devkit, but the result is off -- the points are projected as a narrow band on the top of the image. The band looks like it has some distribution, so I am suspecting I am doing something wrong with the calibration matrices. Or maybe in the PIL.ImageDraw.point?
The projection equation that I am using is per the KITTI devkit documentation:
x = P2 * R0_rect * Tr_velo_to_cam * y, where
y is a 4xN matrix with N points in XYZL format (L is the luminescence),
Tr_velo_to_cam is the 3x4 velodyne to camera transformation matrix
R0_rect is the 3x3 extrinsic camera rotation matrix
P2 is the 3x3 intrinsic camera projection matrix
Below is the code, STDIO of it, and the produced image.
test.py:
import numpy as np
import os
from PIL import Image, ImageDraw
DATASET_PATH = "<DATASET PATH HERE>"
vld_path = os.path.join(DATASET_PATH, "velodyne/{:06d}.bin")
img_path = os.path.join(DATASET_PATH, "image_2/{:06d}.png")
clb_path = os.path.join(DATASET_PATH, "calib/{:06d}.txt")
frame_num = 58
# Load files
img = Image.open(img_path.format(frame_num))
clb = {}
with open(clb_path.format(frame_num), 'r') as clb_f:
for line in clb_f:
calib_line = line.split(':')
if len(calib_line) < 2:
continue
key = calib_line[0]
value = np.array(list(map(float, calib_line[1].split())))
value = value.reshape((3, -1))
clb[key] = value
vld = np.fromfile(vld_path.format(frame_num), dtype=np.float32)
vld = vld.reshape((-1, 4)).T
print("img.shape:", np.shape(img))
print("P2.shape:", clb['P2'].shape)
print("R0_rect.shape:", clb['R0_rect'].shape)
print("Tr_velo_to_cam.shape:", clb['Tr_velo_to_cam'].shape)
print("vld.shape:", vld.shape)
# Reshape calibration files
P2 = clb['P2']
R0 = np.eye(4)
R0[:-1, :-1] = clb['R0_rect']
Tr = np.eye(4)
Tr[:-1, :] = clb['Tr_velo_to_cam']
# Prepare 3d points
pts3d = vld[:, vld[-1, :] > 0].copy()
pts3d[-1, :] = 1
# Project 3d points
pts3d_cam = R0 # Tr # pts3d
mask = pts3d_cam[2, :] >= 0 # Z >= 0
pts2d_cam = P2 # pts3d_cam[:, mask]
pts2d = (pts2d_cam / pts2d_cam[2, :])[:-1, :]
print("pts2d.shape:", pts2d.shape)
# Draw the points
img_draw = ImageDraw.Draw(img)
img_draw.point(pts2d, fill=(255, 0, 0))
img.show()
STDOUT:
$> python ./test.py
img.shape: (370, 1224, 3)
P2.shape: (3, 4)
R0_rect.shape: (3, 3)
Tr_velo_to_cam.shape: (3, 4)
vld.shape: (4, 115052)
pts2d.shape: (2, 53119)
Produced image:

Found the problem: Notice that the dimensions of the pts2d are (2, N), which means it has N points in total. However, the ImageDraw routine expects it to be either Nx2 or 1x2N row vector with alternating x and y values. Although I couldn't get the point routine to work with the Nx2 input, I put it in the for loop (after transposing the points), and it worked.
# ...
pts2d = (pts2d_cam / pts2d_cam[2, :])[:-1, :].T
print("pts2d.shape:", pts2d.shape)
# Draw the points
img_draw = ImageDraw.Draw(img)
for point in pts2d:
img_draw.point(point, fill=(255, 0, 0))
# ...

How to calculate an epipolar line with a stereo pair of images in Python OpenCV

How can I take two images of an object from different angles and draw epipolar lines on one based on points from the other?
For example, I would like to be able to select a point on the left picture using a mouse, mark the point with a circle, and then draw an epipolar line on the right image corresponding to the marked point.
I have 2 XML files which contain a 3x3 camera matrix and a list of 3x4 projection matrices for each picture. The camera matrix is K. The projection matrix for the left picture is P_left. The projection matrix for the right picture is P_right.
I have tried this approach:
Choose a pixel coordinate (x,y) in the left image (via mouse click)
Calculate a point p in the left image with K^-1 * (x,y,1)
Calulate the pseudo inverse matrix P+ of P_left (using np.linalg.pinv)
Calculate the epipole e' of the right image: P_right * (0,0,0,1)
Calculate the skew symmetric matrix e'_skew of e'
Calculate the Fundamental matrix F: e'_skew * P_right * P+
Calculate the epipolar line l' on the right image: F * p
Calculate a point p' in the right image: P_right * P+ * p
Transform p' and l back to pixel coordinates
Draw a line using cv2.line through p' and l

I just did this a few days ago and it works just fine. Here's the method I used:
Calibrate camera(s) to obtain camera matricies and distortion matricies (Using openCV getCorners and calibrateCamera, you can find lots of tutorials on this, but it sounds like you already have this info)
Perform stereo calibration with openCV stereoCalibrate(). It takes as parameters all of the camera and distortion matricies. You need this to determine the correlation between the two visual fields. You will get back several matricies, the rotation matrix R, translation vector T, essential matrix E and fundamental matrix F.
You then want to do undistortion using openCV getOptimalNewCameraMatrix and undistort(). This will get rid of a lot of camera aberrations (it will give you better results)
Finally, use openCV's computeCorrespondEpilines to calculate the lines and plot them. I will include some code below you can try out in Python. When I run it, I can get images like this (The colored points have their corresponding epilines drawn in the other image)
Heres some code (Python 3.0). It uses two static images and static points, but you could easily select the points with the cursor. You can also refer to the OpenCV docs on calibration and stereo calibration here.
import cv2
import numpy as np
# find object corners from chessboard pattern and create a correlation with image corners
def getCorners(images, chessboard_size, show=True):
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((chessboard_size[1] * chessboard_size[0], 3), np.float32)
objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2)*3.88 # multiply by 3.88 for large chessboard squares
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
for image in images:
frame = cv2.imread(image)
# height, width, channels = frame.shape # get image parameters
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None) # Find the chess board corners
if ret: # if corners were found
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) # refine corners
imgpoints.append(corners2) # add to corner array
if show:
# Draw and display the corners
frame = cv2.drawChessboardCorners(frame, chessboard_size, corners2, ret)
cv2.imshow('frame', frame)
cv2.waitKey(100)
cv2.destroyAllWindows() # close open windows
return objpoints, imgpoints, gray.shape[::-1]
# perform undistortion on provided image
def undistort(image, mtx, dist):
img = cv2.imread(image, cv2.IMREAD_GRAYSCALE)
image = os.path.splitext(image)[0]
h, w = img.shape[:2]
newcameramtx, _ = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
return dst
# draw the provided points on the image
def drawPoints(img, pts, colors):
for pt, color in zip(pts, colors):
cv2.circle(img, tuple(pt[0]), 5, color, -1)
# draw the provided lines on the image
def drawLines(img, lines, colors):
_, c, _ = img.shape
for r, color in zip(lines, colors):
x0, y0 = map(int, [0, -r[2]/r[1]])
x1, y1 = map(int, [c, -(r[2]+r[0]*c)/r[1]])
cv2.line(img, (x0, y0), (x1, y1), color, 1)
if __name__ == '__main__':
# undistort our chosen images using the left and right camera and distortion matricies
imgL = undistort("2L/2L34.bmp", mtxL, distL)
imgR = undistort("2R/2R34.bmp", mtxR, distR)
imgL = cv2.cvtColor(imgL, cv2.COLOR_GRAY2BGR)
imgR = cv2.cvtColor(imgR, cv2.COLOR_GRAY2BGR)
# use get corners to get the new image locations of the checcboard corners (undistort will have moved them a little)
_, imgpointsL, _ = getCorners(["2L34_undistorted.bmp"], chessboard_size, show=False)
_, imgpointsR, _ = getCorners(["2R34_undistorted.bmp"], chessboard_size, show=False)
# get 3 image points of interest from each image and draw them
ptsL = np.asarray([imgpointsL[0][0], imgpointsL[0][10], imgpointsL[0][20]])
ptsR = np.asarray([imgpointsR[0][5], imgpointsR[0][15], imgpointsR[0][25]])
drawPoints(imgL, ptsL, colors[3:6])
drawPoints(imgR, ptsR, colors[0:3])
# find epilines corresponding to points in right image and draw them on the left image
epilinesR = cv2.computeCorrespondEpilines(ptsR.reshape(-1, 1, 2), 2, F)
epilinesR = epilinesR.reshape(-1, 3)
drawLines(imgL, epilinesR, colors[0:3])
# find epilines corresponding to points in left image and draw them on the right image
epilinesL = cv2.computeCorrespondEpilines(ptsL.reshape(-1, 1, 2), 1, F)
epilinesL = epilinesL.reshape(-1, 3)
drawLines(imgR, epilinesL, colors[3:6])
# combine the corresponding images into one and display them
combineSideBySide(imgL, imgR, "epipolar_lines", save=True)
Hopefully this helps!

How to change the K matrix as well as the distortion coefficients to simulate the barrel distortion in python with the help of OpenCV

I have the intrensic paramaters of my camera, as well as the distortion coefficents, and I know how to calculate out the barrel distortion. - Mainly from this blogpost:
Barrel distortion calculation
However, now I wish to add the barrel distortion like it would be done by the camera itself.
The code for correcting the barrel distortion is the following:
import numpy as np
import cv2
from matplotlib import pyplot as plt
# Define camera matrix K
K = np.array([[1.051e+03,0,0],
[0, 1.0845e+03,0],
[964.4480,544.2625,1.]])
#Matrix was written in matlab style, hence it has to be transposed ...
K = K.transpose()
# Define distortion coefficients d
d = np.array([0.0719,-0.0833,0.0013,-6.1840e-04,0])
# Read an example image and acquire its size
img = cv2.imread("grid.png")
h, w = img.shape[:2]
# Generate new camera matrix from parameters
newcameramatrix, roi = cv2.getOptimalNewCameraMatrix(K, d, (w,h), 0)
# Generate look-up tables for remapping the camera image
mapx, mapy = cv2.initUndistortRectifyMap(K, d, None, newcameramatrix, (w, h), 5)
# Remap the original image to a new image
newimg = cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)
# Display old and new image
fig, (oldimg_ax, newimg_ax) = plt.subplots(1, 2)
oldimg_ax.imshow(img)
oldimg_ax.set_title('Original image')
newimg_ax.imshow(newimg)
newimg_ax.set_title('Unwarped image')
plt.show()
I tried to simualte the barrel distortion by using the inverse of th K-Matrix, or the transposed K-matrix, as well as multiplicating the d-vector with -1.
I transposed it via:
K = K.transpose()
or inverted it via:
K = np.linalg.inv(K)
Howver, this gave me only a black image. If I do not inverted / transpose it all I just get a negative radial resolution, however I need a positive radial distortion