vpgl/algo/vpgl_rational_adjust_multipt.cxx

#include <cmath>
#include <limits>
#include <utility>
#include "vpgl_rational_adjust_multipt.h"
//:
// \file
#include <cassert>
#include "vgl/vgl_point_3d.h"
#include "vnl/vnl_numeric_traits.h"
#include <vnl/algo/vnl_levenberg_marquardt.h>
#include <vpgl/algo/vpgl_backproject.h>
#include <vpgl/algo/vpgl_ray_intersect.h>
#include <vpgl/algo/vpgl_rational_adjust_onept.h>

#ifdef _MSC_VER
#  include "vcl_msvc_warnings.h"
#endif

double
compute_projection_error(std::vector<vpgl_rational_camera<double>> const & cams,
                         std::vector<vgl_point_2d<double>> const & corrs,
                         vgl_point_3d<double> & intersection)
{
  auto cit = cams.begin();
  auto rit = corrs.begin();
  double error = 0.0;
  for (; cit != cams.end() && rit != corrs.end(); ++cit, ++rit)
  {
    vgl_point_2d<double> uvp = (*cit).project(intersection);
    vgl_point_2d<double> uv = *rit;
    double err = std::sqrt(std::pow(uv.x() - uvp.x(), 2.0) + std::pow(uv.y() - uvp.y(), 2));
    error += err;
  }
  return error;
}

double
error_corr(vpgl_rational_camera<double> const & cam,
           vgl_point_2d<double> const & corr,
           vgl_point_3d<double> const & intersection)
{
  vgl_point_2d<double> uvp = cam.project(intersection);
  return std::sqrt(std::pow(corr.x() - uvp.x(), 2.0) + std::pow(corr.y() - uvp.y(), 2));
}

//: assumes an initial estimate for intersection values, only refines the intersection and computes a re-projection
// error
double
re_projection_error(std::vector<vpgl_rational_camera<double>> const & cams,
                    std::vector<float> const & cam_weights,
                    std::vector<std::vector<vgl_point_2d<double>>> const &
                      corrs, // for each 3d corr (outer vector), 2d locations for each cam (inner vector)
                    std::vector<vgl_point_3d<double>> const & intersections,
                    std::vector<vgl_point_3d<double>> & finals)
{
  double error = 100000.0;

  finals.clear();
  for (unsigned int i = 0; i < corrs.size(); ++i)
  {
    vgl_point_3d<double> final;
    if (!vpgl_rational_adjust_onept::refine_intersection_pt(cams, cam_weights, corrs[i], intersections[i], final))
      return error;
    finals.push_back(final);
  }

  error = 0;
  for (unsigned int i = 0; i < corrs.size(); ++i)
  {
    error += compute_projection_error(cams, corrs[i], finals[i]);
  }
  return error;
}

//: assumes an initial estimate for intersection values, only refines the intersection and computes a re-projection
// error for each corr separately
void
re_projection_error(std::vector<vpgl_rational_camera<double>> const & cams,
                    std::vector<float> const & cam_weights,
                    std::vector<std::vector<vgl_point_2d<double>>> const &
                      corrs, // for each 3d corr (outer vector), 2d locations for each cam (inner vector)
                    std::vector<vgl_point_3d<double>> const & intersections,
                    std::vector<vgl_point_3d<double>> & finals,
                    vnl_vector<double> & errors)
{
  double error = 100000.0;
  errors.fill(error);

  finals.clear();
  for (unsigned int i = 0; i < corrs.size(); ++i)
  {
    vgl_point_3d<double> final;
    if (!vpgl_rational_adjust_onept::refine_intersection_pt(cams, cam_weights, corrs[i], intersections[i], final))
      return;
    finals.push_back(final);
  }

  unsigned k = 0;
  // return an error value for each cam for each corr
  for (unsigned int i = 0; i < corrs.size(); ++i)
  {
    for (unsigned int j = 0; j < cams.size(); ++j)
    {
      errors[k] = error_corr(cams[j], corrs[i][j], intersections[i]);
      k++;
    }
  }
}


void
print_perm(std::vector<unsigned> & params_indices)
{
  for (unsigned int params_indice : params_indices)
    std::cout << params_indice << ' ';
  std::cout << std::endl;
}

