ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

View offset improvements: Automatically compute it for WMR and make it 2D 

- Make view_offset two-dimensional
- Compute it automatically from WMR extrinsics
This commit is contained in:
Mateo de Mayo 2022-05-11 19:56:13 +00:00
parent b379a7c924
commit 594603861b
17 changed files with 131 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
data/d455_640x480_calib.json
View File

@ -97,7 +97,7 @@
0.0007071067802939111
],
"cam_time_offset_ns": 0,
"view_offset": 0,
"view_offset": [0, 0],
"vignette": []
}
}

2
data/d455_848x480_calib.json
View File

@ -97,7 +97,7 @@
0.0007071067802939111
],
"cam_time_offset_ns": 0,
"view_offset": 0,
"view_offset": [0, 0],
"vignette": []
}
}

2
data/d455_calib.json
View File

@ -99,7 +99,7 @@
0.0007071067802939111
],
"cam_time_offset_ns": 0,
"view_offset": 0,
"view_offset": [0, 0],
"vignette": []
}
}

2
data/euroc_ds_calib.json
View File

@ -225,6 +225,6 @@
"mocap_time_offset_ns": 0,
"mocap_to_imu_offset_ns": 140763258159875,
"cam_time_offset_ns": 0,
"view_offset": 0
"view_offset": [0, 0]
}
}

2
data/euroc_eucm_calib.json
View File

@ -209,6 +209,6 @@
"mocap_time_offset_ns": 0,
"mocap_to_imu_offset_ns": 140763258159875,
"cam_time_offset_ns": 0,
"view_offset": 0
"view_offset": [0, 0]
}
}

2
data/euroc_rt8_calib.json
View File

@ -239,6 +239,6 @@
"mocap_time_offset_ns": 0,
"mocap_to_imu_offset_ns": 140763258159875,
"cam_time_offset_ns": 0,
"view_offset": 0
"view_offset": [0, 0]
}
}

68
data/monado/wmr-tools/wmr2bslt_calib.py
View File

@ -37,6 +37,37 @@ def rmat2quat(r):
return np.array([x, y, z, w])
def project(intrinsics, x, y, z):
fx, fy, cx, cy, k1, k2, k3, k4, k5, k6, p1, p2 = (
intrinsics["fx"],
intrinsics["fy"],
intrinsics["cx"],
intrinsics["cy"],
intrinsics["k1"],
intrinsics["k2"],
intrinsics["k3"],
intrinsics["k4"],
intrinsics["k5"],
intrinsics["k6"],
intrinsics["p1"],
intrinsics["p2"],
)
xp = x / z
yp = y / z
r2 = xp * xp + yp * yp
cdist = (1 + r2 * (k1 + r2 * (k2 + r2 * k3))) / (
1 + r2 * (k4 + r2 * (k5 + r2 * k6))
)
deltaX = 2 * p1 * xp * yp + p2 * (r2 + 2 * xp * xp)
deltaY = 2 * p2 * xp * yp + p1 * (r2 + 2 * yp * yp)
xpp = xp * cdist + deltaX
ypp = yp * cdist + deltaY
u = fx * xpp + cx
v = fy * ypp + cy
return u, v
def extrinsics(j, cam):
# NOTE: The `Rt` field seems to be a transform from the sensor to HT0 (i.e.,
# from HT0 space to sensor space). For basalt we need the transforms
@ -106,11 +137,37 @@ def intrinsics(j, cam):
"k4": model_params[7],
"k5": model_params[8],
"k6": model_params[9],
"rpmax": model_params[14]
"rpmax": model_params[14],
},
}
def view_offset(j):
"""
This is a very rough offset in pixels between the two cameras. Originally we
needed to manually estimate it like explained and shown here
https://youtu.be/jyQKjyRVMS4?t=670.
With this calculation we get a similar number without the need to open Gimp.
In reality this offset changes based on distance to the point, nonetheless
it helps to get some features tracked in the right camera.
"""
# Rough approximation of how far from the cameras features will likely be in your room
DISTANCE_TO_WALL = 2 # In meters
cam1 = get(j, "HT1")
width = cam1["SensorWidth"]
height = cam1["SensorHeight"]
cam1_intrinsics = intrinsics(j, "HT1")["intrinsics"]
T_c1_c0 = rt2mat(cam1["Rt"]) # Maps a point in c0 space to c1 space
p = np.array([0, 0, DISTANCE_TO_WALL, 1]) # Fron tof c0, in homogeneous coords
p_in_c1 = T_c1_c0 @ p # Point in c1 coordinates
u, v = project(cam1_intrinsics, *p_in_c1[0:3])
view_offset = [width / 2 - u, height / 2 - v] # We used a point in the middle of c0
return view_offset
def calib_accel_bias(j):
# https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/2feb3425259bf803749065bb6d628c6c180f8e77/include/k4ainternal/calibration.h#L48-L77
# https://vladyslavusenko.gitlab.io/basalt-headers/classbasalt_1_1CalibAccelBias.html#details
@ -176,13 +233,6 @@ def main():
# But in monado we just average those 4 samples to reduce the noise. So we have 250hz.
IMU_UPDATE_RATE = 250
# This is a very rough offset in pixels between the two cameras. I manually
# measured it for some particular point in some particular pair of images
# for my Odyssey+. In reality this offset changes based on distance to the
# point, nonetheless it helps to get some features tracked in the right
# camera.
VIEW_OFFSET = 247
out_calib = {
"value0": {
"T_imu_cam": [extrinsics(j, "HT0"), extrinsics(j, "HT1")],
@ -196,7 +246,7 @@ def main():
"accel_bias_std": bias_std(j, "Accelerometer"),
"gyro_bias_std": bias_std(j, "Gyro"),
"cam_time_offset_ns": 0,
"view_offset": VIEW_OFFSET,
"view_offset": view_offset(j),
"vignette": [],
}
}

