mirror of https://github.com/opencv/opencv.git
Artem Saratovtsev
3 years ago
committed by
GitHub
parent
0a0714bd1b
commit
4ba2b05df8
9 changed files with 330 additions and 42 deletions
@ -0,0 +1,112 @@ |
||||
#!/usr/bin/env python |
||||
|
||||
import numpy as np |
||||
import cv2 as cv |
||||
|
||||
from tests_common import NewOpenCVTests |
||||
|
||||
class odometry_test(NewOpenCVTests): |
||||
def test_OdometryDefault(self): |
||||
depth = self.get_sample('cv/rgbd/depth.png', cv.IMREAD_ANYDEPTH).astype(np.float32) |
||||
radian = np.radians(1) |
||||
Rt_warp = np.array( |
||||
[[np.cos(radian), -np.sin(radian), 0], |
||||
[np.sin(radian), np.cos(radian), 0], |
||||
[0, 0, 1]], dtype=np.float32 |
||||
) |
||||
Rt_curr = np.array( |
||||
[[np.cos(radian), -np.sin(radian), 0, 0], |
||||
[np.sin(radian), np.cos(radian), 0, 0], |
||||
[0, 0, 1, 0], |
||||
[0, 0, 0, 1]], dtype=np.float32 |
||||
) |
||||
Rt_res = np.zeros((4, 4)) |
||||
|
||||
odometry = cv.Odometry() |
||||
warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480)) |
||||
isCorrect = odometry.compute(depth, warped_depth, Rt_res) |
||||
|
||||
res = np.absolute(Rt_curr - Rt_res).sum() |
||||
eps = 0.15 |
||||
self.assertLessEqual(res, eps) |
||||
self.assertTrue(isCorrect) |
||||
|
||||
def test_OdometryDepth(self): |
||||
depth = self.get_sample('cv/rgbd/depth.png', cv.IMREAD_ANYDEPTH).astype(np.float32) |
||||
radian = np.radians(1) |
||||
Rt_warp = np.array( |
||||
[[np.cos(radian), -np.sin(radian), 0], |
||||
[np.sin(radian), np.cos(radian), 0], |
||||
[0, 0, 1]], dtype=np.float32 |
||||
) |
||||
Rt_curr = np.array( |
||||
[[np.cos(radian), -np.sin(radian), 0, 0], |
||||
[np.sin(radian), np.cos(radian), 0, 0], |
||||
[0, 0, 1, 0], |
||||
[0, 0, 0, 1]], dtype=np.float32 |
||||
) |
||||
Rt_res = np.zeros((4, 4)) |
||||
|
||||
odometry = cv.Odometry(cv.DEPTH) |
||||
warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480)) |
||||
|
||||
isCorrect = odometry.compute(depth, warped_depth, Rt_res) |
||||
res = np.absolute(Rt_curr - Rt_res).sum() |
||||
eps = 0.15 |
||||
self.assertLessEqual(res, eps) |
||||
self.assertTrue(isCorrect) |
||||
|
||||
def test_OdometryRGB(self): |
||||
rgb = self.get_sample('cv/rgbd/rgb.png', cv.IMREAD_ANYCOLOR) |
||||
radian = np.radians(1) |
||||
Rt_warp = np.array( |
||||
[[np.cos(radian), -np.sin(radian), 0], |
||||
[np.sin(radian), np.cos(radian), 0], |
||||
[0, 0, 1]], dtype=np.float32 |
||||
) |
||||
Rt_curr = np.array( |
||||
[[np.cos(radian), -np.sin(radian), 0, 0], |
||||
[np.sin(radian), np.cos(radian), 0, 0], |
||||
[0, 0, 1, 0], |
||||
[0, 0, 0, 1]], dtype=np.float32 |
||||
) |
||||
Rt_res = np.zeros((4, 4)) |
||||
|
||||
odometry = cv.Odometry(cv.RGB) |
||||
warped_rgb = cv.warpPerspective(rgb, Rt_warp, (640, 480)) |
||||
isCorrect = odometry.compute(rgb, warped_rgb, Rt_res) |
||||
|
||||
res = np.absolute(Rt_curr - Rt_res).sum() |
||||
eps = 0.15 |
||||
self.assertLessEqual(res, eps) |
||||
self.assertTrue(isCorrect) |
||||
|
||||
def test_OdometryRGB_Depth(self): |
||||
depth = self.get_sample('cv/rgbd/depth.png', cv.IMREAD_ANYDEPTH).astype(np.float32) |
||||
rgb = self.get_sample('cv/rgbd/rgb.png', cv.IMREAD_ANYCOLOR) |
||||
radian = np.radians(1) |
||||
Rt_warp = np.array( |
||||
[[np.cos(radian), -np.sin(radian), 0], |
||||
[np.sin(radian), np.cos(radian), 0], |
||||
[0, 0, 1]], dtype=np.float32 |
||||
) |
||||
Rt_curr = np.array( |
||||
[[np.cos(radian), -np.sin(radian), 0, 0], |
||||
[np.sin(radian), np.cos(radian), 0, 0], |
||||
[0, 0, 1, 0], |
||||
[0, 0, 0, 1]], dtype=np.float32 |
||||
) |
||||
Rt_res = np.zeros((4, 4)) |
||||
|
||||
odometry = cv.Odometry(cv.RGB_DEPTH) |
||||
warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480)) |
||||
warped_rgb = cv.warpPerspective(rgb, Rt_warp, (640, 480)) |
||||
isCorrect = odometry.compute(depth, rgb, warped_depth, warped_rgb, Rt_res) |
||||
|
||||
res = np.absolute(Rt_curr - Rt_res).sum() |
||||
eps = 0.15 |
||||
self.assertLessEqual(res, eps) |
||||
self.assertTrue(isCorrect) |
||||
|
||||
if __name__ == '__main__': |
||||
NewOpenCVTests.bootstrap() |
||||
@ -0,0 +1,68 @@ |
||||
#!/usr/bin/env python |
||||
|
||||
import numpy as np |
||||
import cv2 as cv |
||||
|
||||
import argparse |
||||
import sys |
||||
|
||||
def main(): |
||||
parser = argparse.ArgumentParser() |
||||
parser.add_argument( |
||||
'--algo', |
||||
help="""DEPTH - works with depth, |
||||
RGB - works with images, |
||||
RGB_DEPTH - works with all, |
||||
default - runs all algos""", |
||||
default="") |
||||
parser.add_argument( |
||||
'-src_d', |
||||
'--source_depth_frame', |
||||
default="") |
||||
parser.add_argument( |
||||
'-dst_d', |
||||
'--destination_depth_frame', |
||||
default="") |
||||
parser.add_argument( |
||||
'-src_rgb', |
||||
'--source_rgb_frame', |
||||
default="") |
||||
parser.add_argument( |
||||
'-dst_rgb', |
||||
'--destination_rgb_frame', |
||||
default="") |
||||
|
||||
args = parser.parse_args() |
||||
|
||||
if args.algo == "RGB_DEPTH" or args.algo == "DEPTH" or args.algo == "": |
||||
source_depth_frame = cv.samples.findFile(args.source_depth_frame) |
||||
destination_depth_frame = cv.samples.findFile(args.destination_depth_frame) |
||||
depth1 = cv.imread(source_depth_frame, cv.IMREAD_ANYDEPTH).astype(np.float32) |
||||
depth2 = cv.imread(destination_depth_frame, cv.IMREAD_ANYDEPTH).astype(np.float32) |
||||
|
||||
if args.algo == "RGB_DEPTH" or args.algo == "RGB" or args.algo == "": |
||||
source_rgb_frame = cv.samples.findFile(args.source_rgb_frame) |
||||
destination_rgb_frame = cv.samples.findFile(args.destination_rgb_frame) |
||||
rgb1 = cv.imread(source_rgb_frame, cv.IMREAD_COLOR) |
||||
rgb2 = cv.imread(destination_rgb_frame, cv.IMREAD_COLOR) |
||||
|
||||
if args.algo == "DEPTH" or args.algo == "": |
||||
odometry = cv.Odometry(cv.DEPTH) |
||||
Rt = np.zeros((4, 4)) |
||||
odometry.compute(depth1, depth2, Rt) |
||||
print(Rt) |
||||
if args.algo == "RGB" or args.algo == "": |
||||
odometry = cv.Odometry(cv.RGB) |
||||
Rt = np.zeros((4, 4)) |
||||
odometry.compute(rgb1, rgb2, Rt) |
||||
print(Rt) |
||||
if args.algo == "RGB_DEPTH" or args.algo == "": |
||||
odometry = cv.Odometry(cv.RGB_DEPTH) |
||||
Rt = np.zeros((4, 4)) |
||||
odometry.compute(depth1, rgb1, depth2, rgb2, Rt) |
||||
print(Rt) |
||||
|
||||
|
||||
|
||||
if __name__ == '__main__': |
||||
main() |
||||
Loading…
Reference in new issue