isl-org / Open3D

Open3D: A Modern Library for 3D Data Processing
http://www.open3d.org
Other
11.57k stars 2.32k forks source link

How to scale point clouds according to checker or charuco boards #4924

Closed Rheinwalt closed 2 years ago

Rheinwalt commented 2 years ago

Checklist

My Question

Dear all,

a seemingly simple question for which I can't find a solution so I'm probably searching the wrong way. We have multiple SfM derived point clouds that include well reconstructed checker and charuco boards and we would like to scale all clouds to the real scale.

I guess I could click the distance between two diagonal corners of the board and translate that into a scale, but I was hoping for something less manual.

Best, A

yuecideng commented 2 years ago

Interesting problem, maybe you can find a way to project your reconstructed pointcloud into RGBD image, and you can detect the key points of charuco boards in RGB image. Then you can easily obtain the correspoding depth value of the keypoints so that you can measure the L2 distance of them. The first thing is to create a fake camera intrisic that can be used to project and reproject points from 2d to 3d.

Rheinwalt commented 2 years ago

I like the idea, but could not find a way to get the point cloud as an RGBD image. Usually it's used the other way around. So I basically did a poor-mans rendering myself in order to simplify that idea further. My idea was to have the x and y coordinates rounded to integer image coordinates and re-interprete the z coordinates as a depth field. A simple projection along the z-axis ignoring occlusions that I don't really have anyways. I did this in the following way, but maybe there is a simpler/faster way of doing this within open3d? After all the rendering of open3d is pretty efficient and should be accessible somehow, right?

import numpy as np
from scipy.interpolate import griddata
import open3d as o3d
import cv2 as cv
from matplotlib import pyplot as pl

pc = o3d.io.read_point_cloud('point_cloud.ply')
o3d.visualization.draw_geometries([pc])

get points and rgb colors

pt = np.asarray(pc.points)
cl = np.asarray(pc.colors)

convert to grayscale cl = 0.2989 * cl[:,0] + 0.5870 * cl[:,1] + 0.1140 * cl[:,2]

image coordinates x, y

pt -= np.min(pt, axis = 0)
x = (1000 * pt[:,0]).astype('int')
y = (1000 * pt[:,1]).astype('int')

depth z and color c

c = np.ones(z.shape) * np.nan
for i in range(x.shape[0]):
    xi = x[i]
    yi = y[i]
    if z[yi, xi] < pt[i, 2]:
        z[yi, xi] = pt[i, 2]
        c[yi, xi] = cl[i]

lots of NaNs

fg, ax = pl.subplots(1, 2, sharex = True, sharey = True)
ax[0].imshow(z, origin = 'lower')
ax[1].imshow(c, origin = 'lower')
pl.show()

simple interpolation in 2d

sel = ~np.isnan(c)
xg, yg = np.meshgrid(np.arange(c.shape[1]), np.arange(c.shape[0]))
print(sel.shape, xg.shape)
cg = griddata((xg[sel], yg[sel]), c[sel], (xg, yg), method = 'nearest')
zg = griddata((xg[sel], yg[sel]), z[sel], (xg, yg), method = 'linear')

enhance contrast because opencv cant find my markers

cg -= np.percentile(cg, 10)
cg[cg < 0] = 0
cg /= np.percentile(cg, 90)
cg[cg > 1] = 1

check the filled images

fg, ax = pl.subplots(2, 2, sharex = True, sharey = True)
ax[0,0].imshow(c, origin = 'lower')
ax[0,1].imshow(cg, origin = 'lower')
ax[1,0].imshow(z, origin = 'lower')
ax[1,1].imshow(zg, origin = 'lower')
pl.show()

opencv step which fails to find the markers

im = (255 * cg).astype(np.uint8)
ret, corners = cv.findChessboardCorners(im, (28, 17), None)
print(ret)
print(corners)
yuecideng commented 2 years ago

I think Open3D has no rendering method like this, may be you can try to find that..

As for markers detection,I think chessboard is not suitable here, due to the detection of corners are strongly rely on the contrast of pixel. Maybe you can try CircleGrid or something depends on the fitting of boundary.

Rheinwalt commented 2 years ago

This became more an OpenCV issue than an Open3D one, but I made it work so let me close this issue with a working solution. All I had to do is increase the resolution of and apply a Gaussian filter to the fake image for OpenCV to detect the chessboard corners. This is easy, one just has to change the image_scale parameter that defines the pixel per meter density:

import numpy as np
from scipy.spatial import cKDTree as kdtree
from scipy.ndimage import gaussian_filter
import open3d as o3d
import cv2 as cv

# load point cloud
pc = o3d.io.read_point_cloud('point_cloud.ply')
o3d.visualization.draw_geometries([pc])
pt = np.asarray(pc.points)
cl = np.asarray(pc.colors)

# we just need gray scale
cl = 0.2989 * cl[:,0] + 0.5870 * cl[:,1] + 0.1140 * cl[:,2]

# it's more efficient to focus on the region of the chessboard
sel = (-0.55 < pt[:,2]) * (0.25 < pt[:,1]) * (pt[:,1] < 0.55) * (-0.15 < pt[:,0]) * (pt[:,0] < 0.2)
pt, cl = pt[sel], cl[sel]

# image scale defines the pixel density (have it large)
image_scale = 10000.0
pt -= np.min(pt, axis = 0)
x = (image_scale * pt[:,0]).astype('int')
y = (image_scale * pt[:,1]).astype('int')
z = np.zeros((y.max()+1, x.max()+1))
c = np.ones(z.shape) * np.nan
for i in range(x.shape[0]):
    xi = x[i]
    yi = y[i]
    if z[yi, xi] < pt[i, 2]:
        z[yi, xi] = pt[i, 2]
        c[yi, xi] = cl[i]

# kd-tree based inverse distance interpolation to fill empty pixel
sel = ~np.isnan(c)
xg, yg = np.meshgrid(np.arange(c.shape[1]), np.arange(c.shape[0]))
tr = kdtree(np.c_[xg[sel], yg[sel]])
dd, ii = tr.query(np.c_[xg[~sel], yg[~sel]], k = 50)
ww = 1/dd
cg = c.copy()
cg[~sel] = np.sum(ww * c[sel][ii], axis = 1) / np.sum(ww, axis = 1)

# enhance contrast
cg -= np.percentile(cg, 10)
cg[cg < 0] = 0
cg /= np.percentile(cg, 90)
cg[cg > 1] = 1

# smooth out for opencv
cg = gaussian_filter(cg, sigma = 1)

# convert for opencv
im = (255 * cg).astype(np.uint8)

# chessboard setup
CHESSBOARD = (28, 17)
ret, corners = cv.findChessboardCorners(im, CHESSBOARD,
cv.CALIB_CB_ADAPTIVE_THRESH + cv.CALIB_CB_FAST_CHECK +
cv.CALIB_CB_NORMALIZE_IMAGE + cv.CALIB_CB_FILTER_QUADS)
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
corners2 = cv.cornerSubPix(im, corners, (11,11), (-1,-1), criteria)
corners2.shape = (corners2.shape[0], corners2.shape[2])

# kd-tree based inverse distance interpolation for the
# height of the corner coordinates
sel = z > 0
tr = kdtree(np.c_[xg[sel], yg[sel]])
dd, ii = tr.query(corners2, k = 50)
ww = 1/dd
zc = np.sum(ww * z[sel][ii], axis = 1) / np.sum(ww, axis = 1)

# get the 3D coordinates of corners
pt = np.c_[corners2/image_scale, zc]

That's all I need. From these coordinates I can get a distribution of 3D distances and therefore scales.