diff --git a/include/basalt/optimization/linearize.h b/include/basalt/optimization/linearize.h index 2fe0cf7..3526708 100644 --- a/include/basalt/optimization/linearize.h +++ b/include/basalt/optimization/linearize.h @@ -153,7 +153,7 @@ struct LinearizeBase { } else { valid = cam.project(point3d, proj); } - if (!valid) return; + if (!valid || !proj.array().isFinite().all()) return; Eigen::Vector2d residual = proj - corner_pos;