diff --git a/SurfaceFitView.cc b/SurfaceFitView.cc index ef08747..1efc01a 100644 --- a/SurfaceFitView.cc +++ b/SurfaceFitView.cc @@ -2,7 +2,7 @@ #include -void vw::stereo::SurfaceFitViewBase::fit_2d_polynomial_surface( ImageView > const& input, +bool vw::stereo::SurfaceFitViewBase::fit_2d_polynomial_surface( ImageView > const& input, Matrix3x3* output_h, Matrix3x3* output_v, Vector2* xscaling, Vector2* yscaling) const { // Figure out what our scaling parameters should be @@ -12,12 +12,13 @@ void vw::stereo::SurfaceFitViewBase::fit_2d_polynomial_surface( ImageViewy() = 2.0/double(input.cols()); yscaling->y() = 2.0/double(input.rows()); + bool is_good = false; { // Build a ceres problem to fit a polynomial robustly ceres::Problem problem; for (int j = 0; j < input.rows(); j+=2) { for (int i = 0; i < input.cols(); i+=2 ) { - if (is_valid(input(i,j))) + if (is_valid(input(i,j))){ problem.AddResidualBlock (new ceres::AutoDiffCostFunction (new PolynomialSurfaceFit @@ -26,9 +27,14 @@ void vw::stereo::SurfaceFitViewBase::fit_2d_polynomial_surface( ImageViewx()) * yscaling->y())), new ceres::CauchyLoss(4), &(*output_h)(0,0)); + is_good = true; + } } } + // Quit rather than error out if there is no valid disparity + if (!is_good) return false; + ceres::Solver::Options options; options.max_num_iterations = 300; options.minimizer_progress_to_stdout = false; @@ -61,6 +67,8 @@ void vw::stereo::SurfaceFitViewBase::fit_2d_polynomial_surface( ImageView > const& input, + bool fit_2d_polynomial_surface( ImageView > const& input, Matrix3x3* output_h, Matrix3x3* output_v, Vector2* xscaling, Vector2* yscaling) const; @@ -80,20 +80,26 @@ namespace vw { Vector2 xscaling, yscaling; ImageView > copy = crop(edge_extend(m_input), exp_bbox); - fit_2d_polynomial_surface(copy, - &polynomial_h, &polynomial_v, - &xscaling, &yscaling); - - ImageView fitted_h(exp_bbox.width(), exp_bbox.height()), - fitted_v(exp_bbox.width(), exp_bbox.height()); - render_polynomial_surface(polynomial_h, &fitted_h); - render_polynomial_surface(polynomial_v, &fitted_v); - - ImageView smoothed_disparity(exp_bbox.width(),exp_bbox.height()); - fill(smoothed_disparity, pixel_type(Vector2f())); - select_channel(smoothed_disparity, 0) = fitted_h; - select_channel(smoothed_disparity, 1) = fitted_v; - + ImageView smoothed_disparity(exp_bbox.width(), + exp_bbox.height()); + + bool ans = fit_2d_polynomial_surface(copy, + &polynomial_h, &polynomial_v, + &xscaling, &yscaling); + if (ans){ + ImageView fitted_h(exp_bbox.width(), exp_bbox.height()), + fitted_v(exp_bbox.width(), exp_bbox.height()); + render_polynomial_surface(polynomial_h, &fitted_h); + render_polynomial_surface(polynomial_v, &fitted_v); + + fill(smoothed_disparity, pixel_type(Vector2f())); + select_channel(smoothed_disparity, 0) = fitted_h; + select_channel(smoothed_disparity, 1) = fitted_v; + }else{ + // Could not fit a surface, return the original disparity + smoothed_disparity = copy; + } + return prerasterize_type(smoothed_disparity, -exp_bbox.min().x(), -exp_bbox.min().y(), cols(), rows());