//: to generate the permutations, always increment the one at the very end by one; if it exceeds max, then increment the
// one before as well, etc.
bool
increment_perm(std::vector<unsigned> & params_indices, unsigned size)
{
  if (params_indices.empty()) // no need to permute!!
    return true;

  params_indices[params_indices.size() - 1] += 1;
  if (params_indices[params_indices.size() - 1] == size)
  { // carry on
    params_indices[params_indices.size() - 1] = 0;

    if (params_indices.size() < 2)
      return true; // we're done

    int current_i = (int)params_indices.size() - 2;
    while (current_i >= 0)
    {
      params_indices[current_i] += 1;
      if (params_indices[current_i] < size)
        break;
      if (current_i == 0)
        return true;
      params_indices[current_i] = 0;
      current_i -= 1;
    }
  }
  return false;
}

//: performs an exhaustive search in the parameter space of the cameras.
bool
vpgl_rational_adjust_multiple_pts::adjust(
  std::vector<vpgl_rational_camera<double>> const & cams,
  std::vector<float> const & cam_weights,
  std::vector<std::vector<vgl_point_2d<double>>> const & corrs, // a vector of correspondences for each cam
  double radius,
  int n, // divide radius into n intervals to generate camera translation space
  std::vector<vgl_vector_2d<double>> & cam_translations, // output translations for each cam
  std::vector<vgl_point_3d<double>> & intersections)
{
  cam_translations.clear();
  intersections.clear();
  intersections.resize(corrs.size());
  if (cams.empty() || corrs.empty() || cams.size() != corrs.size())
    return false;
  if (corrs[0].empty())
    return false;
  auto cnt_corrs_for_each_cam = (unsigned)corrs[0].size();
  for (unsigned int i = 1; i < corrs.size(); ++i)
    if (corrs[i].size() != cnt_corrs_for_each_cam) // there needs to be same number of corrs for each cam
      return false;

  // turn the correspondences into the format that we'll need
  std::vector<vgl_point_2d<double>> temp(cams.size());
  std::vector<std::vector<vgl_point_2d<double>>> corrs_reformatted(cnt_corrs_for_each_cam, temp);

  for (unsigned int i = 0; i < cnt_corrs_for_each_cam; ++i)
  {                                                 // for each corr
    for (unsigned int j = 0; j < corrs.size(); ++j) // for each cam (corr size and cams size are equal)
      corrs_reformatted[i][j] = corrs[j][i];
  }
  // find the best intersections for all the correspondences using the given cameras to compute good initial estimates
  // for z of each correspondence
  std::vector<vgl_point_3d<double>> intersections_initial;
  for (const auto & i : corrs_reformatted)
  {
    vgl_point_3d<double> pt;
    if (!vpgl_rational_adjust_onept::find_intersection_point(cams, cam_weights, i, pt))
      return false;
    intersections_initial.push_back(pt);
  }

  // search the camera translation space
  int param_cnt = 2 * (int)cams.size();
  double increment = radius / n;
  std::vector<double> param_values;
  param_values.push_back(0.0);
  for (int i = 1; i <= n; ++i)
  {
    param_values.push_back(i * increment);
    param_values.push_back(-i * increment);
  }
  for (double param_value : param_values)
    std::cout << param_value << ' ';
  std::cout << '\n';

  // now for each param go through all possible param values
  std::vector<unsigned> params_indices(param_cnt, 0);
  int cnt = (int)std::pow((float)param_cnt, (float)param_values.size());
  std::cout << "will try: " << cnt << " param combinations: ";
  std::cout.flush();
  bool done = false;
  double big_value = 10000000.0;
  double min_error = big_value;
  std::vector<unsigned> params_indices_best(params_indices);
  while (!done)
  {
    std::cout << '.';
    std::cout.flush();
    std::vector<vpgl_rational_camera<double>> current_cams(cams);
    // translate current cams
    for (unsigned int i = 0; i < current_cams.size(); ++i)
    {
      double u_off, v_off;
      current_cams[i].image_offset(u_off, v_off);
      current_cams[i].set_image_offset(u_off + param_values[params_indices[i * 2]],
                                       v_off + param_values[params_indices[i * 2 + 1]]);
    }

    // use the initial estimates to compute re-projection errors
    std::vector<vgl_point_3d<double>> finals;
    double err = re_projection_error(current_cams, cam_weights, corrs_reformatted, intersections_initial, finals);

    if (err < min_error)
    {
      min_error = err;
      params_indices_best = params_indices;
      intersections = finals;
    }
    done = increment_perm(params_indices, (unsigned)param_values.size());
  }
  if (min_error < big_value)
  {
    std::cout << " done! found global min! min error: " << min_error << '\n';
    std::vector<vpgl_rational_camera<double>> current_cams(cams);
    // return translations
    std::cout << "translations for each camera:" << std::endl;
    for (unsigned int i = 0; i < current_cams.size(); ++i)
    {
      vgl_vector_2d<double> tr(param_values[params_indices_best[i * 2]], param_values[params_indices_best[i * 2 + 1]]);
      std::cout << tr << std::endl;
      cam_translations.push_back(tr);
      double u_off, v_off;
      current_cams[i].image_offset(u_off, v_off);
      current_cams[i].set_image_offset(u_off + param_values[params_indices_best[i * 2]],
                                       v_off + param_values[params_indices_best[i * 2 + 1]]);
    }
  }
  else
  {
    std::cout << " done! no global min!\n";
    return false;
  }
  return true;
}

