mirror of
https://github.com/opencv/opencv.git
synced 2026-07-29 15:23:05 +04:00
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 CMake
This commit is contained in:
Rostislav Vasilikhin
committed by
GitHub
GitHub
parent
86c6e07326
commit
d49958141e
@@ -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()
|
||||
Reference in New Issue
Block a user