Difference between revisions of "Script: homography estimation.py"
Line 1: | Line 1: | ||
<syntaxhighlight lang='python'> | <syntaxhighlight lang='python'> | ||
#!/usr/bin/env python3 | #!/usr/bin/env python3 | ||
+ | |||
""" Copyright (C) 2020 RidgeRun, LLC (http://www.ridgerun.com) | """ Copyright (C) 2020 RidgeRun, LLC (http://www.ridgerun.com) | ||
All Rights Reserved. | All Rights Reserved. | ||
Line 11: | Line 12: | ||
back to RidgeRun without any encumbrance. """ | back to RidgeRun without any encumbrance. """ | ||
− | """ Tool for | + | """ Tool for estimating the homography matrix """ |
+ | |||
import argparse | import argparse | ||
+ | import json | ||
+ | import sys | ||
+ | |||
import cv2 | import cv2 | ||
− | |||
import numpy as np | import numpy as np | ||
− | |||
HOMOGRAPHY_DIMENSION = 3 | HOMOGRAPHY_DIMENSION = 3 | ||
MIN_MATCHES = 4 | MIN_MATCHES = 4 | ||
+ | KERNEL_SIZE = 5 | ||
+ | CHANNELS = 3 | ||
+ | DEFAULT_REPROJ_ERROR = 4.0 | ||
+ | DEFAULT_RATIO = 0.75 | ||
+ | DEFAULT_SIGMA = 0 | ||
+ | DEFAULT_FOV = 70 | ||
+ | DEFAULT_OVERLAP = 15 | ||
+ | DEFAULT_CROP = 0 | ||
+ | DEFAULT_UNDISTORT = True | ||
+ | |||
− | def drawMatches(imageA, imageB, | + | def drawMatches(imageA, imageB, keypointsA, keypointsB, matches, status): |
''' | ''' | ||
Returns an image with the matches found. | Returns an image with the matches found. | ||
Line 28: | Line 41: | ||
Parameters: | Parameters: | ||
imageA (np.array): Image A | imageA (np.array): Image A | ||
− | imageB (np. | + | imageB (np.array): Image B |
− | + | keypointsA (np.array): Keypoints for image A | |
− | + | keypointsB (np.array): Keypoints for image B | |
− | matches (np.array): List of found matches between image A and image B | + | matches (np.array): List of found matches between image |
+ | A and image B | ||
status (int): status of homography estimation | status (int): status of homography estimation | ||
Returns: | Returns: | ||
− | Image | + | Image (np.array) |
''' | ''' | ||
# initialize the output visualization image | # initialize the output visualization image | ||
− | ( | + | (heightA, widthA) = imageA.shape[:2] |
− | ( | + | (heightB, widthB) = imageB.shape[:2] |
− | vis = np.zeros((max( | + | vis = np.zeros( |
− | vis[0: | + | (max( |
− | vis[0: | + | heightA, |
+ | heightB), | ||
+ | widthA + | ||
+ | widthB, | ||
+ | CHANNELS), | ||
+ | dtype="uint8") | ||
+ | vis[0:heightA, 0:widthA] = imageA | ||
+ | vis[0:heightB, widthA:] = imageB | ||
for ((trainIdx, queryIdx), s) in zip(matches, status): | for ((trainIdx, queryIdx), s) in zip(matches, status): | ||
Line 50: | Line 71: | ||
if s == 1: | if s == 1: | ||
# draw the match | # draw the match | ||
− | ptA = (int( | + | ptA = (int(keypointsA[queryIdx][0]), int(keypointsA[queryIdx][1])) |
− | ptB = (int( | + | ptB = (int(keypointsB[trainIdx][0]) + |
+ | widthA, int(keypointsB[trainIdx][1])) | ||
cv2.line(vis, ptA, ptB, (0, 255, 0), 1) | cv2.line(vis, ptA, ptB, (0, 255, 0), 1) | ||
Line 64: | Line 86: | ||
Parameters: | Parameters: | ||
gray (np.array): Input image in grayscale. | gray (np.array): Input image in grayscale. | ||
− | mask (np. | + | mask (np.array): Mask to apply before the feature |
+ | extraction. | ||
Returns: | Returns: | ||
Line 78: | Line 101: | ||
− | def matchKeypoints( | + | def matchKeypoints(keypointsA, keypointsB, featuresA, featuresB, |
ratio, reprojThresh): | ratio, reprojThresh): | ||
''' | ''' | ||
− | Function that | + | Function that searches the correspondiencies for the |
− | + | given descriptors (featuresA and featuresB) and with those | |
+ | correspondencies, the homography matrix is calculated. | ||
Parameters: | Parameters: | ||
− | + | keypointsA (np.array): Keypoints of image A. | |
− | + | keypointsB (np.array): Keypoints of image B. | |
− | featuresA (np. | + | featuresA (np.array): Descriptors of image A |
− | featuresB (np. | + | featuresB (np.array): Descriptors of image B |
− | ratio (float): Max distance between a possible correspondence. | + | ratio (float): Max distance between a possible |
− | reprojThresh (float): Reprojection error of the homography estimation | + | correspondence. |
+ | reprojThresh (float): Reprojection error of the | ||
+ | homography estimation | ||
Returns: | Returns: | ||
− | Tuple with the resulting matches, the homography H and the estimation status. | + | Tuple with the resulting matches, the homography H |
+ | and the estimation status. | ||
''' | ''' | ||
Line 111: | Line 138: | ||
if len(matches) >= MIN_MATCHES: | if len(matches) >= MIN_MATCHES: | ||
# construct the two sets of points | # construct the two sets of points | ||
− | ptsA = np.float32([ | + | ptsA = np.float32([keypointsA[i] for (_, i) in matches]) |
− | ptsB = np.float32([ | + | ptsB = np.float32([keypointsB[i] for (i, _) in matches]) |
# compute the homography between the two sets of points | # compute the homography between the two sets of points | ||
Line 128: | Line 155: | ||
def imageEnhancement(img, sigma): | def imageEnhancement(img, sigma): | ||
''' | ''' | ||
− | + | Returns an image with some enhancement applied. Remove the | |
− | + | noise of the input image img using a Gaussian filter and | |
+ | then convert it to grayscale. | ||
Parameters: | Parameters: | ||
Line 138: | Line 166: | ||
Image | Image | ||
''' | ''' | ||
− | |||
− | |||
− | |||
# Remove noise | # Remove noise | ||
− | img = cv2.GaussianBlur(img, ( | + | img = cv2.GaussianBlur(img, (KERNEL_SIZE, KERNEL_SIZE), sigma) |
# Convert to grayscale | # Convert to grayscale | ||
Line 151: | Line 176: | ||
− | def removeDistortion(img, | + | def removeDistortion(img, cameraMatrix, distortionParameters): |
''' | ''' | ||
− | Returns an image with the distortion provoked by the camera lens. | + | Returns an image with the distortion provoked by the camera lens removed. |
Parameters: | Parameters: | ||
img (np.array): Image img. | img (np.array): Image img. | ||
− | + | cameraMatrix (np.array): Camera matrix with a dimension | |
− | + | of (3x3). | |
+ | distortionParameters (np.array): Vector with the | ||
+ | distortion coefficients. | ||
Returns: | Returns: | ||
Undistorted image. | Undistorted image. | ||
Line 167: | Line 194: | ||
# Generate new camera matrix from parameters | # Generate new camera matrix from parameters | ||
− | + | newCameraMatrix, roi = cv2.getOptimalNewCameraMatrix( | |
+ | cameraMatrix, distortionParameters, (w, h), 0) | ||
# Generate look-up tables for remapping the camera image | # Generate look-up tables for remapping the camera image | ||
− | + | mapX, mapY = cv2.initUndistortRectifyMap( | |
− | + | cameraMatrix, distortionParameters, None, newCameraMatrix, (w, h), 5) | |
# Remap the original image to a new image | # Remap the original image to a new image | ||
− | + | newImg = cv2.remap(img, mapX, mapY, cv2.INTER_LINEAR) | |
− | return | + | return newImg |
Line 186: | Line 214: | ||
Returns: | Returns: | ||
− | Tuple | + | Tuple the read variables: (K, D, reprojError, |
+ | matchRatio, sigma, overlap, crop, fov, undistort) | ||
''' | ''' | ||
Line 193: | Line 222: | ||
# load config variables | # load config variables | ||
− | reprojError = float(data['reprojError']) | + | if data['reprojError'] is not None: |
− | matchRatio = float(data['matchRatio']) | + | reprojError = float(data['reprojError']) |
− | sigma = float(data['sigma']) | + | else: |
− | fov = float(data['fov']) | + | reprojError = DEFAULT_REPROJ_ERROR |
− | overlap = float(data['overlap']) | + | print("Key reprojError not found in JSON configuration file. " |
− | crop = float(data['crop']) | + | "Setting {} value".format(DEFAULT_REPROJ_ERROR)) |
− | undistort = bool(data['undistort']) | + | |
+ | if data['matchRatio'] is not None: | ||
+ | matchRatio = float(data['matchRatio']) | ||
+ | else: | ||
+ | matchRatio = DEFAULT_RATIO | ||
+ | print("Key matchRatio not found in JSON configuration file. " | ||
+ | "Setting {} value".format(DEFAULT_RATIO)) | ||
+ | |||
+ | if data['sigma'] is not None: | ||
+ | sigma = float(data['sigma']) | ||
+ | else: | ||
+ | sigma = DEFAULT_SIGMA | ||
+ | print("Key sigma not found in JSON configuration file. " | ||
+ | "Setting {} value".format(DEFAULT_SIGMA)) | ||
+ | |||
+ | if data['fov'] is not None: | ||
+ | fov = float(data['fov']) | ||
+ | else: | ||
+ | fov = DEFAULT_FOV | ||
+ | print("Key fov not found in JSON configuration file. " | ||
+ | "Setting {} value".format(DEFAULT_FOV)) | ||
+ | |||
+ | if data['overlap'] is not None: | ||
+ | overlap = float(data['overlap']) | ||
+ | else: | ||
+ | overlap = DEFAULT_OVERLAP | ||
+ | print("Key overlap not found in JSON configuration file. " | ||
+ | "Setting {} value".format(DEFAULT_OVERLAP)) | ||
+ | |||
+ | if data['crop'] is not None: | ||
+ | crop = float(data['crop']) | ||
+ | else: | ||
+ | crop = DEFAULT_CROP | ||
+ | print("Key crop not found in JSON configuration file. " | ||
+ | "Setting {} value".format(DEFAULT_CROP)) | ||
+ | |||
+ | if data['undistort'] is not None: | ||
+ | undistort = bool(data['undistort']) | ||
+ | else: | ||
+ | undistort = DEFAULT_UNDISTORT | ||
+ | print("Key undistort not found in JSON configuration file. " | ||
+ | "Setting {} value".format(DEFAULT_UNDISTORT)) | ||
# load camera matrix and distortion coeficients | # load camera matrix and distortion coeficients | ||
Line 205: | Line 275: | ||
D = None | D = None | ||
if undistort: | if undistort: | ||
+ | if data['cameraMatrix'] is None: | ||
+ | return ( | ||
+ | K, | ||
+ | D, | ||
+ | reprojError, | ||
+ | matchRatio, | ||
+ | sigma, | ||
+ | overlap, | ||
+ | crop, | ||
+ | fov, | ||
+ | False) | ||
+ | |||
K = np.zeros(HOMOGRAPHY_DIMENSION * HOMOGRAPHY_DIMENSION) | K = np.zeros(HOMOGRAPHY_DIMENSION * HOMOGRAPHY_DIMENSION) | ||
− | for i, k in enumerate(data[' | + | for i, k in enumerate(data['cameraMatrix']): |
K[i] = k | K[i] = k | ||
K = K.reshape(HOMOGRAPHY_DIMENSION, HOMOGRAPHY_DIMENSION) | K = K.reshape(HOMOGRAPHY_DIMENSION, HOMOGRAPHY_DIMENSION) | ||
− | D = np.zeros([1, len(data[' | + | if data['distortionParameters'] is None: |
− | for i, d in enumerate(data[' | + | return ( |
+ | K, | ||
+ | D, | ||
+ | reprojError, | ||
+ | matchRatio, | ||
+ | sigma, | ||
+ | overlap, | ||
+ | crop, | ||
+ | fov, | ||
+ | False) | ||
+ | |||
+ | D = np.zeros([1, len(data['distortionParameters'])]) | ||
+ | for i, d in enumerate(data['distortionParameters']): | ||
D[0, i] = d | D[0, i] = d | ||
Line 217: | Line 311: | ||
sigma, overlap, crop, fov, undistort) | sigma, overlap, crop, fov, undistort) | ||
− | def | + | |
+ | def getHomography(left, right, config): | ||
''' | ''' | ||
− | + | Returns the homography matrix along with the processed left and right | |
− | + | images. | |
Parameters: | Parameters: | ||
+ | left (np.array): Left input image. | ||
+ | right (np.array): Right input image. | ||
config (string): Path to the JSON configuration file. | config (string): Path to the JSON configuration file. | ||
− | |||
− | |||
− | |||
Returns: | Returns: | ||
− | + | Tuple with: (left, right, homography H) | |
''' | ''' | ||
− | ( | + | (cameraMatrix, distortionParameters, reprojThresh, ratio, sigma, overlap, |
crop, fov, undistort) = parseJSON(config) | crop, fov, undistort) = parseJSON(config) | ||
− | + | height, width = left.shape[:2] | |
− | |||
− | |||
− | |||
− | height, width = | ||
# crop | # crop | ||
− | cropImg = int((crop * | + | cropImg = int((crop * left.shape[1]) / fov) |
if cropImg > 0: | if cropImg > 0: | ||
− | + | left = left[:, :-1 * cropImg] | |
− | + | left = cv2.resize(left, (width, height), | |
− | + | interpolation=cv2.INTER_CUBIC) | |
− | + | right = right[:, cropImg:] | |
+ | right = cv2.resize(right, (width, height), | ||
+ | interpolation=cv2.INTER_CUBIC) | ||
# remove undistort | # remove undistort | ||
if undistort: | if undistort: | ||
− | + | left = removeDistortion(left, cameraMatrix, distortionParameters) | |
− | + | right = removeDistortion(right, cameraMatrix, distortionParameters) | |
# Enhance images | # Enhance images | ||
− | + | enhancedLeft = imageEnhancement(left, sigma) | |
− | + | enhancedRight = imageEnhancement(right, sigma) | |
# Mask | # Mask | ||
− | overlapImg = int((overlap * | + | overlapImg = int((overlap * left.shape[1]) / fov) |
− | + | leftMask = np.zeros(enhancedLeft.shape, dtype=np.uint8) | |
− | + | leftMask[0:, (-1 * overlapImg):] = 1 | |
− | + | rightMask = np.zeros(enhancedRight.shape, dtype=np.uint8) | |
− | + | rightMask[0:, 0:overlapImg] = 1 | |
# Extract keypoints | # Extract keypoints | ||
− | (kpsA, featuresA) = detectAndDescribe( | + | (kpsA, featuresA) = detectAndDescribe(enhancedRight, rightMask) |
− | (kpsB, featuresB) = detectAndDescribe( | + | (kpsB, featuresB) = detectAndDescribe(enhancedLeft, leftMask) |
# Match features | # Match features | ||
Line 272: | Line 364: | ||
kpsA, kpsB, featuresA, featuresB, ratio, reprojThresh) | kpsA, kpsB, featuresA, featuresB, ratio, reprojThresh) | ||
− | vis = drawMatches( | + | vis = drawMatches(right, left, kpsA, kpsB, matches, status) |
cv2.imwrite('vis.jpg', vis) | cv2.imwrite('vis.jpg', vis) | ||
+ | print("Created vis.jpg image") | ||
+ | |||
+ | return (left, right, H) | ||
+ | |||
+ | |||
+ | def left_fixed(config, targetImage, originalImage, homographyScale): | ||
+ | ''' | ||
+ | Performs the homography estimation betwesn two images, leaving the left one | ||
+ | fixed and transforming the right one to align them. | ||
+ | |||
+ | Parameters: | ||
+ | config (string): Path to the JSON configuration file. | ||
+ | targetImage (string): Path to the target iamge. | ||
+ | originalImage (string): Path to the original image. | ||
+ | homographyScale (float): Scale factor for the generated | ||
+ | homography. | ||
+ | |||
+ | Returns: | ||
+ | No return value | ||
+ | ''' | ||
+ | |||
+ | # Load images | ||
+ | target = cv2.imread(targetImage) | ||
+ | original = cv2.imread(originalImage) | ||
+ | |||
+ | (left, right, H) = getHomography(target, original, config) | ||
# Apply homography | # Apply homography | ||
result = cv2.warpPerspective( | result = cv2.warpPerspective( | ||
− | + | right, H, (left.shape[1] + right.shape[1], right.shape[0])) | |
− | result[0: | + | result[0:left.shape[0], 0:left.shape[1]] = left |
cv2.imwrite('result.jpg', result) | cv2.imwrite('result.jpg', result) | ||
+ | print("Created result.jpg image") | ||
# Scale homography | # Scale homography | ||
Line 287: | Line 406: | ||
H = Hmap.getProjTr() | H = Hmap.getProjTr() | ||
− | print("""RC_HOMOGRAPHY=\"{{\\"h00\\":{},\\"h01\\":{}, \\"h02\\":{}, \\"h10\\":{}, \\"h11\\":{}, \\"h12\\":{}, \\"h20\\":{}, \\"h21\\":{}, \\"h22\\":{}}}\"""".format( | + | print( |
− | + | """RC_HOMOGRAPHY=\"{{\\"h00\\":{},\\"h01\\":{}, \\"h02\\":{}, \\"h10\\":{}""" | |
− | + | """, \\"h11\\":{}, \\"h12\\":{}, \\"h20\\":{}, \\"h21\\":{}, \\"h22\\":{}}}\"""".format( | |
− | + | H[0][0], | |
− | + | H[0][1], | |
− | + | H[0][2], | |
− | + | H[1][0], | |
− | + | H[1][1], | |
− | + | H[1][2], | |
− | + | H[2][0], | |
− | + | H[2][1], | |
+ | H[2][2])) | ||
def right_fixed(config, targetImage, originalImage, homographyScale): | def right_fixed(config, targetImage, originalImage, homographyScale): | ||
''' | ''' | ||
− | Performs the homography estimation | + | Performs the homography estimation between two images, leaving the right one |
− | + | fixed and transforming the left one to align them. | |
Parameters: | Parameters: | ||
config (string): Path to the JSON configuration file. | config (string): Path to the JSON configuration file. | ||
− | targetImage (string): Path to the | + | targetImage (string): Path to the target iamge. |
originalImage (string): Path to the original image. | originalImage (string): Path to the original image. | ||
− | homographyScale (float): Scale factor for the generated homography. | + | homographyScale (float): Scale factor for the generated |
+ | homography. | ||
Returns: | Returns: | ||
No return value | No return value | ||
''' | ''' | ||
− | |||
− | |||
− | |||
# Load images | # Load images | ||
target = cv2.imread(targetImage) | target = cv2.imread(targetImage) | ||
original = cv2.imread(originalImage) | original = cv2.imread(originalImage) | ||
− | |||
− | + | (left, right, H) = getHomography(original, target, config) | |
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | # | + | # X translation shift |
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
H = np.linalg.inv(H) | H = np.linalg.inv(H) | ||
− | |||
− | |||
H[0][-1] += original.shape[1] | H[0][-1] += original.shape[1] | ||
− | |||
− | |||
− | |||
# Apply homography | # Apply homography | ||
result = cv2.warpPerspective( | result = cv2.warpPerspective( | ||
− | + | left, H, (right.shape[1] + left.shape[1], left.shape[0])) | |
− | result[0: | + | result[0:right.shape[0], right.shape[1]:] = right |
cv2.imwrite('result.jpg', result) | cv2.imwrite('result.jpg', result) | ||
+ | print("Created result.jpg image") | ||
# Scale homography | # Scale homography | ||
Line 375: | Line 460: | ||
H = Hmap.getProjTr() | H = Hmap.getProjTr() | ||
− | print("""LC_HOMOGRAPHY=\"{{\\"h00\\":{},\\"h01\\":{}, \\"h02\\":{}, \\"h10\\":{}, \\"h11\\":{}, \\"h12\\":{}, \\"h20\\":{}, \\"h21\\":{}, \\"h22\\":{}}}\"""".format( | + | print( |
− | + | """LC_HOMOGRAPHY=\"{{\\"h00\\":{},\\"h01\\":{}, \\"h02\\":{}, \\"h10\\":{}""" | |
− | + | """, \\"h11\\":{}, \\"h12\\":{}, \\"h20\\":{}, \\"h21\\":{}, \\"h22\\":{}}}\"""".format( | |
− | + | H[0][0], | |
− | + | H[0][1], | |
− | + | H[0][2], | |
− | + | H[1][0], | |
− | + | H[1][1], | |
− | + | H[1][2], | |
− | + | H[2][0], | |
− | + | H[2][1], | |
+ | H[2][2])) | ||
+ | |||
def cmdline(argv): | def cmdline(argv): | ||
''' | ''' | ||
− | Function | + | Function that parses the command line options before executing the algorithm |
Parameters: | Parameters: | ||
Line 401: | Line 488: | ||
parser = argparse.ArgumentParser( | parser = argparse.ArgumentParser( | ||
prog=prog, | prog=prog, | ||
− | description='Tool for | + | description='Tool for estimating the homography between two images.', |
− | epilog='Type "%s <command> -h" for more information.' % prog) | + | epilog='Type "%s <command> -h" for more information.' % |
+ | prog) | ||
subparsers = parser.add_subparsers(dest='command') | subparsers = parser.add_subparsers(dest='command') | ||
Line 413: | Line 501: | ||
cmd, description=desc, help=desc, epilog=epilog) | cmd, description=desc, help=desc, epilog=epilog) | ||
− | p = add_command('left_fixed', 'Estimation of homography between two images') | + | p = add_command( |
+ | 'left_fixed', | ||
+ | 'Estimation of homography between two images, with the left one fixed.') | ||
− | p.add_argument('--config', | + | p.add_argument( |
− | p.add_argument('--targetImage', | + | '--config', |
− | p.add_argument('--originalImage', | + | help='Path of the configuration file', |
− | p.add_argument('--homographyScale', help='Scale factor of the homography. For example if you go from 1920x1080 in the estimation to 640x360 in the processing the scale factor should be 1/3', type=float, default=0) | + | default='') |
+ | p.add_argument( | ||
+ | '--targetImage', | ||
+ | help='Path of the target image', | ||
+ | default='') | ||
+ | p.add_argument( | ||
+ | '--originalImage', | ||
+ | help='Path of the original image', | ||
+ | default='') | ||
+ | p.add_argument( | ||
+ | '--homographyScale', | ||
+ | help='Scale factor of the homography. For example if you go from 1920x1080 ' | ||
+ | 'in the estimation to 640x360 in the processing the scale factor should be 1/3', | ||
+ | type=float, | ||
+ | default=0) | ||
− | p = add_command('right_fixed', 'Estimation of | + | p = add_command( |
+ | 'right_fixed', | ||
+ | 'Estimation of homography between two images, with the right one fixed.') | ||
− | p.add_argument('--config', | + | p.add_argument( |
− | p.add_argument('--targetImage', | + | '--config', |
− | p.add_argument('--originalImage', | + | help='Path of the configuration file', |
− | p.add_argument('--homographyScale', help='Scale factor of the homography. For example if you go from 1920x1080 in the estimation to 640x360 in the processing the scale factor should be 1/3', type=float, default=0) | + | default='') |
+ | p.add_argument( | ||
+ | '--targetImage', | ||
+ | help='Path of the target image', | ||
+ | default='') | ||
+ | p.add_argument( | ||
+ | '--originalImage', | ||
+ | help='Path of the original image', | ||
+ | default='') | ||
+ | p.add_argument( | ||
+ | '--homographyScale', | ||
+ | help='Scale factor of the homography. For example if you go from 1920x1080 ' | ||
+ | 'in the estimation to 640x360 in the processing the scale factor should be 1/3', | ||
+ | type=float, | ||
+ | default=0) | ||
args = parser.parse_args(argv[1:] if len(argv) > 1 else ['-h']) | args = parser.parse_args(argv[1:] if len(argv) > 1 else ['-h']) | ||
Line 431: | Line 551: | ||
del args.command | del args.command | ||
func(**vars(args)) | func(**vars(args)) | ||
− | |||
− | |||
if __name__ == "__main__": | if __name__ == "__main__": | ||
cmdline(sys.argv) | cmdline(sys.argv) | ||
− | |||
− | |||
</syntaxhighlight> | </syntaxhighlight> |
Revision as of 15:17, 29 July 2020
#!/usr/bin/env python3
""" Copyright (C) 2020 RidgeRun, LLC (http://www.ridgerun.com)
All Rights Reserved.
The contents of this software are proprietary and confidential to RidgeRun,
LLC. No part of this program may be photocopied, reproduced or translated
into another programming language without prior written consent of
RidgeRun, LLC. The user is free to modify the source code after obtaining
a software license from RidgeRun. All source code changes must be provided
back to RidgeRun without any encumbrance. """
""" Tool for estimating the homography matrix """
import argparse
import json
import sys
import cv2
import numpy as np
HOMOGRAPHY_DIMENSION = 3
MIN_MATCHES = 4
KERNEL_SIZE = 5
CHANNELS = 3
DEFAULT_REPROJ_ERROR = 4.0
DEFAULT_RATIO = 0.75
DEFAULT_SIGMA = 0
DEFAULT_FOV = 70
DEFAULT_OVERLAP = 15
DEFAULT_CROP = 0
DEFAULT_UNDISTORT = True
def drawMatches(imageA, imageB, keypointsA, keypointsB, matches, status):
'''
Returns an image with the matches found.
Parameters:
imageA (np.array): Image A
imageB (np.array): Image B
keypointsA (np.array): Keypoints for image A
keypointsB (np.array): Keypoints for image B
matches (np.array): List of found matches between image
A and image B
status (int): status of homography estimation
Returns:
Image (np.array)
'''
# initialize the output visualization image
(heightA, widthA) = imageA.shape[:2]
(heightB, widthB) = imageB.shape[:2]
vis = np.zeros(
(max(
heightA,
heightB),
widthA +
widthB,
CHANNELS),
dtype="uint8")
vis[0:heightA, 0:widthA] = imageA
vis[0:heightB, widthA:] = imageB
for ((trainIdx, queryIdx), s) in zip(matches, status):
# only process the match if the keypoint was successfully
# matched
if s == 1:
# draw the match
ptA = (int(keypointsA[queryIdx][0]), int(keypointsA[queryIdx][1]))
ptB = (int(keypointsB[trainIdx][0]) +
widthA, int(keypointsB[trainIdx][1]))
cv2.line(vis, ptA, ptB, (0, 255, 0), 1)
return vis
def detectAndDescribe(gray, mask):
'''
Returns the keyponts and the correspoinding descriptors for the
gray image.
Parameters:
gray (np.array): Input image in grayscale.
mask (np.array): Mask to apply before the feature
extraction.
Returns:
Tuple with the keypoins and their descriptors.
'''
# detect and extract features from the image
detector = cv2.xfeatures2d.SIFT_create()
(kps, features) = detector.detectAndCompute(gray, mask)
kps = np.float32([kp.pt for kp in kps])
return (kps, features)
def matchKeypoints(keypointsA, keypointsB, featuresA, featuresB,
ratio, reprojThresh):
'''
Function that searches the correspondiencies for the
given descriptors (featuresA and featuresB) and with those
correspondencies, the homography matrix is calculated.
Parameters:
keypointsA (np.array): Keypoints of image A.
keypointsB (np.array): Keypoints of image B.
featuresA (np.array): Descriptors of image A
featuresB (np.array): Descriptors of image B
ratio (float): Max distance between a possible
correspondence.
reprojThresh (float): Reprojection error of the
homography estimation
Returns:
Tuple with the resulting matches, the homography H
and the estimation status.
'''
# compute the raw matches and initialize the list of actual matches
matcher = cv2.DescriptorMatcher_create("BruteForce")
rawMatches = matcher.knnMatch(featuresA, featuresB, 2)
matches = []
# loop over the raw matches
for m in rawMatches:
# ensure the distance is within a certain ratio of each
# other (i.e. Lowe's ratio test)
if len(m) == 2 and m[0].distance < m[1].distance * ratio:
matches.append((m[0].trainIdx, m[0].queryIdx))
print("matches: " + str(len(matches)))
if len(matches) >= MIN_MATCHES:
# construct the two sets of points
ptsA = np.float32([keypointsA[i] for (_, i) in matches])
ptsB = np.float32([keypointsB[i] for (i, _) in matches])
# compute the homography between the two sets of points
(H, status) = cv2.findHomography(ptsA, ptsB, cv2.RANSAC,
reprojThresh)
# return the matches along with the homograpy matrix
# and status of each matched point
return (matches, H, status)
# otherwise, no homograpy could be computed
return None
def imageEnhancement(img, sigma):
'''
Returns an image with some enhancement applied. Remove the
noise of the input image img using a Gaussian filter and
then convert it to grayscale.
Parameters:
img (np.array): Input image img
sigma (float): Sigma value of the Gaussian filter.
Returns:
Image
'''
# Remove noise
img = cv2.GaussianBlur(img, (KERNEL_SIZE, KERNEL_SIZE), sigma)
# Convert to grayscale
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
return gray
def removeDistortion(img, cameraMatrix, distortionParameters):
'''
Returns an image with the distortion provoked by the camera lens removed.
Parameters:
img (np.array): Image img.
cameraMatrix (np.array): Camera matrix with a dimension
of (3x3).
distortionParameters (np.array): Vector with the
distortion coefficients.
Returns:
Undistorted image.
'''
# Read an example image and acquire its size
h, w = img.shape[:2]
# Generate new camera matrix from parameters
newCameraMatrix, roi = cv2.getOptimalNewCameraMatrix(
cameraMatrix, distortionParameters, (w, h), 0)
# Generate look-up tables for remapping the camera image
mapX, mapY = cv2.initUndistortRectifyMap(
cameraMatrix, distortionParameters, None, newCameraMatrix, (w, h), 5)
# Remap the original image to a new image
newImg = cv2.remap(img, mapX, mapY, cv2.INTER_LINEAR)
return newImg
def parseJSON(filename):
'''
Returns the algorithm variables read from a JSON configuration file.
Parameters:
filename (string): Path to the JSON configuration file.
Returns:
Tuple the read variables: (K, D, reprojError,
matchRatio, sigma, overlap, crop, fov, undistort)
'''
with open(filename) as json_file:
data = json.load(json_file)
# load config variables
if data['reprojError'] is not None:
reprojError = float(data['reprojError'])
else:
reprojError = DEFAULT_REPROJ_ERROR
print("Key reprojError not found in JSON configuration file. "
"Setting {} value".format(DEFAULT_REPROJ_ERROR))
if data['matchRatio'] is not None:
matchRatio = float(data['matchRatio'])
else:
matchRatio = DEFAULT_RATIO
print("Key matchRatio not found in JSON configuration file. "
"Setting {} value".format(DEFAULT_RATIO))
if data['sigma'] is not None:
sigma = float(data['sigma'])
else:
sigma = DEFAULT_SIGMA
print("Key sigma not found in JSON configuration file. "
"Setting {} value".format(DEFAULT_SIGMA))
if data['fov'] is not None:
fov = float(data['fov'])
else:
fov = DEFAULT_FOV
print("Key fov not found in JSON configuration file. "
"Setting {} value".format(DEFAULT_FOV))
if data['overlap'] is not None:
overlap = float(data['overlap'])
else:
overlap = DEFAULT_OVERLAP
print("Key overlap not found in JSON configuration file. "
"Setting {} value".format(DEFAULT_OVERLAP))
if data['crop'] is not None:
crop = float(data['crop'])
else:
crop = DEFAULT_CROP
print("Key crop not found in JSON configuration file. "
"Setting {} value".format(DEFAULT_CROP))
if data['undistort'] is not None:
undistort = bool(data['undistort'])
else:
undistort = DEFAULT_UNDISTORT
print("Key undistort not found in JSON configuration file. "
"Setting {} value".format(DEFAULT_UNDISTORT))
# load camera matrix and distortion coeficients
K = None
D = None
if undistort:
if data['cameraMatrix'] is None:
return (
K,
D,
reprojError,
matchRatio,
sigma,
overlap,
crop,
fov,
False)
K = np.zeros(HOMOGRAPHY_DIMENSION * HOMOGRAPHY_DIMENSION)
for i, k in enumerate(data['cameraMatrix']):
K[i] = k
K = K.reshape(HOMOGRAPHY_DIMENSION, HOMOGRAPHY_DIMENSION)
if data['distortionParameters'] is None:
return (
K,
D,
reprojError,
matchRatio,
sigma,
overlap,
crop,
fov,
False)
D = np.zeros([1, len(data['distortionParameters'])])
for i, d in enumerate(data['distortionParameters']):
D[0, i] = d
return (K, D, reprojError, matchRatio,
sigma, overlap, crop, fov, undistort)
def getHomography(left, right, config):
'''
Returns the homography matrix along with the processed left and right
images.
Parameters:
left (np.array): Left input image.
right (np.array): Right input image.
config (string): Path to the JSON configuration file.
Returns:
Tuple with: (left, right, homography H)
'''
(cameraMatrix, distortionParameters, reprojThresh, ratio, sigma, overlap,
crop, fov, undistort) = parseJSON(config)
height, width = left.shape[:2]
# crop
cropImg = int((crop * left.shape[1]) / fov)
if cropImg > 0:
left = left[:, :-1 * cropImg]
left = cv2.resize(left, (width, height),
interpolation=cv2.INTER_CUBIC)
right = right[:, cropImg:]
right = cv2.resize(right, (width, height),
interpolation=cv2.INTER_CUBIC)
# remove undistort
if undistort:
left = removeDistortion(left, cameraMatrix, distortionParameters)
right = removeDistortion(right, cameraMatrix, distortionParameters)
# Enhance images
enhancedLeft = imageEnhancement(left, sigma)
enhancedRight = imageEnhancement(right, sigma)
# Mask
overlapImg = int((overlap * left.shape[1]) / fov)
leftMask = np.zeros(enhancedLeft.shape, dtype=np.uint8)
leftMask[0:, (-1 * overlapImg):] = 1
rightMask = np.zeros(enhancedRight.shape, dtype=np.uint8)
rightMask[0:, 0:overlapImg] = 1
# Extract keypoints
(kpsA, featuresA) = detectAndDescribe(enhancedRight, rightMask)
(kpsB, featuresB) = detectAndDescribe(enhancedLeft, leftMask)
# Match features
(matches, H, status) = matchKeypoints(
kpsA, kpsB, featuresA, featuresB, ratio, reprojThresh)
vis = drawMatches(right, left, kpsA, kpsB, matches, status)
cv2.imwrite('vis.jpg', vis)
print("Created vis.jpg image")
return (left, right, H)
def left_fixed(config, targetImage, originalImage, homographyScale):
'''
Performs the homography estimation betwesn two images, leaving the left one
fixed and transforming the right one to align them.
Parameters:
config (string): Path to the JSON configuration file.
targetImage (string): Path to the target iamge.
originalImage (string): Path to the original image.
homographyScale (float): Scale factor for the generated
homography.
Returns:
No return value
'''
# Load images
target = cv2.imread(targetImage)
original = cv2.imread(originalImage)
(left, right, H) = getHomography(target, original, config)
# Apply homography
result = cv2.warpPerspective(
right, H, (left.shape[1] + right.shape[1], right.shape[0]))
result[0:left.shape[0], 0:left.shape[1]] = left
cv2.imwrite('result.jpg', result)
print("Created result.jpg image")
# Scale homography
if homographyScale > 0:
Hmap = cv2.reg_MapProjec(H)
Hmap.scale(homographyScale)
H = Hmap.getProjTr()
print(
"""RC_HOMOGRAPHY=\"{{\\"h00\\":{},\\"h01\\":{}, \\"h02\\":{}, \\"h10\\":{}"""
""", \\"h11\\":{}, \\"h12\\":{}, \\"h20\\":{}, \\"h21\\":{}, \\"h22\\":{}}}\"""".format(
H[0][0],
H[0][1],
H[0][2],
H[1][0],
H[1][1],
H[1][2],
H[2][0],
H[2][1],
H[2][2]))
def right_fixed(config, targetImage, originalImage, homographyScale):
'''
Performs the homography estimation between two images, leaving the right one
fixed and transforming the left one to align them.
Parameters:
config (string): Path to the JSON configuration file.
targetImage (string): Path to the target iamge.
originalImage (string): Path to the original image.
homographyScale (float): Scale factor for the generated
homography.
Returns:
No return value
'''
# Load images
target = cv2.imread(targetImage)
original = cv2.imread(originalImage)
(left, right, H) = getHomography(original, target, config)
# X translation shift
H = np.linalg.inv(H)
H[0][-1] += original.shape[1]
# Apply homography
result = cv2.warpPerspective(
left, H, (right.shape[1] + left.shape[1], left.shape[0]))
result[0:right.shape[0], right.shape[1]:] = right
cv2.imwrite('result.jpg', result)
print("Created result.jpg image")
# Scale homography
if homographyScale > 0:
Hmap = cv2.reg_MapProjec(H)
H[0][-1] -= original.shape[1]
Hmap.scale(homographyScale)
H = Hmap.getProjTr()
print(
"""LC_HOMOGRAPHY=\"{{\\"h00\\":{},\\"h01\\":{}, \\"h02\\":{}, \\"h10\\":{}"""
""", \\"h11\\":{}, \\"h12\\":{}, \\"h20\\":{}, \\"h21\\":{}, \\"h22\\":{}}}\"""".format(
H[0][0],
H[0][1],
H[0][2],
H[1][0],
H[1][1],
H[1][2],
H[2][0],
H[2][1],
H[2][2]))
def cmdline(argv):
'''
Function that parses the command line options before executing the algorithm
Parameters:
config (list): List of command line arguments.
Returns:
No return value
'''
prog = argv[0]
parser = argparse.ArgumentParser(
prog=prog,
description='Tool for estimating the homography between two images.',
epilog='Type "%s <command> -h" for more information.' %
prog)
subparsers = parser.add_subparsers(dest='command')
subparsers.required = True
def add_command(cmd, desc, example=None):
epilog = 'Example: %s %s' % (
prog, example) if example is not None else None
return subparsers.add_parser(
cmd, description=desc, help=desc, epilog=epilog)
p = add_command(
'left_fixed',
'Estimation of homography between two images, with the left one fixed.')
p.add_argument(
'--config',
help='Path of the configuration file',
default='')
p.add_argument(
'--targetImage',
help='Path of the target image',
default='')
p.add_argument(
'--originalImage',
help='Path of the original image',
default='')
p.add_argument(
'--homographyScale',
help='Scale factor of the homography. For example if you go from 1920x1080 '
'in the estimation to 640x360 in the processing the scale factor should be 1/3',
type=float,
default=0)
p = add_command(
'right_fixed',
'Estimation of homography between two images, with the right one fixed.')
p.add_argument(
'--config',
help='Path of the configuration file',
default='')
p.add_argument(
'--targetImage',
help='Path of the target image',
default='')
p.add_argument(
'--originalImage',
help='Path of the original image',
default='')
p.add_argument(
'--homographyScale',
help='Scale factor of the homography. For example if you go from 1920x1080 '
'in the estimation to 640x360 in the processing the scale factor should be 1/3',
type=float,
default=0)
args = parser.parse_args(argv[1:] if len(argv) > 1 else ['-h'])
func = globals()[args.command]
del args.command
func(**vars(args))
if __name__ == "__main__":
cmdline(sys.argv)