vpgl_cam_trans_search_lsqr::vpgl_cam_trans_search_lsqr(
  std::vector<vpgl_rational_camera<double>> const & cams,
  std::vector<float> cam_weights,
  std::vector<std::vector<vgl_point_2d<double>>> const &
    image_pts, // for each 3D corr, an array of 2D corrs for each camera
  std::vector<vgl_point_3d<double>> initial_pts)
  : vnl_least_squares_function(2 * (unsigned)cams.size(),
                               (unsigned)(cams.size() * image_pts.size()),
                               vnl_least_squares_function::no_gradient)
  , initial_pts_(std::move(initial_pts))
  , cameras_(cams)
  , cam_weights_(std::move(cam_weights))
  , corrs_(image_pts)
{}

void
vpgl_cam_trans_search_lsqr::f(vnl_vector<double> const & translation, // size is 2*cams.size()
                              vnl_vector<double> & projection_errors) // size is cams.size()*image_pts.size() -->
                                                                      // compute a residual for each 3D corr point
{
  // compute the new set of cameras with the current cam parameters
  std::vector<vpgl_rational_camera<double>> current_cams(cameras_);
  // translate current cams
  for (unsigned int i = 0; i < current_cams.size(); ++i)
  {
    double u_off, v_off;
    current_cams[i].image_offset(u_off, v_off);
    current_cams[i].set_image_offset(u_off + translation[i * 2], v_off + translation[i * 2 + 1]);
  }
  // compute the projection error for each cam for each corr
  // use the initial estimates to compute re-projection errors
  re_projection_error(current_cams, cam_weights_, corrs_, initial_pts_, finals_, projection_errors);
}

void
vpgl_cam_trans_search_lsqr::get_finals(std::vector<vgl_point_3d<double>> & finals)
{
  finals = finals_;
}

