Add view_offset setting for cameras with low overlap

This commit is contained in:
Mateo de Mayo 2022-05-06 15:21:29 -03:00
parent 74ed0f5061
commit 8e4b5d8cc4
7 changed files with 36 additions and 23 deletions

View File

@ -224,6 +224,7 @@
}, },
"mocap_time_offset_ns": 0, "mocap_time_offset_ns": 0,
"mocap_to_imu_offset_ns": 140763258159875, "mocap_to_imu_offset_ns": 140763258159875,
"cam_time_offset_ns": 0 "cam_time_offset_ns": 0,
"view_offset": 0
} }
} }

View File

@ -208,6 +208,7 @@
}, },
"mocap_time_offset_ns": 0, "mocap_time_offset_ns": 0,
"mocap_to_imu_offset_ns": 140763258159875, "mocap_to_imu_offset_ns": 140763258159875,
"cam_time_offset_ns": 0 "cam_time_offset_ns": 0,
"view_offset": 0
} }
} }

View File

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

View File

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

View File

@ -103,6 +103,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
if (output_queue) output_queue->push(nullptr); if (output_queue) output_queue->push(nullptr);
break; break;
} }
input_ptr->addTime("opticalflow_received");
processFrame(input_ptr->t_ns, input_ptr); processFrame(input_ptr->t_ns, input_ptr);
} }
@ -172,18 +173,20 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
} }
if (output_queue && frame_counter % config.optical_flow_skip_frames == 0) { if (output_queue && frame_counter % config.optical_flow_skip_frames == 0) {
transforms->input_images->addTime("opticalflow_produced");
output_queue->push(transforms); output_queue->push(transforms);
} }
frame_counter++; frame_counter++;
} }
void trackPoints(const basalt::ManagedImagePyr<uint16_t>& pyr_1, void trackPoints(
const basalt::ManagedImagePyr<uint16_t>& pyr_1,
const basalt::ManagedImagePyr<uint16_t>& pyr_2, const basalt::ManagedImagePyr<uint16_t>& pyr_2,
const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>& const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
transform_map_1, transform_map_1,
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>& Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>& transform_map_2,
transform_map_2) const { int64_t view_offset = 0) const {
size_t num_points = transform_map_1.size(); size_t num_points = transform_map_1.size();
std::vector<KeypointId> ids; std::vector<KeypointId> ids;
@ -207,25 +210,29 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
const Eigen::AffineCompact2f& transform_1 = init_vec[r]; const Eigen::AffineCompact2f& transform_1 = init_vec[r];
Eigen::AffineCompact2f transform_2 = transform_1; Eigen::AffineCompact2f transform_2 = transform_1;
transform_2.translation()(0) -= view_offset;
bool valid = trackPoint(pyr_1, pyr_2, transform_1, transform_2); bool valid = transform_2.translation()(0) >= 0 &&
transform_2.translation()(0) < pyr_2.lvl(0).w;
if (!valid) continue;
valid = trackPoint(pyr_1, pyr_2, transform_1, transform_2);
if (!valid) continue;
if (valid) {
Eigen::AffineCompact2f transform_1_recovered = transform_2; Eigen::AffineCompact2f transform_1_recovered = transform_2;
transform_1_recovered.translation()(0) += view_offset;
valid = trackPoint(pyr_2, pyr_1, transform_2, transform_1_recovered); valid = trackPoint(pyr_2, pyr_1, transform_2, transform_1_recovered);
if (!valid) continue;
if (valid) { Scalar dist2 =
Scalar dist2 = (transform_1.translation() - (transform_1.translation() - transform_1_recovered.translation())
transform_1_recovered.translation())
.squaredNorm(); .squaredNorm();
if (dist2 < config.optical_flow_max_recovered_dist2) { if (dist2 < config.optical_flow_max_recovered_dist2) {
result[id] = transform_2; result[id] = transform_2;
} }
} }
}
}
}; };
tbb::blocked_range<size_t> range(0, num_points); tbb::blocked_range<size_t> range(0, num_points);
@ -332,7 +339,8 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
} }
if (calib.intrinsics.size() > 1) { if (calib.intrinsics.size() > 1) {
trackPoints(pyramid->at(0), pyramid->at(1), new_poses0, new_poses1); trackPoints(pyramid->at(0), pyramid->at(1), new_poses0, new_poses1,
calib.view_offset);
for (const auto& kv : new_poses1) { for (const auto& kv : new_poses1) {
transforms->observations.at(1).emplace(kv); transforms->observations.at(1).emplace(kv);

View File

@ -111,7 +111,8 @@ calib_template = Template('''{
"gyro_noise_std": [0.0, 0.0, 0.0], "gyro_noise_std": [0.0, 0.0, 0.0],
"accel_bias_std": [0.0, 0.0, 0.0], "accel_bias_std": [0.0, 0.0, 0.0],
"gyro_bias_std": [0.0, 0.0, 0.0], "gyro_bias_std": [0.0, 0.0, 0.0],
"cam_time_offset_ns": 0 "cam_time_offset_ns": 0,
"view_offset": 0
} }
} }
''') ''')

@ -1 +1 @@
Subproject commit 42b623e812c12422e7522a652bc0a2e3ee6924d5 Subproject commit 8c39d10b3d5463bf257701a32293caee088b8836