mirror of https://github.com/opencv/opencv.git
Merge pull request #22925 from savuor:pytsdf_from_scratch
Fixes #22799 Replaces #21559 which was taken as a base Connected PR in contrib: [#3388@contrib](https://github.com/opencv/opencv_contrib/pull/3388) ### Changes OK, now this is more Odometry-related PR than Volume-related. Anyway, * `Volume` class gets wrapped * The same was done for helper classes like `VolumeSettings`, `OdometryFrame` and `OdometrySettings` * `OdometryFrame` constructor signature changed to more convenient where depth goes on 1st place, RGB image on 2nd. This works better for depth-only `Odometry` algorithms. * `OdometryFrame` is checked for amount of pyramid layers inside `Odometry::compute()` * `Odometry` was fully wrapped + more docs added * Added Python tests for `Odometry`, `OdometryFrame` and `Volume` * Added Python sample for `Volume` * Minor fixes including better var names ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [x] There is a reference to the original bug report and related work - [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [x] The feature is well documented and sample code can be built with the project CMakepull/20371/merge
Rostislav Vasilikhin
2 years ago
committed by
GitHub
parent
86c6e07326
commit
d49958141e
22 changed files with 586 additions and 232 deletions
@ -0,0 +1,123 @@ |
||||
import numpy as np |
||||
import cv2 as cv |
||||
|
||||
from tests_common import NewOpenCVTests |
||||
|
||||
class volume_test(NewOpenCVTests): |
||||
def test_VolumeDefault(self): |
||||
depthPath = 'cv/rgbd/depth.png' |
||||
depth = self.get_sample(depthPath, cv.IMREAD_ANYDEPTH).astype(np.float32) |
||||
if depth.size <= 0: |
||||
raise Exception('Failed to load depth file: %s' % depthPath) |
||||
|
||||
Rt = np.eye(4) |
||||
volume = cv.Volume() |
||||
volume.integrate(depth, Rt) |
||||
|
||||
size = (480, 640, 4) |
||||
points = np.zeros(size, np.float32) |
||||
normals = np.zeros(size, np.float32) |
||||
|
||||
Kraycast = np.array([[525. , 0. , 319.5], |
||||
[ 0. , 525. , 239.5], |
||||
[ 0. , 0. , 1. ]]) |
||||
|
||||
volume.raycastEx(Rt, size[0], size[1], Kraycast, points, normals) |
||||
|
||||
self.assertEqual(points.shape, size) |
||||
self.assertEqual(points.shape, normals.shape) |
||||
|
||||
volume.raycast(Rt, points, normals) |
||||
|
||||
self.assertEqual(points.shape, size) |
||||
self.assertEqual(points.shape, normals.shape) |
||||
|
||||
def test_VolumeTSDF(self): |
||||
depthPath = 'cv/rgbd/depth.png' |
||||
depth = self.get_sample(depthPath, cv.IMREAD_ANYDEPTH).astype(np.float32) |
||||
if depth.size <= 0: |
||||
raise Exception('Failed to load depth file: %s' % depthPath) |
||||
|
||||
Rt = np.eye(4) |
||||
|
||||
settings = cv.VolumeSettings(cv.VolumeType_TSDF) |
||||
volume = cv.Volume(cv.VolumeType_TSDF, settings) |
||||
volume.integrate(depth, Rt) |
||||
|
||||
size = (480, 640, 4) |
||||
points = np.zeros(size, np.float32) |
||||
normals = np.zeros(size, np.float32) |
||||
|
||||
Kraycast = settings.getCameraRaycastIntrinsics() |
||||
volume.raycastEx(Rt, size[0], size[1], Kraycast, points, normals) |
||||
|
||||
self.assertEqual(points.shape, size) |
||||
self.assertEqual(points.shape, normals.shape) |
||||
|
||||
volume.raycast(Rt, points, normals) |
||||
|
||||
self.assertEqual(points.shape, size) |
||||
self.assertEqual(points.shape, normals.shape) |
||||
|
||||
def test_VolumeHashTSDF(self): |
||||
depthPath = 'cv/rgbd/depth.png' |
||||
depth = self.get_sample(depthPath, cv.IMREAD_ANYDEPTH).astype(np.float32) |
||||
if depth.size <= 0: |
||||
raise Exception('Failed to load depth file: %s' % depthPath) |
||||
|
||||
Rt = np.eye(4) |
||||
settings = cv.VolumeSettings(cv.VolumeType_HashTSDF) |
||||
volume = cv.Volume(cv.VolumeType_HashTSDF, settings) |
||||
volume.integrate(depth, Rt) |
||||
|
||||
size = (480, 640, 4) |
||||
points = np.zeros(size, np.float32) |
||||
normals = np.zeros(size, np.float32) |
||||
|
||||
Kraycast = settings.getCameraRaycastIntrinsics() |
||||
volume.raycastEx(Rt, size[0], size[1], Kraycast, points, normals) |
||||
|
||||
self.assertEqual(points.shape, size) |
||||
self.assertEqual(points.shape, normals.shape) |
||||
|
||||
volume.raycast(Rt, points, normals) |
||||
|
||||
self.assertEqual(points.shape, size) |
||||
self.assertEqual(points.shape, normals.shape) |
||||
|
||||
def test_VolumeColorTSDF(self): |
||||
depthPath = 'cv/rgbd/depth.png' |
||||
rgbPath = 'cv/rgbd/rgb.png' |
||||
depth = self.get_sample(depthPath, cv.IMREAD_ANYDEPTH).astype(np.float32) |
||||
rgb = self.get_sample(rgbPath, cv.IMREAD_ANYCOLOR).astype(np.float32) |
||||
|
||||
if depth.size <= 0: |
||||
raise Exception('Failed to load depth file: %s' % depthPath) |
||||
if rgb.size <= 0: |
||||
raise Exception('Failed to load RGB file: %s' % rgbPath) |
||||
|
||||
Rt = np.eye(4) |
||||
settings = cv.VolumeSettings(cv.VolumeType_ColorTSDF) |
||||
volume = cv.Volume(cv.VolumeType_ColorTSDF, settings) |
||||
volume.integrateColor(depth, rgb, Rt) |
||||
|
||||
size = (480, 640, 4) |
||||
points = np.zeros(size, np.float32) |
||||
normals = np.zeros(size, np.float32) |
||||
colors = np.zeros(size, np.float32) |
||||
|
||||
Kraycast = settings.getCameraRaycastIntrinsics() |
||||
volume.raycastExColor(Rt, size[0], size[1], Kraycast, points, normals, colors) |
||||
|
||||
self.assertEqual(points.shape, size) |
||||
self.assertEqual(points.shape, normals.shape) |
||||
|
||||
volume.raycastColor(Rt, points, normals, colors) |
||||
|
||||
self.assertEqual(points.shape, size) |
||||
self.assertEqual(points.shape, normals.shape) |
||||
self.assertEqual(points.shape, colors.shape) |
||||
|
||||
|
||||
if __name__ == '__main__': |
||||
NewOpenCVTests.bootstrap() |
||||
@ -0,0 +1,140 @@ |
||||
import numpy as np |
||||
import cv2 as cv |
||||
import argparse |
||||
|
||||
# Use source data from this site: |
||||
# https://vision.in.tum.de/data/datasets/rgbd-dataset/download |
||||
# For example if you use rgbd_dataset_freiburg1_xyz sequence, your prompt should be: |
||||
# python /path_to_opencv/samples/python/volume.py --source_folder /path_to_datasets/rgbd_dataset_freiburg1_xyz --algo <some algo> |
||||
# so that the folder contains files groundtruth.txt and depth.txt |
||||
|
||||
# for more info about this function look cv::Quat::toRotMat3x3(...) |
||||
def quatToMat3(a, b, c, d): |
||||
return np.array([ |
||||
[1 - 2 * (c * c + d * d), 2 * (b * c - a * d) , 2 * (b * d + a * c)], |
||||
[2 * (b * c + a * d) , 1 - 2 * (b * b + d * d), 2 * (c * d - a * b)], |
||||
[2 * (b * d - a * c) , 2 * (c * d + a * b) , 1 - 2 * (b * b + c * c)] |
||||
]) |
||||
|
||||
def make_Rt(val): |
||||
R = quatToMat3(val[6], val[3], val[4] ,val[5]) |
||||
t = np.array([ [val[0]], [val[1]], [val[2]] ]) |
||||
tmp = np.array([0, 0, 0, 1]) |
||||
|
||||
Rt = np.append(R, t , axis=1 ) |
||||
Rt = np.vstack([Rt, tmp]) |
||||
|
||||
return Rt |
||||
|
||||
def get_image_info(path, is_depth): |
||||
image_info = {} |
||||
source = 'depth.txt' |
||||
if not is_depth: |
||||
source = 'rgb.txt' |
||||
with open(path+source) as file: |
||||
lines = file.readlines() |
||||
for line in lines: |
||||
words = line.split(' ') |
||||
if words[0] == '#': |
||||
continue |
||||
image_info[float(words[0])] = words[1][:-1] |
||||
return image_info |
||||
|
||||
def get_groundtruth_info(path): |
||||
groundtruth_info = {} |
||||
with open(path+'groundtruth.txt') as file: |
||||
lines = file.readlines() |
||||
for line in lines: |
||||
words = line.split(' ') |
||||
if words[0] == '#': |
||||
continue |
||||
groundtruth_info[float(words[0])] = [float(i) for i in words[1:]] |
||||
return groundtruth_info |
||||
|
||||
def main(): |
||||
|
||||
parser = argparse.ArgumentParser() |
||||
parser.add_argument( |
||||
'--algo', |
||||
help="""TSDF - reconstruct data in volume with bounds, |
||||
HashTSDF - reconstruct data in volume without bounds (infinite volume), |
||||
ColorTSDF - like TSDF but also keeps color data, |
||||
default - runs TSDF""", |
||||
default="") |
||||
parser.add_argument( |
||||
'-src', |
||||
'--source_folder', |
||||
default="") |
||||
|
||||
args = parser.parse_args() |
||||
|
||||
path = args.source_folder |
||||
if path[-1] != '/': |
||||
path += '/' |
||||
|
||||
depth_info = get_image_info(path, True) |
||||
rgb_info = get_image_info(path, False) |
||||
groundtruth_info = get_groundtruth_info(path) |
||||
|
||||
volume_type = cv.VolumeType_TSDF |
||||
if args.algo == "HashTSDF": |
||||
volume_type = cv.VolumeType_HashTSDF |
||||
elif args.algo == "ColorTSDF": |
||||
volume_type = cv.VolumeType_ColorTSDF |
||||
|
||||
settings = cv.VolumeSettings(volume_type) |
||||
volume = cv.Volume(volume_type, settings) |
||||
|
||||
for key in list(depth_info.keys())[:]: |
||||
Rt = np.eye(4) |
||||
for key1 in groundtruth_info: |
||||
if np.abs(key1 - key) < 0.01: |
||||
Rt = make_Rt(groundtruth_info[key1]) |
||||
break |
||||
|
||||
rgb_path = '' |
||||
for key1 in rgb_info: |
||||
if np.abs(key1 - key) < 0.05: |
||||
rgb_path = path + rgb_info[key1] |
||||
break |
||||
|
||||
depthPath = path + depth_info[key] |
||||
depth = cv.imread(depthPath, cv.IMREAD_ANYDEPTH).astype(np.float32) |
||||
if depth.size <= 0: |
||||
raise Exception('Failed to load depth file: %s' % depthPath) |
||||
|
||||
rgb = cv.imread(rgb_path, cv.IMREAD_COLOR).astype(np.float32) |
||||
if rgb.size <= 0: |
||||
raise Exception('Failed to load RGB file: %s' % rgb_path) |
||||
|
||||
if volume_type != cv.VolumeType_ColorTSDF: |
||||
volume.integrate(depth, Rt) |
||||
else: |
||||
volume.integrateColor(depth, rgb, Rt) |
||||
|
||||
size = (480, 640, 4) |
||||
|
||||
points = np.zeros(size, np.float32) |
||||
normals = np.zeros(size, np.float32) |
||||
colors = np.zeros(size, np.float32) |
||||
|
||||
if volume_type != cv.VolumeType_ColorTSDF: |
||||
volume.raycast(Rt, points, normals) |
||||
else: |
||||
volume.raycastColor(Rt, points, normals, colors) |
||||
|
||||
channels = list(cv.split(points)) |
||||
|
||||
cv.imshow("X", np.absolute(channels[0])) |
||||
cv.imshow("Y", np.absolute(channels[1])) |
||||
cv.imshow("Z", channels[2]) |
||||
|
||||
if volume_type == cv.VolumeType_ColorTSDF: |
||||
cv.imshow("Color", colors.astype(np.uint8)) |
||||
|
||||
#TODO: also display normals |
||||
|
||||
cv.waitKey(10) |
||||
|
||||
if __name__ == '__main__': |
||||
main() |
||||
Loading…
Reference in new issue