//: run Lev-Marq optimization to search the param space to find the best parameter setting
bool
vpgl_rational_adjust_multiple_pts::adjust_lev_marq(
  std::vector<vpgl_rational_camera<double>> const & cams,
  std::vector<float> const & cam_weights,
  std::vector<std::vector<vgl_point_2d<double>>> const & corrs, // a vector of correspondences for each cam
  std::vector<vgl_vector_2d<double>> & cam_translations,        // output translations for each cam
  std::vector<vgl_point_3d<double>> & intersections)            // output 3d locations for each correspondence
{
  cam_translations.clear();
  intersections.clear();
  intersections.resize(corrs.size());
  if (cams.empty() || corrs.empty() || cams.size() != corrs.size())
    return false;
  if (corrs[0].empty())
    return false;
  auto cnt_corrs_for_each_cam = (unsigned)corrs[0].size();
  for (unsigned int i = 1; i < corrs.size(); ++i)
    if (corrs[i].size() != cnt_corrs_for_each_cam) // there needs to be same number of corrs for each cam
      return false;

  // turn the correspondences into the format that we'll need
  std::vector<vgl_point_2d<double>> temp(cams.size());
  std::vector<std::vector<vgl_point_2d<double>>> corrs_reformatted(cnt_corrs_for_each_cam, temp);

  for (unsigned int i = 0; i < cnt_corrs_for_each_cam; ++i)
  {                                                 // for each corr
    for (unsigned int j = 0; j < corrs.size(); ++j) // for each cam (corr size and cams size are equal)
      corrs_reformatted[i][j] = corrs[j][i];
  }
  // find the best intersections for all the correspondences using the given cameras to compute good initial estimates
  // for z of each correspondence
  std::vector<vgl_point_3d<double>> intersections_initial;
  for (const auto & i : corrs_reformatted)
  {
    vgl_point_3d<double> pt;
    if (!vpgl_rational_adjust_onept::find_intersection_point(cams, cam_weights, i, pt))
      return false;
    intersections_initial.push_back(pt);
  }
  // now refine those using Lev_Marqs
  for (unsigned int i = 0; i < corrs_reformatted.size(); ++i)
  {
    vgl_point_3d<double> final;
    if (!vpgl_rational_adjust_onept::refine_intersection_pt(
          cams, cam_weights, corrs_reformatted[i], intersections_initial[i], final))
      return false;
    intersections_initial[i] = final;
  }
  for (const auto & i : intersections_initial)
    std::cout << "before adjustment initial 3D intersection point: " << i << std::endl;

  // search the camera translation space using Lev-Marq
  vpgl_cam_trans_search_lsqr transsf(cams, cam_weights, corrs_reformatted, intersections_initial);
  vnl_levenberg_marquardt levmarq(transsf);
  levmarq.set_verbose(true);
  // Set the x-tolerance.  When the length of the steps taken in X (variables)
  // are no longer than this, the minimization terminates.
  levmarq.set_x_tolerance(1e-10);
  // Set the epsilon-function.  This is the step length for FD Jacobian.
  levmarq.set_epsilon_function(0.01);
  // Set the f-tolerance.  When the successive RMS errors are less than this,
  // minimization terminates.
  levmarq.set_f_tolerance(1e-15);
  // Set the maximum number of iterations
  levmarq.set_max_function_evals(10000);
  vnl_vector<double> translations(2 * (unsigned)cams.size(), 0.0);
  std::cout << "Minimization x epsilon: " << levmarq.get_f_tolerance() << std::endl;

  // Minimize the error and get the best intersection point
  levmarq.minimize(translations);
  levmarq.diagnose_outcome();
  transsf.get_finals(intersections);
  std::cout << "final translations:" << std::endl;
  for (unsigned int i = 0; i < cams.size(); ++i)
  {
    // if cam weight is 1 for one of them, it is a special case, then pass 0 as translation for that one, cause
    // projections still cause a tiny translation, ignore that
    vgl_vector_2d<double> trans(0.0, 0.0);
    if (cam_weights[i] == 1.0f)
    {
      cam_translations.push_back(trans);
    }
    else
    {
      trans.set(translations[2 * i], translations[2 * i + 1]);
      cam_translations.push_back(trans);
    }
    // sanity check
    if (std::abs(trans.x()) > 200 || std::abs(trans.y()) > 200)
    {
      std::cerr << " trans: " << trans << " failed sanity check! returning false!\n";
      return false;
    }
    std::cout << trans << '\n';
  }
  return true;
}

// run Lev-Marq optimization to search the param spae to find the best parameter setting
bool
vpgl_rational_adjust_multiple_pts::adjust_lev_marq(
  std::vector<vpgl_rational_camera<double>> const & cams,       // cameras that will be corrected
  std::vector<float> const & cam_weights,                       // camera weight parameters
  std::vector<std::vector<vgl_point_2d<double>>> const & corrs, // a vector of correspondences for each cam
  vgl_point_3d<double> const & initial_pt,                      // initial 3-d point for back-projection
  double const & zmin,                                          // minimum allowed height of the 3-d intersection point
  double const & zmax,                                          // maximum allowed height of the 3-d intersection point
  std::vector<vgl_vector_2d<double>> & cam_translations,        // output translations for each camera
  std::vector<vgl_point_3d<double>> & intersections,            // output 3-d locations for each correspondence
  double const relative_diameter)
{
  cam_translations.clear();
  intersections.clear();
  intersections.clear();
  intersections.resize(corrs.size());
  if (cams.empty() || corrs.empty() || cams.size() != corrs.size())
    return false;
  if (corrs[0].empty())
    return false;
  auto cnt_corrs_for_each_cam = (unsigned)corrs[0].size();
  for (unsigned i = 1; i < corrs.size(); ++i)
    if (corrs[i].size() != cnt_corrs_for_each_cam)
      return false;

  // reformat the correspondences array
  std::vector<vgl_point_2d<double>> temp(cams.size());
  std::vector<std::vector<vgl_point_2d<double>>> corrs_reformatted(cnt_corrs_for_each_cam, temp);

  for (unsigned int i = 0; i < cnt_corrs_for_each_cam; ++i)
  {                                                 // for each corr
    for (unsigned int j = 0; j < corrs.size(); ++j) // for each cam (corr size and cams size are equal)
      corrs_reformatted[i][j] = corrs[j][i];
  }
  // find the best 3-d intersections for all the correspondences using the given cameras to compute good initial
  // estimates for z of each correspondence
  std::vector<vgl_point_3d<double>> intersections_initial;
  for (const auto & i : corrs_reformatted)
  {
    vgl_point_3d<double> pt;
    if (!vpgl_rational_adjust_onept::find_intersection_point(
          cams, cam_weights, i, initial_pt, zmin, zmax, pt, relative_diameter))
      return false;
    intersections_initial.push_back(pt);
  }

  // now refine the intersections using Lev_Marqs
  for (unsigned i = 0; i < corrs_reformatted.size(); i++)
  {
    vgl_point_3d<double> final;
    if (!vpgl_rational_adjust_onept::refine_intersection_pt(
          cams, cam_weights, corrs_reformatted[i], intersections_initial[i], final, relative_diameter))
      return false;
    intersections_initial[i] = final;
  }
  for (const auto & i : intersections_initial)
    std::cout << "before adjustment initial 3D intersection point: " << i << std::endl;

  // search the camera translation space using Lev-Marq
  vpgl_cam_trans_search_lsqr transsf(cams, cam_weights, corrs_reformatted, intersections_initial);
  vnl_levenberg_marquardt levmarq(transsf);
  levmarq.set_verbose(true);
  // Set the x-tolerance.  Minimization terminates when the length of the steps taken in X (variables) are less than
  // input x-tolerance
  levmarq.set_x_tolerance(1e-10);
  // Set the epsilon-function.  This is the step length for FD Jacobian
  levmarq.set_epsilon_function(0.01);
  // Set the f-tolerance.  Minimization terminates when the successive RSM errors are less then this
  levmarq.set_f_tolerance(1e-15);
  // Set the maximum number of iterations
  levmarq.set_max_function_evals(10000);
  vnl_vector<double> translations(2 * (unsigned)cams.size(), 0.0);
  std::cout << "Minimization x epsilon: " << levmarq.get_f_tolerance() << std::endl;

  // Minimize the error and get the best intersection point
  levmarq.minimize(translations);
  levmarq.diagnose_outcome();
  transsf.get_finals(intersections);
  std::cout << "final translations:" << std::endl;
  for (unsigned i = 0; i < cams.size(); i++)
  {
    // if cam weight is 1 for one of them, it is a special case, then pass 0 as translation for that one, cause
    // projections still cause a tiny translation, ignore that
    vgl_vector_2d<double> trans(0.0, 0.0);
    if (cam_weights[i] == 1.0f)
    {
      cam_translations.push_back(trans);
    }
    else
    {
      trans.set(translations[2 * i], translations[2 * i + 1]);
      cam_translations.push_back(trans);
    }
    if (std::abs(trans.x()) > 200 || std::abs(trans.y()) > 200)
    {
      std::cerr << " trans: " << trans << " failed sanity check! returning false!\n";
      return false;
    }
    std::cout << trans << '\n';
  }
  return true;
}