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

Fixes for datasets with many skipped frames

This commit is contained in:
Vladyslav Usenko 2019-05-10 16:18:26 +02:00
parent 9a9f707bbd
commit b0c523e12b
4 changed files with 58 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/basalt/calibration/vignette.h
View File

@ -51,6 +51,7 @@ class VignetteEstimator {
VignetteEstimator(const VioDatasetPtr &vio_dataset,
const Eigen::vector<Eigen::Vector2d> &optical_centers,
const Eigen::vector<Eigen::Vector2i> &resolutions,
const std::map<TimeCamId, Eigen::vector<Eigen::Vector3d>>
&reprojected_vignette,
const AprilGrid &april_grid);
@ -77,6 +78,7 @@ class VignetteEstimator {
private:
const VioDatasetPtr vio_dataset;
Eigen::vector<Eigen::Vector2d> optical_centers;
Eigen::vector<Eigen::Vector2i> resolutions;
std::map<TimeCamId, Eigen::vector<Eigen::Vector3d>> reprojected_vignette;
const AprilGrid &april_grid;

44
src/calibration/cam_calib.cpp
View File

@ -172,7 +172,8 @@ void CamCalib::computeVign() {
}
}
VignetteEstimator ve(vio_dataset, optical_centers, reprojected_vignette2,
VignetteEstimator ve(vio_dataset, optical_centers,
calib_opt->calib->resolution, reprojected_vignette2,
april_grid);
ve.optimize();
@ -353,8 +354,25 @@ void CamCalib::initCamIntrinsics() {
// set resolution
{
int64_t t_ns = vio_dataset->get_image_timestamps()[1];
const auto img_data = vio_dataset->get_image_data(t_ns);
size_t img_idx = 1;
int64_t t_ns = vio_dataset->get_image_timestamps()[img_idx];
auto img_data = vio_dataset->get_image_data(t_ns);
// Find the frame with all valid images
while (img_idx < vio_dataset->get_image_timestamps().size()) {
bool img_data_valid = true;
for (size_t i = 0; i < vio_dataset->get_num_cams(); i++) {
if (!img_data[i].img.get()) img_data_valid = false;
}
if (!img_data_valid) {
img_idx++;
int64_t t_ns_new = vio_dataset->get_image_timestamps()[img_idx];
img_data = vio_dataset->get_image_data(t_ns_new);
} else {
break;
}
}
Eigen::vector<Eigen::Vector2i> res;
@ -481,11 +499,17 @@ void CamCalib::initOptimization() {
for (size_t j = 0; j < vio_dataset->get_image_timestamps().size(); ++j) {
int64_t timestamp_ns = vio_dataset->get_image_timestamps()[j];
TimeCamId tcid = std::make_pair(timestamp_ns, 0);
const CalibInitPoseData &cp = calib_init_poses.at(tcid);
for (size_t cam_id = 0; cam_id < calib_opt->calib->T_i_c.size(); cam_id++) {
TimeCamId tcid = std::make_pair(timestamp_ns, cam_id);
const auto cp_it = calib_init_poses.find(tcid);
calib_opt->addPoseMeasurement(
timestamp_ns, cp.T_a_c * calib_opt->calib->T_i_c[0].inverse());
if (cp_it != calib_init_poses.end()) {
calib_opt->addPoseMeasurement(
timestamp_ns,
cp_it->second.T_a_c * calib_opt->calib->T_i_c[cam_id].inverse());
break;
}
}
}
calib_opt->init();
@ -742,7 +766,8 @@ void CamCalib::drawImageOverlay(pangolin::View &v, size_t cam_id) {
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
if (reprojected_corners.find(tcid) != reprojected_corners.end()) {
if (calib_corners.at(tcid).corner_ids.size() >= MIN_CORNERS) {
if (calib_corners.count(tcid) > 0 &&
calib_corners.at(tcid).corner_ids.size() >= MIN_CORNERS) {
const auto &rc = reprojected_corners.at(tcid);
for (size_t i = 0; i < rc.size(); i++) {
@ -764,7 +789,8 @@ void CamCalib::drawImageOverlay(pangolin::View &v, size_t cam_id) {
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
if (reprojected_vignette.find(tcid) != reprojected_vignette.end()) {
if (calib_corners.at(tcid).corner_ids.size() >= MIN_CORNERS) {
if (calib_corners.count(tcid) > 0 &&
calib_corners.at(tcid).corner_ids.size() >= MIN_CORNERS) {
const auto &rc = reprojected_vignette.at(tcid);
bool has_errors = false;

30
src/calibration/cam_imu_calib.cpp
View File

@ -435,20 +435,23 @@ void CamImuCalib::initOptimization() {
int64_t timestamp_ns = vio_dataset->get_image_timestamps()[j];
TimeCamId tcid = std::make_pair(timestamp_ns, 0);
const CalibInitPoseData &cp = calib_init_poses.at(tcid);
const auto cp_it = calib_init_poses.find(tcid);
calib_opt->addPoseMeasurement(
timestamp_ns, cp.T_a_c * calib_opt->getCamT_i_c(0).inverse());
if (cp_it != calib_init_poses.end()) {
calib_opt->addPoseMeasurement(
timestamp_ns,
cp_it->second.T_a_c * calib_opt->getCamT_i_c(0).inverse());
if (!g_initialized) {
for (size_t i = 0;
i < vio_dataset->get_accel_data().size() && !g_initialized; i++) {
const basalt::AccelData &ad = vio_dataset->get_accel_data()[i];
if (std::abs(ad.timestamp_ns - timestamp_ns) < 3000000) {
g_a_init = cp.T_a_c.so3() * ad.data;
g_initialized = true;
std::cout << "g_a initialized with " << g_a_init.transpose()
<< std::endl;
if (!g_initialized) {
for (size_t i = 0;
i < vio_dataset->get_accel_data().size() && !g_initialized; i++) {
const basalt::AccelData &ad = vio_dataset->get_accel_data()[i];
if (std::abs(ad.timestamp_ns - timestamp_ns) < 3000000) {
g_a_init = cp_it->second.T_a_c.so3() * ad.data;
g_initialized = true;
std::cout << "g_a initialized with " << g_a_init.transpose()
<< std::endl;
}
}
}
}
@ -795,7 +798,8 @@ void CamImuCalib::drawImageOverlay(pangolin::View &v, size_t cam_id) {
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
if (reprojected_corners.find(tcid) != reprojected_corners.end()) {
if (calib_corners.at(tcid).corner_ids.size() >= MIN_CORNERS) {
if (calib_corners.count(tcid) > 0 &&
calib_corners.at(tcid).corner_ids.size() >= MIN_CORNERS) {
const auto &rc = reprojected_corners.at(tcid);
for (size_t i = 0; i < rc.size(); i++) {

7
src/calibration/vignette.cpp
View File

@ -40,11 +40,13 @@ namespace basalt {
VignetteEstimator::VignetteEstimator(
const VioDatasetPtr &vio_dataset,
const Eigen::vector<Eigen::Vector2d> &optical_centers,
const Eigen::vector<Eigen::Vector2i> &resolutions,
const std::map<TimeCamId, Eigen::vector<Eigen::Vector3d>>
&reprojected_vignette,
const AprilGrid &april_grid)
: vio_dataset(vio_dataset),
optical_centers(optical_centers),
resolutions(resolutions),
reprojected_vignette(reprojected_vignette),
april_grid(april_grid),
vign_param(vio_dataset->get_num_cams(),
@ -278,9 +280,8 @@ void VignetteEstimator::compute_data_log(pangolin::DataLog &vign_data_log) {
void VignetteEstimator::save_vign_png(const std::string &path) {
for (size_t k = 0; k < vio_dataset->get_num_cams(); k++) {
auto img =
vio_dataset->get_image_data(vio_dataset->get_image_timestamps()[0])[0];
pangolin::ManagedImage<uint16_t> vign_img(img.img->w, img.img->h);
pangolin::ManagedImage<uint16_t> vign_img(resolutions[k][0],
resolutions[k][1]);
vign_img.Fill(0);
Eigen::Vector2d oc = optical_centers[k];