diff --git a/src/calibration/calibraiton_helper.cpp b/src/calibration/calibraiton_helper.cpp index c96c74b..323f26d 100644 --- a/src/calibration/calibraiton_helper.cpp +++ b/src/calibration/calibraiton_helper.cpp @@ -69,7 +69,9 @@ bool estimateTransformation( for (size_t i = 0; i < corners.size(); i++) { Eigen::Vector4d tmp; - cam_calib.unproject(corners[i], tmp); + if (!cam_calib.unproject(corners[i], tmp)) { + continue; + } Eigen::Vector3d bearing = tmp.head<3>(); Eigen::Vector3d point = aprilgrid_corner_pos_3d[corner_ids[i]].head<3>(); bearing.normalize(); diff --git a/src/calibration/cam_calib.cpp b/src/calibration/cam_calib.cpp index da21587..b70f402 100644 --- a/src/calibration/cam_calib.cpp +++ b/src/calibration/cam_calib.cpp @@ -486,7 +486,8 @@ void CamCalib::initCamIntrinsics() { for (size_t j = 0; j < vio_dataset->get_num_cams(); j++) { if (!cam_initialized[j]) { std::vector pinhole_corners; - int w, h; + int w = 0; + int h = 0; for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i += inc) { @@ -507,6 +508,8 @@ void CamCalib::initCamIntrinsics() { h = img_vec[j].img->h; } + BASALT_ASSERT(w > 0 && h > 0); + Eigen::Vector4d init_intr; bool success = CalibHelper::initializeIntrinsicsPinhole(