5
data/odysseyplus_kb4_calib.json
View File

@ -108,7 +108,10 @@
0.009999999873689375
],
"cam_time_offset_ns": 0,
"view_offset": 247,
"view_offset": [
248.7971689190779,
-5.785840906463989
],
"vignette": []
}
}

76
data/odysseyplus_rt8_calib.json
View File

@ -5,19 +5,19 @@
"px": -0.08394275605678558,
"py": -0.0025952591095119715,
"pz": 0.0026445253752171993,
"qx": -0.17704728245735168,
"qy": -0.19861078262329102,
"qz": 0.035641565918922424,
"qw": 0.9632952809333801
"qx": -0.17704726835670878,
"qy": -0.19861076922141732,
"qz": 0.03564156642768224,
"qw": 0.9632952306248018
},
{
"px": 0.021131351590156555,
"py": -0.002306802896782756,
"pz": 0.0023415968753397465,
"qx": -0.18363775312900543,
"qy": 0.19827023148536682,
"qz": -0.03922446444630623,
"qw": 0.961991548538208
"px": 0.02113135496007891,
"py": -0.0023068031820524327,
"pz": 0.002341595642298417,
"qx": -0.1836377420226455,
"qy": 0.1982701960061761,
"qz": -0.03922445856481098,
"qw": 0.9619914477874183
}
],
"intrinsics": [
@ -29,14 +29,14 @@
"cx": 324.3333053588867,
"cy": 245.22674560546875,
"k1": 0.6257319450378418,
"k2": 0.46612036228179932,
"k2": 0.4661203622817993,
"p1": -0.00018502399325370789,
"p2": -4.2882973502855748e-5,
"k3": 0.0041795829311013222,
"k4": 0.89431935548782349,
"k5": 0.54253977537155151,
"p2": -4.288297350285575e-05,
"k3": 0.004179582931101322,
"k4": 0.8943193554878235,
"k5": 0.5425397753715515,
"k6": 0.06621214747428894,
"rpmax": 2.7941114902496338
"rpmax": 2.794111490249634
}
},
{
@ -46,15 +46,15 @@
"fy": 270.2496528625488,
"cx": 323.54679107666016,
"cy": 250.34966468811035,
"k1": 0.55718272924423218,
"k1": 0.5571827292442322,
"k2": 0.22437196969985962,
"p1": 0.00011782468209275976,
"p2": 0.0001220563062815927,
"k3": 0.0068156048655509949,
"k4": 0.83317267894744873,
"k5": 0.26174271106719971,
"k6": 0.043505862355232239,
"rpmax": 2.7899987697601318
"k3": 0.006815604865550995,
"k4": 0.8331726789474487,
"k5": 0.2617427110671997,
"k6": 0.04350586235523224,
"rpmax": 2.789998769760132
}
}
],
@ -68,9 +68,6 @@
480
]
],
"// about calib_accel/gyro_bias": "Bias values (first three elements) are negated, in basalt they get substracted, but in azure-kinect they are added",
"// - ": "Also the mixing matrix has I3x3 substracted, i.e. diagonal are one unit smaller",
"// about calib_accel_bias": "See https://gitlab.com/VladyslavUsenko/basalt-headers/-/issues/8 to understand why accel only uses 6 elements of the mixing matrix",
"calib_accel_bias": [
0.18563582003116608,
-0.057621683925390244,
@ -78,34 +75,34 @@
-0.001865983009338379,
0.00030630044057033956,
-0.0001675997773418203,
0.0009244680404663,
0.0009244680404663086,
0.000295753387035802,
-0.0022075772285461426
],
"calib_gyro_bias": [
-0.016492774710059166,
0.01642640121281147,
0.0045625129714608192,
0.0000957250595093,
-0.00082384591223672032,
0.004562512971460819,
9.572505950927734e-05,
-0.0008238459122367203,
-0.0015064212493598461,
-0.00082343036774545908,
-0.0008234303677454591,
-0.00041097402572631836,
-4.7357993935293052e-6,
-4.735799393529305e-06,
-0.0015050634974613786,
-4.7339185584860388e-6,
-4.733918558486039e-06,
-0.0008063316345214844
],
"imu_update_rate": 250.0,
"imu_update_rate": 250,
"accel_noise_std": [
0.010700000450015068,
0.010700000450015068,
0.010700000450015068
],
"gyro_noise_std": [
0.00095000001601874828,
0.00095000001601874828,
0.00095000001601874828
0.0009500000160187483,
0.0009500000160187483,
0.0009500000160187483
],
"accel_bias_std": [
0.0999999988824129,
@ -118,7 +115,10 @@
0.009999999873689375
],
"cam_time_offset_ns": 0,
"view_offset": 247,
"view_offset": [
248.7971689190779,
-5.785840906463989
],
"vignette": []
}
}

