Skip to content
GitLab
Menu
Projects
Groups
Snippets
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Pinard Clement
drone-depth-validation-set
Commits
df1f2f41
Commit
df1f2f41
authored
Jan 07, 2021
by
Clément Pinard
Browse files
Final update of Bundle adjustment
parent
cff407b7
Changes
1
Hide whitespace changes
Inline
Side-by-side
bundle_adjustment.py
View file @
df1f2f41
...
...
@@ -58,6 +58,7 @@ def main(args):
print
(
cameras_df
)
ref_pointcloud
=
PyntCloud
.
from_file
(
args
.
ply
)
ref_pointcloud
=
torch
.
from_numpy
(
ref_pointcloud
.
xyz
).
to
(
device
,
dtype
=
args
.
dtype
)
points_3d
=
np
.
stack
(
points_df
[
"xyz"
].
values
)
points_3d
=
torch
.
from_numpy
(
points_3d
).
to
(
device
,
dtype
=
args
.
dtype
)
...
...
@@ -157,6 +158,7 @@ def main(args):
with
torch
.
no_grad
():
padded_points
=
list_to_padded
([
points_3d
[
visibility
[
c
]]
for
c
in
range
(
N
)],
pad_value
=
1e-3
)
points_2D_gt
=
cameras_absolute_gt
.
transform_points
(
padded_points
,
eps
=
1e-4
)[:,
:,
:
2
]
relative_points_gt
=
padded_points
@
cameras_R
+
cameras_T
# Starting point is normally points_3d and camera_R and camera_T
# For stability test, you can try to add noise and see if the otpitmization
...
...
@@ -196,7 +198,7 @@ def main(args):
# Compute inliers
# In the model, some 3d points have their reprojection way off compared to the
# target 2d point. It is potentially a great source of instability. inliers is
#
K
eeping track of those problematic points to
keep them out of the
optimization
#
k
eeping track of those problematic points to
discard them from
optimization
discard_outliers
=
True
if
discard_outliers
:
with
torch
.
no_grad
():
...
...
@@ -213,13 +215,14 @@ def main(args):
for
it
in
range
(
n_iter
):
# re-init the optimizer gradients
optimizer
.
zero_grad
()
R
=
so3_exponential_map
(
log_R
)
fxfyu0v0
=
cams_params
[
cameras_id_per_image
]
# get the current absolute cameras
cameras_absolute
=
PerspectiveCameras
(
focal_length
=
fxfyu0v0
[:,
:
2
],
principal_point
=
fxfyu0v0
[:,
2
:],
R
=
so3_exponential_map
(
log_R
)
,
R
=
R
,
T
=
T
,
device
=
device
,
)
...
...
@@ -231,14 +234,21 @@ def main(args):
# This is problematic as close points are the ones with which we want the pose modification to be low
# but gradient descent makes them with the highest step size. We can maybe use Adam, but unstability remains.
#
# 2) minimize 3d relative position error. No more unstability for very close points.
# 2d reprojection error is not guaranteed to be minimized
# 2) minimize 3d relative position error
(initial 3d relative position is considered groundtruth)
. No more unstability for very close points.
# 2d reprojection error is not guaranteed to be minimized
though
minimize_2d
=
True
chamfer_weight
=
1e3
verbose
=
True
chamfer_dist
=
chamfer_distance
(
ref_pointcloud
[
None
],
points
[
None
])[
0
]
if
minimize_2d
:
projected_points_3D
=
cameras_absolute
.
transform_points
(
padded_points
,
eps
=
1e-4
)
projected_points_3D
=
cameras_absolute
.
transform_points
(
padded_points
,
eps
=
1e-4
)
[...,
:
2
]
projected_points
=
projected_points_3D
[:,
:,
:
2
]
# Discard points with a depth < 0 (theoretically impossible)
inliers
=
inliers
&
(
projected_points_3D
[:,
:,
2
][
nonzer
]
>
0
)
# Plot point distants for first image
# distances = (projected_points[0] - points_2D_gt[0]).norm(dim=-1).detach().cpu().numpy()
# from matplotlib import pyplot as plt
# plt.plot(distances[:(visibility[0]).sum()])
...
...
@@ -246,29 +256,51 @@ def main(args):
# Different loss functions for reprojection error minimization
# points_distance = smooth_l1_loss(projected_points, points_2D_gt)
# points_distance = (smooth_l1_loss(projected_points, points_2D_gt, reduction='none')[nonzer]).sum(dim=1)
points_distance
=
((
projected_points
[
nonzer
]
-
points_2D_gt
[
nonzer
])
**
2
).
sum
(
dim
=
1
)
points_distance_filtered
=
points_distance
[
inliers
]
# 100*((points_gt - points_absolute) ** 2).sum() #
loss
=
points_distance_filtered
.
mean
()
+
10000
*
chamfer_distance
(
points_gt
[
None
],
points_absolute
[
None
])[
0
]
# our loss function is the camera_distance
proj_error
=
((
projected_points
[
nonzer
]
-
points_2D_gt
[
nonzer
])
**
2
).
sum
(
dim
=
1
)
proj_error_filtered
=
proj_error
[
inliers
]
else
:
projected_points_3D
=
padded_points
@
R
+
T
# Plot point distants for first image
# distances = (projected_points_3D[0] - relative_points_gt[0]).norm(dim=-1).detach().cpu().numpy()
# from matplotlib import pyplot as plt
# plt.plot(distances[:(visibility[0]).sum()])
# Different loss functions for reprojection error minimization
# points_distance = smooth_l1_loss(projected_points, points_2D_gt)
# points_distance = (smooth_l1_loss(projected_points, points_2D_gt, reduction='none')[nonzer]).sum(dim=1)
proj_error
=
((
projected_points_3D
[
nonzer
]
-
relative_points_gt
[
nonzer
])
**
2
).
sum
(
dim
=
1
)
proj_error_filtered
=
proj_error
[
inliers
]
loss
=
proj_error_filtered
.
mean
()
+
chamfer_weight
*
chamfer_dist
loss
.
backward
()
# print("faulty elements :")
# faulty_points = torch.arange(points_absolute.shape[0])[points_absolute.grad[:, 0] != points_absolute.grad[:, 0]]
# # faulty_images = torch.arange(log_R_absolute.shape[0])[log_R_absolute.grad[:, 0] != log_R_absolute.grad[:, 0]]
# faulty_cams = torch.arange(cams_params.shape[0])[cams_params.grad[:, 0] != cams_params.grad[:, 0]]
# print(torch.isnan(projected_points.grad).any(dim=2))
# print(projected_points.grad.shape)
# faulty_projected_points = torch.arange(projected_points.shape[1])[torch.isnan(projected_points.grad).any(dim=2)[0]]
# # print(log_R_absolute[faulty_images])
# # print(T_absolute[faulty_images])
# print(points_gt[faulty_points])
# print(cams_params[faulty_cams])
# print(projected_points[torch.isnan(projected_points.grad)])
# first_faulty_point = points_df.iloc[int(faulty_points[0])]
# faulty_images = images_df.loc[first_faulty_point["image_ids"][0]]
# print(first_faulty_point)
# print(faulty_images)
if
verbose
:
print
(
"faulty elements (with nan grad) :"
)
faulty_points
=
torch
.
arange
(
points
.
shape
[
0
])[
points
.
grad
[:,
0
]
!=
points
.
grad
[:,
0
]]
faulty_images
=
torch
.
arange
(
log_R
.
shape
[
0
])[
log_R
.
grad
[:,
0
]
!=
log_R
.
grad
[:,
0
]]
faulty_cams
=
torch
.
arange
(
cams_params
.
shape
[
0
])[
cams_params
.
grad
[:,
0
]
!=
cams_params
.
grad
[:,
0
]]
faulty_projected_points
=
torch
.
arange
(
projected_points
.
shape
[
1
])[
torch
.
isnan
(
projected_points
.
grad
).
any
(
dim
=
2
)[
0
]]
# Print Tensor that would become NaN, should the gradient be applied
print
(
"Faulty Rotation (log) and translation"
)
print
(
faulty_images
)
print
(
log_R
[
faulty_images
])
print
(
T
[
faulty_images
])
print
(
"Faulty 3D colmap points"
)
print
(
faulty_points
)
print
(
points
[
faulty_points
])
print
(
"Faulty Cameras"
)
print
(
faulty_cams
)
print
(
cams_params
[
faulty_cams
])
print
(
"Faulty 2D points"
)
print
(
projected_points
[
faulty_projected_points
])
first_faulty_point
=
points_df
.
iloc
[
int
(
faulty_points
[
0
])]
related_faulty_images
=
images_df
.
loc
[
first_faulty_point
[
"image_ids"
][
0
]]
print
(
"First faulty point, and images where it is seen"
)
print
(
first_faulty_point
)
print
(
related_faulty_images
)
# apply the gradients
optimizer
.
step
()
...
...
@@ -276,25 +308,27 @@ def main(args):
# plot and print status message
if
it
%
2000
==
0
or
it
==
n_iter
-
1
:
camera_distance
=
calc_camera_distance
(
cameras_absolute
,
cameras_absolute_gt
)
points_3d_distance
=
(
points_gt
-
points_absolute
).
norm
(
dim
=-
1
).
mean
()
print
(
'iteration=%3d; loss=%1.3e, points_distance=%1.3e, camera_distance=%1.3e'
%
(
it
,
loss
,
points_3d_distance
,
camera_distance
))
print
(
'iteration = {}; loss = {}, chamfer distance = {}, camera_distance = {}'
.
format
(
it
,
loss
,
chamfer_distance
,
camera_distance
))
loss_log
.
append
(
loss
.
item
())
pts_dist_log
.
append
(
points_3d
_distance
.
item
())
pts_dist_log
.
append
(
chamfer
_distance
.
item
())
cam_dist_log
.
append
(
camera_distance
.
item
())
if
it
%
20000
==
0
or
it
==
n_iter
-
1
:
if
it
%
20000
==
0
or
it
==
n_iter
-
1
:
with
torch
.
no_grad
():
from
matplotlib
import
pyplot
as
plt
plt
.
hist
(
torch
.
sqrt
(
p
oints_distance
_filtered
).
detach
().
cpu
().
numpy
())
plt
.
hist
(
torch
.
sqrt
(
p
roj_error
_filtered
).
detach
().
cpu
().
numpy
())
if
it
%
200000
==
0
or
it
==
n_iter
-
1
:
plt
.
figure
()
plt
.
plot
(
loss_log
)
plt
.
figure
()
plt
.
plot
(
pts_dist_log
,
label
=
"
pts
_dist"
)
plt
.
plot
(
pts_dist_log
,
label
=
"
chamfer
_dist"
)
plt
.
plot
(
cam_dist_log
,
label
=
"cam_dist"
)
plt
.
legend
()
plot_camera_scene
(
cameras_absolute
,
cameras_absolute_gt
,
points
_absolute
,
points_gt
,
'iteration=
%3d; points_distance=%1.3e'
%
(
it
,
points_3d
_distance
))
points
,
ref_pointcloud
,
'iteration=
{}; chamfer distance={}'
.
format
(
it
,
chamfer
_distance
))
print
(
'Optimization finished.'
)
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment