提交scripts
This commit is contained in:
hesuicong committed
1 parent
82caf72036
commit
07dabbd186
224 files changed
+130146
No files matched your search
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,15 @@
|
||||
import numpy as np
|
||||
|
||||
|
||||
def calculate_distance(w2c1: np.ndarray, w2c2: np.ndarray) -> float:
|
||||
'''
|
||||
计算两个相机之间的距离
|
||||
|
||||
Args:
|
||||
w2c1: 相机1的世界坐标到相机坐标变换矩阵
|
||||
w2c2: 相机2的世界坐标到相机坐标变换矩阵
|
||||
|
||||
Returns:
|
||||
float: 两个相机之间的距离
|
||||
'''
|
||||
return np.linalg.norm(w2c1[:3, 3] - w2c2[:3, 3])
|
||||
@@ -0,0 +1,840 @@
|
||||
#
|
||||
# Copyright (C) 2023, Inria
|
||||
# GRAPHDECO research group, https://team.inria.fr/graphdeco
|
||||
# All rights reserved.
|
||||
#
|
||||
# This software is free for non-commercial, research and evaluation use
|
||||
# under the terms of the LICENSE.md file.
|
||||
#
|
||||
# For inquiries contact george.drettakis@inria.fr
|
||||
#
|
||||
|
||||
import numpy as np
|
||||
import collections
|
||||
import struct
|
||||
import math
|
||||
import os
|
||||
|
||||
CameraModel = collections.namedtuple(
|
||||
"CameraModel", ["model_id", "model_name", "num_params"]
|
||||
)
|
||||
Camera = collections.namedtuple("Camera", ["id", "model", "width", "height", "params"])
|
||||
BaseImage = collections.namedtuple(
|
||||
"Image", ["id", "qvec", "tvec", "camera_id", "name", "xys", "point3D_ids"]
|
||||
)
|
||||
Point3D = collections.namedtuple(
|
||||
"Point3D", ["id", "xyz", "rgb", "error", "image_ids", "point2D_idxs"]
|
||||
)
|
||||
CAMERA_MODELS = {
|
||||
CameraModel(model_id=0, model_name="SIMPLE_PINHOLE", num_params=3),
|
||||
CameraModel(model_id=1, model_name="PINHOLE", num_params=4),
|
||||
CameraModel(model_id=2, model_name="SIMPLE_RADIAL", num_params=4),
|
||||
CameraModel(model_id=3, model_name="RADIAL", num_params=5),
|
||||
CameraModel(model_id=4, model_name="OPENCV", num_params=8),
|
||||
CameraModel(model_id=5, model_name="OPENCV_FISHEYE", num_params=8),
|
||||
CameraModel(model_id=6, model_name="FULL_OPENCV", num_params=12),
|
||||
CameraModel(model_id=7, model_name="FOV", num_params=5),
|
||||
CameraModel(model_id=8, model_name="SIMPLE_RADIAL_FISHEYE", num_params=4),
|
||||
CameraModel(model_id=9, model_name="RADIAL_FISHEYE", num_params=5),
|
||||
CameraModel(model_id=10, model_name="THIN_PRISM_FISHEYE", num_params=12),
|
||||
}
|
||||
CAMERA_MODEL_IDS = dict(
|
||||
[(camera_model.model_id, camera_model) for camera_model in CAMERA_MODELS]
|
||||
)
|
||||
CAMERA_MODEL_NAMES = dict(
|
||||
[(camera_model.model_name, camera_model) for camera_model in CAMERA_MODELS]
|
||||
)
|
||||
|
||||
|
||||
def qvec2rotmat(qvec):
|
||||
return np.array(
|
||||
[
|
||||
[
|
||||
1 - 2 * qvec[2] ** 2 - 2 * qvec[3] ** 2,
|
||||
2 * qvec[1] * qvec[2] - 2 * qvec[0] * qvec[3],
|
||||
2 * qvec[3] * qvec[1] + 2 * qvec[0] * qvec[2],
|
||||
],
|
||||
[
|
||||
2 * qvec[1] * qvec[2] + 2 * qvec[0] * qvec[3],
|
||||
1 - 2 * qvec[1] ** 2 - 2 * qvec[3] ** 2,
|
||||
2 * qvec[2] * qvec[3] - 2 * qvec[0] * qvec[1],
|
||||
],
|
||||
[
|
||||
2 * qvec[3] * qvec[1] - 2 * qvec[0] * qvec[2],
|
||||
2 * qvec[2] * qvec[3] + 2 * qvec[0] * qvec[1],
|
||||
1 - 2 * qvec[1] ** 2 - 2 * qvec[2] ** 2,
|
||||
],
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
def rotmat2qvec(R):
|
||||
Rxx, Ryx, Rzx, Rxy, Ryy, Rzy, Rxz, Ryz, Rzz = R.flat
|
||||
K = (
|
||||
np.array(
|
||||
[
|
||||
[Rxx - Ryy - Rzz, 0, 0, 0],
|
||||
[Ryx + Rxy, Ryy - Rxx - Rzz, 0, 0],
|
||||
[Rzx + Rxz, Rzy + Ryz, Rzz - Rxx - Ryy, 0],
|
||||
[Ryz - Rzy, Rzx - Rxz, Rxy - Ryx, Rxx + Ryy + Rzz],
|
||||
]
|
||||
)
|
||||
/ 3.0
|
||||
)
|
||||
eigvals, eigvecs = np.linalg.eigh(K)
|
||||
qvec = eigvecs[[3, 0, 1, 2], np.argmax(eigvals)]
|
||||
if qvec[0] < 0:
|
||||
qvec *= -1
|
||||
return qvec
|
||||
|
||||
|
||||
class Image(BaseImage):
|
||||
def qvec2rotmat(self):
|
||||
return qvec2rotmat(self.qvec)
|
||||
|
||||
|
||||
def read_next_bytes(fid, num_bytes, format_char_sequence, endian_character="<"):
|
||||
"""Read and unpack the next bytes from a binary file.
|
||||
:param fid:
|
||||
:param num_bytes: Sum of combination of {2, 4, 8}, e.g. 2, 6, 16, 30, etc.
|
||||
:param format_char_sequence: List of {c, e, f, d, h, H, i, I, l, L, q, Q}.
|
||||
:param endian_character: Any of {@, =, <, >, !}
|
||||
:return: Tuple of read and unpacked values.
|
||||
"""
|
||||
data = fid.read(num_bytes)
|
||||
return struct.unpack(endian_character + format_char_sequence, data)
|
||||
|
||||
|
||||
def read_points3D_text(path):
|
||||
"""
|
||||
see: src/base/reconstruction.cc
|
||||
void Reconstruction::ReadPoints3DText(const std::string& path)
|
||||
void Reconstruction::WritePoints3DText(const std::string& path)
|
||||
"""
|
||||
xyzs = None
|
||||
rgbs = None
|
||||
errors = None
|
||||
num_points = 0
|
||||
with open(path, "r") as fid:
|
||||
while True:
|
||||
line = fid.readline()
|
||||
if not line:
|
||||
break
|
||||
line = line.strip()
|
||||
if len(line) > 0 and line[0] != "#":
|
||||
num_points += 1
|
||||
|
||||
xyzs = np.empty((num_points, 3))
|
||||
rgbs = np.empty((num_points, 3))
|
||||
errors = np.empty((num_points, 1))
|
||||
count = 0
|
||||
with open(path, "r") as fid:
|
||||
while True:
|
||||
line = fid.readline()
|
||||
if not line:
|
||||
break
|
||||
line = line.strip()
|
||||
if len(line) > 0 and line[0] != "#":
|
||||
elems = line.split()
|
||||
xyz = np.array(tuple(map(float, elems[1:4])))
|
||||
rgb = np.array(tuple(map(int, elems[4:7])))
|
||||
error = np.array(float(elems[7]))
|
||||
xyzs[count] = xyz
|
||||
rgbs[count] = rgb
|
||||
errors[count] = error
|
||||
count += 1
|
||||
|
||||
return xyzs, rgbs, errors
|
||||
|
||||
|
||||
def read_points3D_binary(path_to_model_file):
|
||||
"""
|
||||
see: src/base/reconstruction.cc
|
||||
void Reconstruction::ReadPoints3DBinary(const std::string& path)
|
||||
void Reconstruction::WritePoints3DBinary(const std::string& path)
|
||||
"""
|
||||
|
||||
with open(path_to_model_file, "rb") as fid:
|
||||
num_points = read_next_bytes(fid, 8, "Q")[0]
|
||||
|
||||
xyzs = np.empty((num_points, 3))
|
||||
rgbs = np.empty((num_points, 3))
|
||||
errors = np.empty((num_points, 1))
|
||||
|
||||
for p_id in range(num_points):
|
||||
binary_point_line_properties = read_next_bytes(
|
||||
fid, num_bytes=43, format_char_sequence="QdddBBBd"
|
||||
)
|
||||
xyz = np.array(binary_point_line_properties[1:4])
|
||||
rgb = np.array(binary_point_line_properties[4:7])
|
||||
error = np.array(binary_point_line_properties[7])
|
||||
track_length = read_next_bytes(fid, num_bytes=8, format_char_sequence="Q")[
|
||||
0
|
||||
]
|
||||
track_elems = read_next_bytes(
|
||||
fid,
|
||||
num_bytes=8 * track_length,
|
||||
format_char_sequence="ii" * track_length,
|
||||
)
|
||||
xyzs[p_id] = xyz
|
||||
rgbs[p_id] = rgb
|
||||
errors[p_id] = error
|
||||
return xyzs, rgbs, errors
|
||||
|
||||
|
||||
def read_intrinsics_text(path):
|
||||
"""
|
||||
Taken from https://github.com/colmap/colmap/blob/dev/scripts/python/read_write_model.py
|
||||
"""
|
||||
cameras = {}
|
||||
with open(path, "r") as fid:
|
||||
while True:
|
||||
line = fid.readline()
|
||||
if not line:
|
||||
break
|
||||
line = line.strip()
|
||||
if len(line) > 0 and line[0] != "#":
|
||||
elems = line.split()
|
||||
camera_id = int(elems[0])
|
||||
model = elems[1]
|
||||
assert (
|
||||
model == "PINHOLE"
|
||||
), "While the loader support other types, the rest of the code assumes PINHOLE"
|
||||
width = int(elems[2])
|
||||
height = int(elems[3])
|
||||
params = np.array(tuple(map(float, elems[4:])))
|
||||
cameras[camera_id] = Camera(
|
||||
id=camera_id, model=model, width=width, height=height, params=params
|
||||
)
|
||||
return cameras
|
||||
|
||||
|
||||
def read_extrinsics_binary(path_to_model_file):
|
||||
"""
|
||||
see: src/base/reconstruction.cc
|
||||
void Reconstruction::ReadImagesBinary(const std::string& path)
|
||||
void Reconstruction::WriteImagesBinary(const std::string& path)
|
||||
"""
|
||||
images = {}
|
||||
with open(path_to_model_file, "rb") as fid:
|
||||
num_reg_images = read_next_bytes(fid, 8, "Q")[0]
|
||||
for _ in range(num_reg_images):
|
||||
binary_image_properties = read_next_bytes(
|
||||
fid, num_bytes=64, format_char_sequence="idddddddi"
|
||||
)
|
||||
image_id = binary_image_properties[0]
|
||||
qvec = np.array(binary_image_properties[1:5])
|
||||
tvec = np.array(binary_image_properties[5:8])
|
||||
camera_id = binary_image_properties[8]
|
||||
image_name = ""
|
||||
current_char = read_next_bytes(fid, 1, "c")[0]
|
||||
while current_char != b"\x00": # look for the ASCII 0 entry
|
||||
image_name += current_char.decode("utf-8")
|
||||
current_char = read_next_bytes(fid, 1, "c")[0]
|
||||
num_points2D = read_next_bytes(fid, num_bytes=8, format_char_sequence="Q")[
|
||||
0
|
||||
]
|
||||
x_y_id_s = read_next_bytes(
|
||||
fid,
|
||||
num_bytes=24 * num_points2D,
|
||||
format_char_sequence="ddq" * num_points2D,
|
||||
)
|
||||
xys = np.column_stack(
|
||||
[tuple(map(float, x_y_id_s[0::3])), tuple(map(float, x_y_id_s[1::3]))]
|
||||
)
|
||||
point3D_ids = np.array(tuple(map(int, x_y_id_s[2::3])))
|
||||
images[image_id] = Image(
|
||||
id=image_id,
|
||||
qvec=qvec,
|
||||
tvec=tvec,
|
||||
camera_id=camera_id,
|
||||
name=image_name,
|
||||
xys=xys,
|
||||
point3D_ids=point3D_ids,
|
||||
)
|
||||
return images
|
||||
|
||||
|
||||
def read_intrinsics_binary(path_to_model_file):
|
||||
"""
|
||||
see: src/base/reconstruction.cc
|
||||
void Reconstruction::WriteCamerasBinary(const std::string& path)
|
||||
void Reconstruction::ReadCamerasBinary(const std::string& path)
|
||||
"""
|
||||
cameras = {}
|
||||
with open(path_to_model_file, "rb") as fid:
|
||||
num_cameras = read_next_bytes(fid, 8, "Q")[0]
|
||||
for _ in range(num_cameras):
|
||||
camera_properties = read_next_bytes(
|
||||
fid, num_bytes=24, format_char_sequence="iiQQ"
|
||||
)
|
||||
camera_id = camera_properties[0]
|
||||
model_id = camera_properties[1]
|
||||
model_name = CAMERA_MODEL_IDS[camera_properties[1]].model_name
|
||||
width = camera_properties[2]
|
||||
height = camera_properties[3]
|
||||
num_params = CAMERA_MODEL_IDS[model_id].num_params
|
||||
params = read_next_bytes(
|
||||
fid, num_bytes=8 * num_params, format_char_sequence="d" * num_params
|
||||
)
|
||||
cameras[camera_id] = Camera(
|
||||
id=camera_id,
|
||||
model=model_name,
|
||||
width=width,
|
||||
height=height,
|
||||
params=np.array(params),
|
||||
)
|
||||
assert len(cameras) == num_cameras
|
||||
return cameras
|
||||
|
||||
|
||||
def focal2fov(focal, pixels):
|
||||
return 2 * math.atan(pixels / (2 * focal))
|
||||
|
||||
|
||||
def read_extrinsics_text(path):
|
||||
"""
|
||||
Taken from https://github.com/colmap/colmap/blob/dev/scripts/python/read_write_model.py
|
||||
"""
|
||||
images = {}
|
||||
with open(path, "r") as fid:
|
||||
while True:
|
||||
line = fid.readline()
|
||||
if not line:
|
||||
break
|
||||
line = line.strip()
|
||||
if len(line) > 0 and line[0] != "#":
|
||||
elems = line.split()
|
||||
image_id = int(elems[0])
|
||||
qvec = np.array(tuple(map(float, elems[1:5])))
|
||||
tvec = np.array(tuple(map(float, elems[5:8])))
|
||||
camera_id = int(elems[8])
|
||||
image_name = elems[9]
|
||||
elems = fid.readline().split()
|
||||
xys = np.column_stack(
|
||||
[tuple(map(float, elems[0::3])), tuple(map(float, elems[1::3]))]
|
||||
)
|
||||
point3D_ids = np.array(tuple(map(int, elems[2::3])))
|
||||
images[image_id] = Image(
|
||||
id=image_id,
|
||||
qvec=qvec,
|
||||
tvec=tvec,
|
||||
camera_id=camera_id,
|
||||
name=image_name,
|
||||
xys=xys,
|
||||
point3D_ids=point3D_ids,
|
||||
)
|
||||
return images
|
||||
|
||||
|
||||
def read_colmap_bin_array(path):
|
||||
"""
|
||||
Taken from https://github.com/colmap/colmap/blob/dev/scripts/python/read_dense.py
|
||||
|
||||
:param path: path to the colmap binary file.
|
||||
:return: nd array with the floating point values in the value
|
||||
"""
|
||||
with open(path, "rb") as fid:
|
||||
width, height, channels = np.genfromtxt(
|
||||
fid, delimiter="&", max_rows=1, usecols=(0, 1, 2), dtype=int
|
||||
)
|
||||
fid.seek(0)
|
||||
num_delimiter = 0
|
||||
byte = fid.read(1)
|
||||
while True:
|
||||
if byte == b"&":
|
||||
num_delimiter += 1
|
||||
if num_delimiter >= 3:
|
||||
break
|
||||
byte = fid.read(1)
|
||||
array = np.fromfile(fid, np.float32)
|
||||
array = array.reshape((width, height, channels), order="F")
|
||||
return np.transpose(array, (1, 0, 2)).squeeze()
|
||||
|
||||
|
||||
def read_next_bytes(fid, num_bytes, format_char_sequence, endian_character="<"):
|
||||
"""Read and unpack the next bytes from a binary file.
|
||||
:param fid:
|
||||
:param num_bytes: Sum of combination of {2, 4, 8}, e.g. 2, 6, 16, 30, etc.
|
||||
:param format_char_sequence: List of {c, e, f, d, h, H, i, I, l, L, q, Q}.
|
||||
:param endian_character: Any of {@, =, <, >, !}
|
||||
:return: Tuple of read and unpacked values.
|
||||
"""
|
||||
data = fid.read(num_bytes)
|
||||
return struct.unpack(endian_character + format_char_sequence, data)
|
||||
|
||||
|
||||
def write_next_bytes(fid, data, format_char_sequence, endian_character="<"):
|
||||
"""pack and write to a binary file.
|
||||
:param fid:
|
||||
:param data: data to send, if multiple elements are sent at the same time,
|
||||
they should be encapsuled either in a list or a tuple
|
||||
:param format_char_sequence: List of {c, e, f, d, h, H, i, I, l, L, q, Q}.
|
||||
should be the same length as the data list or tuple
|
||||
:param endian_character: Any of {@, =, <, >, !}
|
||||
"""
|
||||
if isinstance(data, (list, tuple)):
|
||||
bytes = struct.pack(endian_character + format_char_sequence, *data)
|
||||
else:
|
||||
bytes = struct.pack(endian_character + format_char_sequence, data)
|
||||
fid.write(bytes)
|
||||
|
||||
|
||||
def read_cameras_text(path):
|
||||
"""
|
||||
see: src/colmap/scene/reconstruction.cc
|
||||
void Reconstruction::WriteCamerasText(const std::string& path)
|
||||
void Reconstruction::ReadCamerasText(const std::string& path)
|
||||
"""
|
||||
cameras = {}
|
||||
with open(path, "r") as fid:
|
||||
while True:
|
||||
line = fid.readline()
|
||||
if not line:
|
||||
break
|
||||
line = line.strip()
|
||||
if len(line) > 0 and line[0] != "#":
|
||||
elems = line.split()
|
||||
camera_id = int(elems[0])
|
||||
model = elems[1]
|
||||
width = int(elems[2])
|
||||
height = int(elems[3])
|
||||
params = np.array(tuple(map(float, elems[4:])))
|
||||
cameras[camera_id] = Camera(
|
||||
id=camera_id,
|
||||
model=model,
|
||||
width=width,
|
||||
height=height,
|
||||
params=params,
|
||||
)
|
||||
return cameras
|
||||
|
||||
|
||||
def read_cameras_binary(path_to_model_file):
|
||||
"""
|
||||
see: src/colmap/scene/reconstruction.cc
|
||||
void Reconstruction::WriteCamerasBinary(const std::string& path)
|
||||
void Reconstruction::ReadCamerasBinary(const std::string& path)
|
||||
"""
|
||||
cameras = {}
|
||||
with open(path_to_model_file, "rb") as fid:
|
||||
num_cameras = read_next_bytes(fid, 8, "Q")[0]
|
||||
for _ in range(num_cameras):
|
||||
camera_properties = read_next_bytes(
|
||||
fid, num_bytes=24, format_char_sequence="iiQQ"
|
||||
)
|
||||
camera_id = camera_properties[0]
|
||||
model_id = camera_properties[1]
|
||||
model_name = CAMERA_MODEL_IDS[camera_properties[1]].model_name
|
||||
width = camera_properties[2]
|
||||
height = camera_properties[3]
|
||||
num_params = CAMERA_MODEL_IDS[model_id].num_params
|
||||
params = read_next_bytes(
|
||||
fid,
|
||||
num_bytes=8 * num_params,
|
||||
format_char_sequence="d" * num_params,
|
||||
)
|
||||
cameras[camera_id] = Camera(
|
||||
id=camera_id,
|
||||
model=model_name,
|
||||
width=width,
|
||||
height=height,
|
||||
params=np.array(params),
|
||||
)
|
||||
assert len(cameras) == num_cameras
|
||||
return cameras
|
||||
|
||||
|
||||
def write_cameras_text(cameras, path):
|
||||
"""
|
||||
see: src/colmap/scene/reconstruction.cc
|
||||
void Reconstruction::WriteCamerasText(const std::string& path)
|
||||
void Reconstruction::ReadCamerasText(const std::string& path)
|
||||
"""
|
||||
HEADER = (
|
||||
"# Camera list with one line of data per camera:\n"
|
||||
+ "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]\n"
|
||||
+ "# Number of cameras: {}\n".format(len(cameras))
|
||||
)
|
||||
with open(path, "w") as fid:
|
||||
fid.write(HEADER)
|
||||
for _, cam in cameras.items():
|
||||
to_write = [cam.id, cam.model, cam.width, cam.height, *cam.params]
|
||||
line = " ".join([str(elem) for elem in to_write])
|
||||
fid.write(line + "\n")
|
||||
|
||||
|
||||
def write_cameras_binary(cameras, path_to_model_file):
|
||||
"""
|
||||
see: src/colmap/scene/reconstruction.cc
|
||||
void Reconstruction::WriteCamerasBinary(const std::string& path)
|
||||
void Reconstruction::ReadCamerasBinary(const std::string& path)
|
||||
"""
|
||||
with open(path_to_model_file, "wb") as fid:
|
||||
write_next_bytes(fid, len(cameras), "Q")
|
||||
for _, cam in cameras.items():
|
||||
model_id = CAMERA_MODEL_NAMES[cam.model].model_id
|
||||
camera_properties = [cam.id, model_id, cam.width, cam.height]
|
||||
write_next_bytes(fid, camera_properties, "iiQQ")
|
||||
for p in cam.params:
|
||||
write_next_bytes(fid, float(p), "d")
|
||||
return cameras
|
||||
|
||||
|
||||
def read_images_text(path):
|
||||
"""
|
||||
see: src/colmap/scene/reconstruction.cc
|
||||
void Reconstruction::ReadImagesText(const std::string& path)
|
||||
void Reconstruction::WriteImagesText(const std::string& path)
|
||||
"""
|
||||
images = {}
|
||||
with open(path, "r") as fid:
|
||||
while True:
|
||||
line = fid.readline()
|
||||
if not line:
|
||||
break
|
||||
line = line.strip()
|
||||
if len(line) > 0 and line[0] != "#":
|
||||
elems = line.split()
|
||||
image_id = int(elems[0])
|
||||
qvec = np.array(tuple(map(float, elems[1:5])))
|
||||
tvec = np.array(tuple(map(float, elems[5:8])))
|
||||
camera_id = int(elems[8])
|
||||
image_name = elems[9]
|
||||
elems = fid.readline().split()
|
||||
xys = np.column_stack(
|
||||
[
|
||||
tuple(map(float, elems[0::3])),
|
||||
tuple(map(float, elems[1::3])),
|
||||
]
|
||||
)
|
||||
point3D_ids = np.array(tuple(map(int, elems[2::3])))
|
||||
images[image_id] = Image(
|
||||
id=image_id,
|
||||
qvec=qvec,
|
||||
tvec=tvec,
|
||||
camera_id=camera_id,
|
||||
name=image_name,
|
||||
xys=xys,
|
||||
point3D_ids=point3D_ids,
|
||||
)
|
||||
return images
|
||||
|
||||
|
||||
def read_images_binary(path_to_model_file):
|
||||
"""
|
||||
see: src/colmap/scene/reconstruction.cc
|
||||
void Reconstruction::ReadImagesBinary(const std::string& path)
|
||||
void Reconstruction::WriteImagesBinary(const std::string& path)
|
||||
"""
|
||||
images = {}
|
||||
with open(path_to_model_file, "rb") as fid:
|
||||
num_reg_images = read_next_bytes(fid, 8, "Q")[0]
|
||||
for _ in range(num_reg_images):
|
||||
binary_image_properties = read_next_bytes(
|
||||
fid, num_bytes=64, format_char_sequence="idddddddi"
|
||||
)
|
||||
image_id = binary_image_properties[0]
|
||||
qvec = np.array(binary_image_properties[1:5])
|
||||
tvec = np.array(binary_image_properties[5:8])
|
||||
camera_id = binary_image_properties[8]
|
||||
binary_image_name = b""
|
||||
current_char = read_next_bytes(fid, 1, "c")[0]
|
||||
while current_char != b"\x00": # look for the ASCII 0 entry
|
||||
binary_image_name += current_char
|
||||
current_char = read_next_bytes(fid, 1, "c")[0]
|
||||
image_name = binary_image_name.decode("utf-8")
|
||||
num_points2D = read_next_bytes(fid, num_bytes=8, format_char_sequence="Q")[
|
||||
0
|
||||
]
|
||||
x_y_id_s = read_next_bytes(
|
||||
fid,
|
||||
num_bytes=24 * num_points2D,
|
||||
format_char_sequence="ddq" * num_points2D,
|
||||
)
|
||||
xys = np.column_stack(
|
||||
[
|
||||
tuple(map(float, x_y_id_s[0::3])),
|
||||
tuple(map(float, x_y_id_s[1::3])),
|
||||
]
|
||||
)
|
||||
point3D_ids = np.array(tuple(map(int, x_y_id_s[2::3])))
|
||||
images[image_id] = Image(
|
||||
id=image_id,
|
||||
qvec=qvec,
|
||||
tvec=tvec,
|
||||
camera_id=camera_id,
|
||||
name=image_name,
|
||||
xys=xys,
|
||||
point3D_ids=point3D_ids,
|
||||
)
|
||||
return images
|
||||
|
||||
|
||||
def write_images_text(images, path):
|
||||
"""
|
||||
see: src/colmap/scene/reconstruction.cc
|
||||
void Reconstruction::ReadImagesText(const std::string& path)
|
||||
void Reconstruction::WriteImagesText(const std::string& path)
|
||||
"""
|
||||
if len(images) == 0:
|
||||
mean_observations = 0
|
||||
else:
|
||||
mean_observations = sum(
|
||||
(len(img.point3D_ids) for _, img in images.items())
|
||||
) / len(images)
|
||||
HEADER = (
|
||||
"# Image list with two lines of data per image:\n"
|
||||
+ "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n"
|
||||
+ "# POINTS2D[] as (X, Y, POINT3D_ID)\n"
|
||||
+ "# Number of images: {}, mean observations per image: {}\n".format(
|
||||
len(images), mean_observations
|
||||
)
|
||||
)
|
||||
|
||||
with open(path, "w") as fid:
|
||||
fid.write(HEADER)
|
||||
for _, img in images.items():
|
||||
image_header = [
|
||||
img.id,
|
||||
*img.qvec,
|
||||
*img.tvec,
|
||||
img.camera_id,
|
||||
img.name,
|
||||
]
|
||||
first_line = " ".join(map(str, image_header))
|
||||
fid.write(first_line + "\n")
|
||||
|
||||
points_strings = []
|
||||
for xy, point3D_id in zip(img.xys, img.point3D_ids):
|
||||
points_strings.append(" ".join(map(str, [*xy, point3D_id])))
|
||||
fid.write(" ".join(points_strings) + "\n")
|
||||
|
||||
|
||||
def write_images_binary(images, path_to_model_file):
|
||||
"""
|
||||
see: src/colmap/scene/reconstruction.cc
|
||||
void Reconstruction::ReadImagesBinary(const std::string& path)
|
||||
void Reconstruction::WriteImagesBinary(const std::string& path)
|
||||
"""
|
||||
with open(path_to_model_file, "wb") as fid:
|
||||
write_next_bytes(fid, len(images), "Q")
|
||||
for _, img in images.items():
|
||||
write_next_bytes(fid, img.id, "i")
|
||||
write_next_bytes(fid, img.qvec.tolist(), "dddd")
|
||||
write_next_bytes(fid, img.tvec.tolist(), "ddd")
|
||||
write_next_bytes(fid, img.camera_id, "i")
|
||||
for char in img.name:
|
||||
write_next_bytes(fid, char.encode("utf-8"), "c")
|
||||
write_next_bytes(fid, b"\x00", "c")
|
||||
write_next_bytes(fid, len(img.point3D_ids), "Q")
|
||||
for xy, p3d_id in zip(img.xys, img.point3D_ids):
|
||||
write_next_bytes(fid, [*xy, p3d_id], "ddq")
|
||||
|
||||
|
||||
def read_points3D_text(path):
|
||||
"""
|
||||
see: src/colmap/scene/reconstruction.cc
|
||||
void Reconstruction::ReadPoints3DText(const std::string& path)
|
||||
void Reconstruction::WritePoints3DText(const std::string& path)
|
||||
"""
|
||||
points3D = {}
|
||||
with open(path, "r") as fid:
|
||||
while True:
|
||||
line = fid.readline()
|
||||
if not line:
|
||||
break
|
||||
line = line.strip()
|
||||
if len(line) > 0 and line[0] != "#":
|
||||
elems = line.split()
|
||||
point3D_id = int(elems[0])
|
||||
xyz = np.array(tuple(map(float, elems[1:4])))
|
||||
rgb = np.array(tuple(map(int, elems[4:7])))
|
||||
error = float(elems[7])
|
||||
image_ids = np.array(tuple(map(int, elems[8::2])))
|
||||
point2D_idxs = np.array(tuple(map(int, elems[9::2])))
|
||||
points3D[point3D_id] = Point3D(
|
||||
id=point3D_id,
|
||||
xyz=xyz,
|
||||
rgb=rgb,
|
||||
error=error,
|
||||
image_ids=image_ids,
|
||||
point2D_idxs=point2D_idxs,
|
||||
)
|
||||
return points3D
|
||||
|
||||
|
||||
def read_points3D_binary(path_to_model_file):
|
||||
"""
|
||||
see: src/colmap/scene/reconstruction.cc
|
||||
void Reconstruction::ReadPoints3DBinary(const std::string& path)
|
||||
void Reconstruction::WritePoints3DBinary(const std::string& path)
|
||||
"""
|
||||
points3D = {}
|
||||
with open(path_to_model_file, "rb") as fid:
|
||||
num_points = read_next_bytes(fid, 8, "Q")[0]
|
||||
for _ in range(num_points):
|
||||
binary_point_line_properties = read_next_bytes(
|
||||
fid, num_bytes=43, format_char_sequence="QdddBBBd"
|
||||
)
|
||||
point3D_id = binary_point_line_properties[0]
|
||||
xyz = np.array(binary_point_line_properties[1:4])
|
||||
rgb = np.array(binary_point_line_properties[4:7])
|
||||
error = np.array(binary_point_line_properties[7])
|
||||
track_length = read_next_bytes(fid, num_bytes=8, format_char_sequence="Q")[
|
||||
0
|
||||
]
|
||||
track_elems = read_next_bytes(
|
||||
fid,
|
||||
num_bytes=8 * track_length,
|
||||
format_char_sequence="ii" * track_length,
|
||||
)
|
||||
image_ids = np.array(tuple(map(int, track_elems[0::2])))
|
||||
point2D_idxs = np.array(tuple(map(int, track_elems[1::2])))
|
||||
points3D[point3D_id] = Point3D(
|
||||
id=point3D_id,
|
||||
xyz=xyz,
|
||||
rgb=rgb,
|
||||
error=error,
|
||||
image_ids=image_ids,
|
||||
point2D_idxs=point2D_idxs,
|
||||
)
|
||||
return points3D
|
||||
|
||||
|
||||
def write_points3D_text(points3D, path):
|
||||
"""
|
||||
see: src/colmap/scene/reconstruction.cc
|
||||
void Reconstruction::ReadPoints3DText(const std::string& path)
|
||||
void Reconstruction::WritePoints3DText(const std::string& path)
|
||||
"""
|
||||
if len(points3D) == 0:
|
||||
mean_track_length = 0
|
||||
else:
|
||||
mean_track_length = sum(
|
||||
(len(pt.image_ids) for _, pt in points3D.items())
|
||||
) / len(points3D)
|
||||
HEADER = (
|
||||
"# 3D point list with one line of data per point:\n"
|
||||
+ "# POINT3D_ID, X, Y, Z, R, G, B, ERROR, TRACK[] as (IMAGE_ID, POINT2D_IDX)\n"
|
||||
+ "# Number of points: {}, mean track length: {}\n".format(
|
||||
len(points3D), mean_track_length
|
||||
)
|
||||
)
|
||||
|
||||
with open(path, "w") as fid:
|
||||
fid.write(HEADER)
|
||||
for _, pt in points3D.items():
|
||||
point_header = [pt.id, *pt.xyz, *pt.rgb, pt.error]
|
||||
fid.write(" ".join(map(str, point_header)) + " ")
|
||||
track_strings = []
|
||||
for image_id, point2D in zip(pt.image_ids, pt.point2D_idxs):
|
||||
track_strings.append(" ".join(map(str, [image_id, point2D])))
|
||||
fid.write(" ".join(track_strings) + "\n")
|
||||
|
||||
|
||||
def write_points3D_binary(points3D, path_to_model_file):
|
||||
"""
|
||||
see: src/colmap/scene/reconstruction.cc
|
||||
void Reconstruction::ReadPoints3DBinary(const std::string& path)
|
||||
void Reconstruction::WritePoints3DBinary(const std::string& path)
|
||||
"""
|
||||
with open(path_to_model_file, "wb") as fid:
|
||||
write_next_bytes(fid, len(points3D), "Q")
|
||||
for _, pt in points3D.items():
|
||||
write_next_bytes(fid, pt.id, "Q")
|
||||
write_next_bytes(fid, pt.xyz.tolist(), "ddd")
|
||||
write_next_bytes(fid, pt.rgb.tolist(), "BBB")
|
||||
write_next_bytes(fid, pt.error, "d")
|
||||
track_length = pt.image_ids.shape[0]
|
||||
write_next_bytes(fid, track_length, "Q")
|
||||
for image_id, point2D_id in zip(pt.image_ids, pt.point2D_idxs):
|
||||
write_next_bytes(fid, [image_id, point2D_id], "ii")
|
||||
|
||||
|
||||
def detect_model_format(path, ext):
|
||||
if (
|
||||
os.path.isfile(os.path.join(path, "cameras" + ext))
|
||||
and os.path.isfile(os.path.join(path, "images" + ext))
|
||||
and os.path.isfile(os.path.join(path, "points3D" + ext))
|
||||
):
|
||||
print("Detected model format: '" + ext + "'")
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
|
||||
def read_model(path, ext=""):
|
||||
# try to detect the extension automatically
|
||||
if ext == "":
|
||||
if detect_model_format(path, ".bin"):
|
||||
ext = ".bin"
|
||||
elif detect_model_format(path, ".txt"):
|
||||
ext = ".txt"
|
||||
else:
|
||||
print("Provide model format: '.bin' or '.txt'")
|
||||
return
|
||||
|
||||
if ext == ".txt":
|
||||
cameras = read_cameras_text(os.path.join(path, "cameras" + ext))
|
||||
images = read_images_text(os.path.join(path, "images" + ext))
|
||||
points3D = read_points3D_text(os.path.join(path, "points3D") + ext)
|
||||
else:
|
||||
cameras = read_cameras_binary(os.path.join(path, "cameras" + ext))
|
||||
images = read_images_binary(os.path.join(path, "images" + ext))
|
||||
points3D = read_points3D_binary(os.path.join(path, "points3D") + ext)
|
||||
return cameras, images, points3D
|
||||
|
||||
|
||||
def write_model(cameras, images, points3D, path, ext=".bin"):
|
||||
if ext == ".txt":
|
||||
write_cameras_text(cameras, os.path.join(path, "cameras" + ext))
|
||||
write_images_text(images, os.path.join(path, "images" + ext))
|
||||
write_points3D_text(points3D, os.path.join(path, "points3D") + ext)
|
||||
else:
|
||||
write_cameras_binary(cameras, os.path.join(path, "cameras" + ext))
|
||||
write_images_binary(images, os.path.join(path, "images" + ext))
|
||||
write_points3D_binary(points3D, os.path.join(path, "points3D") + ext)
|
||||
return cameras, images, points3D
|
||||
|
||||
|
||||
def qvec2rotmat(qvec):
|
||||
return np.array(
|
||||
[
|
||||
[
|
||||
1 - 2 * qvec[2] ** 2 - 2 * qvec[3] ** 2,
|
||||
2 * qvec[1] * qvec[2] - 2 * qvec[0] * qvec[3],
|
||||
2 * qvec[3] * qvec[1] + 2 * qvec[0] * qvec[2],
|
||||
],
|
||||
[
|
||||
2 * qvec[1] * qvec[2] + 2 * qvec[0] * qvec[3],
|
||||
1 - 2 * qvec[1] ** 2 - 2 * qvec[3] ** 2,
|
||||
2 * qvec[2] * qvec[3] - 2 * qvec[0] * qvec[1],
|
||||
],
|
||||
[
|
||||
2 * qvec[3] * qvec[1] - 2 * qvec[0] * qvec[2],
|
||||
2 * qvec[2] * qvec[3] + 2 * qvec[0] * qvec[1],
|
||||
1 - 2 * qvec[1] ** 2 - 2 * qvec[2] ** 2,
|
||||
],
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
def rotmat2qvec(R):
|
||||
Rxx, Ryx, Rzx, Rxy, Ryy, Rzy, Rxz, Ryz, Rzz = R.flat
|
||||
K = (
|
||||
np.array(
|
||||
[
|
||||
[Rxx - Ryy - Rzz, 0, 0, 0],
|
||||
[Ryx + Rxy, Ryy - Rxx - Rzz, 0, 0],
|
||||
[Rzx + Rxz, Rzy + Ryz, Rzz - Rxx - Ryy, 0],
|
||||
[Ryz - Rzy, Rzx - Rxz, Rxy - Ryx, Rxx + Ryy + Rzz],
|
||||
]
|
||||
)
|
||||
/ 3.0
|
||||
)
|
||||
eigvals, eigvecs = np.linalg.eigh(K)
|
||||
qvec = eigvecs[[3, 0, 1, 2], np.argmax(eigvals)]
|
||||
if qvec[0] < 0:
|
||||
qvec *= -1
|
||||
return qvec
|
||||
@@ -0,0 +1,36 @@
|
||||
import numpy as np
|
||||
import open3d as o3d
|
||||
from utils.get_pose_matrix import get_c2w,get_w2c
|
||||
|
||||
def get_cam_infos(images: dict, cameras:dict):
|
||||
cam_infos = {}
|
||||
for image_id, image_info in images.items():
|
||||
camera_info = cameras[image_info.camera_id]
|
||||
fx = camera_info.params[0]
|
||||
fy = camera_info.params[1]
|
||||
cx = camera_info.params[2]
|
||||
cy = camera_info.params[3]
|
||||
width = camera_info.width
|
||||
height = camera_info.height
|
||||
name = image_info.name
|
||||
cam_infos[image_id] = {
|
||||
"name": name,
|
||||
"w2c": get_w2c(image_info.qvec, image_info.tvec),
|
||||
"c2w": get_c2w(image_info.qvec, image_info.tvec),
|
||||
"width": camera_info.width,
|
||||
"height": camera_info.height,
|
||||
"intrinsics": o3d.camera.PinholeCameraIntrinsic(
|
||||
width,
|
||||
height,
|
||||
fx=fx,
|
||||
fy=fy,
|
||||
cx=cx,
|
||||
cy=cy
|
||||
),
|
||||
"K": np.array([
|
||||
[fx, 0, cx],
|
||||
[0, fy, cy],
|
||||
[0, 0, 1]
|
||||
])
|
||||
}
|
||||
return cam_infos
|
||||
@@ -0,0 +1,26 @@
|
||||
'''
|
||||
获取Mesh的深度图
|
||||
|
||||
required:
|
||||
- open3d
|
||||
- numpy
|
||||
'''
|
||||
|
||||
import open3d as o3d
|
||||
import numpy as np
|
||||
|
||||
def get_depth(w2c: np.ndarray, intrinsic: o3d.camera.PinholeCameraIntrinsic, render: o3d.visualization.rendering.OffscreenRenderer) -> o3d.geometry.Image:
|
||||
'''
|
||||
获取rgbd图像
|
||||
|
||||
param:
|
||||
mesh: 需要渲染的mesh
|
||||
w2c: 世界坐标系到相机坐标系的变换矩阵
|
||||
K: 相机内参
|
||||
render: 渲染器
|
||||
'''
|
||||
render.setup_camera(intrinsic, w2c)
|
||||
|
||||
depth = render.render_to_depth_image(z_in_view_space=True)
|
||||
|
||||
return np.asarray(depth)
|
||||
@@ -0,0 +1,53 @@
|
||||
|
||||
import numpy as np
|
||||
import cv2
|
||||
|
||||
def create_depth_difference_mask(depth1: np.ndarray, depth2: np.ndarray,
|
||||
threshold: float = 0.1,
|
||||
gaussian_kernel: int = 5,
|
||||
min_region_size: int = 50) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""
|
||||
创建两个深度图之间差异的掩码图像，返回原始掩码和滤波后的掩码
|
||||
|
||||
Args:
|
||||
depth1: 第一个深度图
|
||||
depth2: 第二个深度图
|
||||
threshold: 判定为不一致的深度差异阈值
|
||||
gaussian_kernel: 高斯滤波核大小
|
||||
min_region_size: 最小连通区域大小，小于该值的区域将被过滤掉
|
||||
|
||||
Returns:
|
||||
tuple[np.ndarray, np.ndarray]:
|
||||
- raw_mask: 原始差异掩码，未经滤波处理
|
||||
- filtered_mask: 经过滤波处理后的差异掩码
|
||||
"""
|
||||
# 创建有效区域的掩码（两个深度图都有有效值的区域）
|
||||
valid_mask = (depth1 > 0) & (depth2 > 0)
|
||||
|
||||
# 计算深度差异
|
||||
diff = np.abs(depth1 - depth2)
|
||||
diff[~valid_mask] = 0
|
||||
|
||||
# 创建原始差异掩码
|
||||
raw_mask = np.zeros_like(depth1, dtype=np.uint8)
|
||||
raw_mask[valid_mask & (diff > threshold)] = 255
|
||||
|
||||
# 对原始掩码进行高斯滤波
|
||||
filtered_mask = cv2.GaussianBlur(raw_mask, (gaussian_kernel, gaussian_kernel), 0)
|
||||
|
||||
# 重新二值化
|
||||
_, filtered_mask = cv2.threshold(filtered_mask, 127, 255, cv2.THRESH_BINARY)
|
||||
|
||||
# 应用形态学操作
|
||||
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
|
||||
filtered_mask = cv2.morphologyEx(filtered_mask, cv2.MORPH_OPEN, kernel) # 开运算去除小噪点
|
||||
filtered_mask = cv2.morphologyEx(filtered_mask, cv2.MORPH_CLOSE, kernel) # 闭运算填充小孔
|
||||
|
||||
# 连通区域分析和过滤
|
||||
if min_region_size > 0:
|
||||
num_labels, labels, stats, centroids = cv2.connectedComponentsWithStats(filtered_mask, connectivity=8)
|
||||
for i in range(1, num_labels): # 从1开始，跳过背景
|
||||
if stats[i, cv2.CC_STAT_AREA] < min_region_size:
|
||||
filtered_mask[labels == i] = 0
|
||||
|
||||
return raw_mask, filtered_mask
|
||||
@@ -0,0 +1,105 @@
|
||||
'''
|
||||
获取pose矩阵
|
||||
|
||||
required:
|
||||
- numpy
|
||||
'''
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
def qvec2rotmat(qvec):
|
||||
'''将四元数转换为旋转矩阵
|
||||
|
||||
Args:
|
||||
qvec: 四元数
|
||||
|
||||
Returns:
|
||||
旋转矩阵
|
||||
'''
|
||||
return Rotation.from_quat([qvec[1], qvec[2], qvec[3], qvec[0]]).as_matrix()
|
||||
"""
|
||||
return np.array(
|
||||
[
|
||||
[
|
||||
1 - 2 * qvec[2] ** 2 - 2 * qvec[3] ** 2,
|
||||
2 * qvec[1] * qvec[2] - 2 * qvec[0] * qvec[3],
|
||||
2 * qvec[3] * qvec[1] + 2 * qvec[0] * qvec[2],
|
||||
],
|
||||
[
|
||||
2 * qvec[1] * qvec[2] + 2 * qvec[0] * qvec[3],
|
||||
1 - 2 * qvec[1] ** 2 - 2 * qvec[3] ** 2,
|
||||
2 * qvec[2] * qvec[3] - 2 * qvec[0] * qvec[1],
|
||||
],
|
||||
[
|
||||
2 * qvec[3] * qvec[1] - 2 * qvec[0] * qvec[2],
|
||||
2 * qvec[2] * qvec[3] + 2 * qvec[0] * qvec[1],
|
||||
1 - 2 * qvec[1] ** 2 - 2 * qvec[2] ** 2,
|
||||
],
|
||||
]
|
||||
)
|
||||
#"""
|
||||
def get_w2c(qvec: list, tvec: list) -> np.ndarray:
|
||||
'''获取世界坐标系到相机坐标系的变换矩阵
|
||||
|
||||
Args:
|
||||
qvec: 四元数
|
||||
tvec: 平移向量
|
||||
|
||||
Returns:
|
||||
世界坐标系到相机坐标系的变换矩阵
|
||||
'''
|
||||
#"""
|
||||
w2c = np.eye(4).astype(np.float64)
|
||||
w2c[:3, :3] = qvec2rotmat(qvec)
|
||||
w2c[:3, 3] = tvec
|
||||
#"""
|
||||
"""
|
||||
R = qvec2rotmat(qvec) # 相机到世界的旋转
|
||||
R_w2c = R.T # 世界到相机的旋转
|
||||
t_w2c = -R_w2c @ tvec # 平移分量
|
||||
|
||||
w2c = np.eye(4)
|
||||
w2c[:3, :3] = R_w2c
|
||||
w2c[:3, 3] = t_w2c
|
||||
#"""
|
||||
"""
|
||||
R2 = Rotation.from_quat([qvec[0], qvec[1], qvec[2], qvec[3]]).as_matrix()
|
||||
# w2c = [R | -R @ tvec]
|
||||
w2c2 = np.eye(4)
|
||||
w2c2[:3, :3] = R2
|
||||
w2c2[:3, 3] = -R2 @ tvec
|
||||
print("w2c", w2c)
|
||||
print("w2c2", w2c2)
|
||||
#"""
|
||||
"""
|
||||
w2c = np.eye(4).astype(np.float64)
|
||||
R = qvec2rotmat(qvec)
|
||||
w2c[:3, :3] = R.T
|
||||
w2c[:3, 3] = -R @ tvec
|
||||
#"""
|
||||
"""
|
||||
w2c = np.eye(4).astype(np.float64)
|
||||
R = Rotation.from_quat([qvec[1], qvec[2], qvec[3], qvec[0]]).as_matrix()
|
||||
# COLMAP的tvec是相机在世界坐标系中的位置，需取反
|
||||
t = -R.T @ tvec
|
||||
w2c = np.eye(4)
|
||||
w2c[:3, :3] = R.T # 旋转部分
|
||||
w2c[:3, 3] = t # 平移部分
|
||||
#"""
|
||||
return w2c
|
||||
|
||||
def get_c2w(qvec: list, tvec: list) -> np.ndarray:
|
||||
'''获取相机坐标系到世界坐标系的变换矩阵
|
||||
|
||||
Args:
|
||||
qvec: 四元数
|
||||
tvec: 平移向量
|
||||
|
||||
Returns:
|
||||
相机坐标系到世界坐标系的变换矩阵
|
||||
'''
|
||||
c2w = np.eye(4).astype(np.float64)
|
||||
c2w[:3, :3] = qvec2rotmat(qvec).T
|
||||
c2w[:3, 3] = -qvec2rotmat(qvec).T @ tvec
|
||||
return c2w
|
||||
@@ -0,0 +1,117 @@
|
||||
'''
|
||||
获取Mesh的深度图
|
||||
|
||||
required:
|
||||
- open3d
|
||||
- numpy
|
||||
- numba
|
||||
'''
|
||||
|
||||
import open3d as o3d
|
||||
import numpy as np
|
||||
from numba import njit
|
||||
|
||||
@njit
|
||||
def _process_all_points(Z, width, height, cx, cy, fx, fy, c2w):
|
||||
"""JIT优化的所有点处理函数"""
|
||||
# 手动创建像素网格（替代np.meshgrid）
|
||||
u = np.zeros((height, width), dtype=np.float64)
|
||||
v = np.zeros((height, width), dtype=np.float64)
|
||||
|
||||
for y in range(height):
|
||||
for x in range(width):
|
||||
u[y, x] = x
|
||||
v[y, x] = y
|
||||
|
||||
# 创建输出数组
|
||||
Z_all = np.zeros_like(Z)
|
||||
|
||||
# 手动处理有效点
|
||||
for y in range(height):
|
||||
for x in range(width):
|
||||
if Z[y, x] > 0 and Z[y, x] < 1e9 and not np.isnan(Z[y, x]):
|
||||
Z_all[y, x] = Z[y, x]
|
||||
else:
|
||||
Z_all[y, x] = -10000
|
||||
|
||||
# 计算所有点的3D坐标
|
||||
X_all = (u - cx) * Z_all / fx
|
||||
Y_all = (v - cy) * Z_all / fy
|
||||
|
||||
# 创建齐次坐标
|
||||
points_3d = np.zeros((height * width, 4))
|
||||
points_3d[:, 0] = X_all.reshape(-1)
|
||||
points_3d[:, 1] = Y_all.reshape(-1)
|
||||
points_3d[:, 2] = Z_all.reshape(-1)
|
||||
points_3d[:, 3] = 1.0
|
||||
|
||||
# 坐标变换
|
||||
return points_3d @ c2w.T
|
||||
|
||||
@njit
|
||||
def _process_valid_points(Z, width, height, cx, cy, fx, fy, c2w):
|
||||
"""JIT优化的有效点处理函数"""
|
||||
# 首先计算有效点的数量
|
||||
valid_count = 0
|
||||
for y in range(height):
|
||||
for x in range(width):
|
||||
if Z[y, x] > 0 and Z[y, x] < 1e9 and not np.isnan(Z[y, x]):
|
||||
valid_count += 1
|
||||
|
||||
# 创建输出数组
|
||||
X_valid = np.zeros(valid_count)
|
||||
Y_valid = np.zeros(valid_count)
|
||||
Z_valid = np.zeros(valid_count)
|
||||
|
||||
# 填充有效点数据
|
||||
idx = 0
|
||||
for y in range(height):
|
||||
for x in range(width):
|
||||
if Z[y, x] > 0 and Z[y, x] < 1e9 and not np.isnan(Z[y, x]):
|
||||
Z_valid[idx] = Z[y, x]
|
||||
X_valid[idx] = (x - cx) * Z_valid[idx] / fx
|
||||
Y_valid[idx] = (y - cy) * Z_valid[idx] / fy
|
||||
idx += 1
|
||||
|
||||
# 创建齐次坐标
|
||||
points_3d = np.zeros((valid_count, 4))
|
||||
points_3d[:, 0] = X_valid
|
||||
points_3d[:, 1] = Y_valid
|
||||
points_3d[:, 2] = Z_valid
|
||||
points_3d[:, 3] = 1.0
|
||||
|
||||
# 坐标变换
|
||||
return points_3d @ c2w.T
|
||||
|
||||
def get_world_points(depth_map: o3d.geometry.Image, K: np.ndarray, c2w: np.ndarray, all_point: bool = False) -> np.ndarray:
|
||||
'''
|
||||
获取深度图对应的世界坐标点云
|
||||
|
||||
param:
|
||||
depth_map: 深度图
|
||||
K: 相机内参矩阵
|
||||
c2w: 相机坐标系到世界坐标系的变换矩阵
|
||||
all_point: 是否返回所有点（包括无效点）
|
||||
|
||||
return:
|
||||
np.ndarray: 点云坐标（世界坐标系）
|
||||
- 当all_point=False时：只包含有效深度值的点云
|
||||
- 当all_point=True时：包含所有点的点云，无效点深度值设为10000
|
||||
'''
|
||||
# 转换为numpy数组
|
||||
Z = np.asarray(depth_map)
|
||||
height, width = Z.shape
|
||||
|
||||
# 获取内参
|
||||
fx, fy = K[0, 0], K[1, 1]
|
||||
cx, cy = K[0, 2], K[1, 2]
|
||||
|
||||
# 确保数据类型兼容性（Numba要求）
|
||||
Z = Z.astype(np.float64)
|
||||
c2w = c2w.astype(np.float64)
|
||||
|
||||
if all_point:
|
||||
return _process_all_points(Z, width, height, cx, cy, fx, fy, c2w)
|
||||
else:
|
||||
return _process_valid_points(Z, width, height, cx, cy, fx, fy, c2w)
|
||||
|
||||
@@ -0,0 +1,66 @@
|
||||
'''
|
||||
将点云投影到新的图像上
|
||||
|
||||
required:
|
||||
- numpy
|
||||
'''
|
||||
|
||||
import numpy as np
|
||||
|
||||
def project_to_image(world_points: np.ndarray, w2c: np.ndarray,
|
||||
K: np.ndarray, width: int, height: int) -> np.ndarray:
|
||||
'''
|
||||
将点云投影到新的图像上，生成深度图
|
||||
|
||||
Args:
|
||||
world_points: 世界坐标点云，形状为(N, 4)的齐次坐标
|
||||
w2c: 世界坐标到相机坐标变换矩阵 (4x4)
|
||||
K: 相机内参矩阵 (3x3)
|
||||
width: 图像宽度
|
||||
height: 图像高度
|
||||
|
||||
Returns:
|
||||
深度图，值为无穷大表示没有投影到该像素的点
|
||||
'''
|
||||
# 检查输入数据的维度
|
||||
if world_points.shape[1] != 4:
|
||||
raise ValueError(f"世界点云应为齐次坐标 (N,4)，但得到 {world_points.shape}")
|
||||
|
||||
# 初始化深度图
|
||||
depth_image = np.full((height, width), np.inf, dtype=np.float32)
|
||||
|
||||
# 将世界坐标系点转换到相机坐标系
|
||||
points_camera = (w2c @ world_points.T).T
|
||||
|
||||
# 筛选相机前方的点 (z > 0)
|
||||
valid_mask = points_camera[:, 2] > 0
|
||||
points_camera = points_camera[valid_mask]
|
||||
|
||||
if len(points_camera) == 0:
|
||||
return depth_image
|
||||
|
||||
# 投影到归一化图像平面 - 修复广播错误
|
||||
# 只使用深度值z进行归一化，而不是整个z之后的子数组
|
||||
z_values = points_camera[:, 2:3] # 使用 2:3 保持列向量形式
|
||||
points_normalized = points_camera[:, :3] / z_values
|
||||
|
||||
# 应用相机内参矩阵
|
||||
points_image = (K @ points_normalized[:, :3].T).T
|
||||
|
||||
# 提取像素坐标和深度值
|
||||
x_pixels = np.round(points_image[:, 0]).astype(int)
|
||||
y_pixels = np.round(points_image[:, 1]).astype(int)
|
||||
depths = points_camera[:, 2]
|
||||
|
||||
# 筛选出图像范围内的点
|
||||
inbounds = (x_pixels >= 0) & (x_pixels < width) & (y_pixels >= 0) & (y_pixels < height)
|
||||
x_pixels = x_pixels[inbounds]
|
||||
y_pixels = y_pixels[inbounds]
|
||||
depths = depths[inbounds]
|
||||
|
||||
# 使用numpy的高级索引更新深度图 (更高效的实现)
|
||||
# 为每个像素找到最小深度值
|
||||
indices = y_pixels * width + x_pixels
|
||||
np.minimum.at(depth_image.reshape(-1), indices, depths)
|
||||
|
||||
return depth_image
|
||||
Reference in new issue
Block a user