2
data/reverbg1_calib.json
View File

@ -115,7 +115,7 @@
0.009999999873689375
],
"cam_time_offset_ns": 0,
"view_offset": 247,
"view_offset": [247, 0],
"vignette": []
}
}

2
data/tumvi_512_ds_calib.json
View File

@ -105,7 +105,7 @@
"mocap_time_offset_ns": 0,
"mocap_to_imu_offset_ns": 0,
"cam_time_offset_ns": 0,
"view_offset": 0,
"view_offset": [0, 0],
"vignette": [
{
"value0": 0,

2
data/tumvi_512_eucm_calib.json
View File

@ -105,7 +105,7 @@
"mocap_time_offset_ns": 0,
"mocap_to_imu_offset_ns": 0,
"cam_time_offset_ns": 0,
"view_offset": 0,
"view_offset": [0, 0],
"vignette": [
{
"value0": 0,

2
data/x3pro_calib.json
View File

@ -97,7 +97,7 @@
0.0001
],
"cam_time_offset_ns": 0,
"view_offset": 300,
"view_offset": [0, 0],
"vignette": []
}
}

10
include/basalt/optical_flow/frame_to_frame_optical_flow.h
View File

@ -186,7 +186,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
transform_map_1,
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>& transform_map_2,
int64_t view_offset = 0) const {
Vector2 view_offset = Vector2::Zero()) const {
size_t num_points = transform_map_1.size();
std::vector<KeypointId> ids;
@ -210,17 +210,19 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
const Eigen::AffineCompact2f& transform_1 = init_vec[r];
Eigen::AffineCompact2f transform_2 = transform_1;
transform_2.translation()(0) -= view_offset;
transform_2.translation() -= view_offset;
bool valid = transform_2.translation()(0) >= 0 &&
transform_2.translation()(0) < pyr_2.lvl(0).w;
transform_2.translation()(1) &&
transform_2.translation()(0) < pyr_2.lvl(0).w &&
transform_2.translation()(1) < pyr_2.lvl(0).h;
if (!valid) continue;
valid = trackPoint(pyr_1, pyr_2, transform_1, transform_2);
if (!valid) continue;
Eigen::AffineCompact2f transform_1_recovered = transform_2;
transform_1_recovered.translation()(0) += view_offset;
transform_1_recovered.translation() += view_offset;
valid = trackPoint(pyr_2, pyr_1, transform_2, transform_1_recovered);
if (!valid) continue;

16
include/basalt/optical_flow/multiscale_frame_to_frame_optical_flow.h
View File

@ -200,7 +200,8 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase {
transform_map_1,
const std::map<KeypointId, size_t>& pyramid_levels_1,
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>& transform_map_2,
std::map<KeypointId, size_t>& pyramid_levels_2, int64_t view_offset = 0) const {
std::map<KeypointId, size_t>& pyramid_levels_2,
Vector2 view_offset = Vector2::Zero()) const {
size_t num_points = transform_map_1.size();
std::vector<KeypointId> ids;
@ -229,17 +230,20 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase {
const Eigen::AffineCompact2f& transform_1 = init_vec[r];
Eigen::AffineCompact2f transform_2 = transform_1;
transform_2.translation()(0) -= view_offset;
transform_2.translation() -= view_offset;
bool valid = transform_2.translation()(0) >= 0 &&
transform_2.translation()(0) < pyr_2.lvl(pyramid_level[r]).w;
bool valid =
transform_2.translation()(0) >= 0 &&
transform_2.translation()(1) >= 0 &&
transform_2.translation()(0) < pyr_2.lvl(pyramid_level[r]).w &&
transform_2.translation()(1) < pyr_2.lvl(pyramid_level[r]).h;
if (!valid) continue;
valid = trackPoint(pyr_1, pyr_2, transform_1, pyramid_level[r],
transform_2);
transform_2);
if (valid) {
Eigen::AffineCompact2f transform_1_recovered = transform_2;
transform_1_recovered.translation()(0) += view_offset;
transform_1_recovered.translation() += view_offset;
valid = trackPoint(pyr_2, pyr_1, transform_2, pyramid_level[r],
transform_1_recovered);

6
scripts/basalt_convert_kitti_calib.py
View File

@ -112,7 +112,7 @@ calib_template = Template('''{
"accel_bias_std": [0.0, 0.0, 0.0],
"gyro_bias_std": [0.0, 0.0, 0.0],
"cam_time_offset_ns": 0,
"view_offset": 0
"view_offset": [0.0, 0.0]
}
}
''')
@ -124,7 +124,7 @@ with open(kitti_calib_file, 'r') as stream:
if len(lines) != 52:
print('Issues loading calibration')
print(lines)
P0 = np.array([float(x) for x in lines[1:13]]).reshape(3,4)
P1 = np.array([float(x) for x in lines[14:26]]).reshape(3,4)
print('P0\n', P0)
@ -142,4 +142,4 @@ with open(kitti_calib_file, 'r') as stream:
print(calib)
with open(dataset_path + '/basalt_calib.json', 'w') as stream2:
stream2.write(calib)
stream2.write(calib)

2
thirdparty/basalt-headers vendored

@ -1 +1 @@
Subproject commit 1473e8a417313082372bd0356c732cfcd60e183e
Subproject commit 1a92e765bd65037313bb482c56886842c4d3d547