xref: /dports/misc/vxl/vxl-3.3.2/core/vpgl/algo/vpgl_camera_convert.cxx

// This is core/vpgl/algo/vpgl_camera_convert.cxx
#ifndef vpgl_camera_convert_cxx_
#define vpgl_camera_convert_cxx_

#include <iostream>
#include <cstdlib>
#include <cmath>
#include "vpgl_camera_convert.h"
//:
// \file
#include <cassert>
#ifdef _MSC_VER
#  include "vcl_msvc_warnings.h"
#endif
#include "vnl/vnl_numeric_traits.h"
#include "vnl/vnl_det.h"
#include "vnl/vnl_vector_fixed.h"
#include "vnl/vnl_matrix_fixed.h"
#include "vnl/vnl_random.h"
#include <vnl/algo/vnl_svd.h>
#include <vnl/algo/vnl_qr.h>
#include <vgl/algo/vgl_rotation_3d.h>
#include <vgl/algo/vgl_h_matrix_3d.h>
#include "vgl/vgl_homg_point_3d.h"
#include <vpgl/algo/vpgl_ortho_procrustes.h>
#include <vpgl/algo/vpgl_optimize_camera.h>
#include <vpgl/algo/vpgl_camera_compute.h>
#include "vgl/vgl_point_2d.h"
#include "vgl/vgl_point_3d.h"
#include "vgl/vgl_box_3d.h"
#include "vgl/vgl_homg_point_2d.h"
#include "vgl/vgl_ray_3d.h"
#include "vgl/vgl_plane_3d.h"
#include "vbl/vbl_array_2d.h"
#include "vpgl/vpgl_lvcs.h"
#include <vpgl/algo/vpgl_backproject.h>


static std::vector<double>
pvector(const double x, const double y, const double z)
{
  // fill the vector
  std::vector<double> pv(20);
  pv[0] = x * x * x;
  pv[1] = x * x * y;
  pv[2] = x * x * z;
  pv[3] = x * x;
  pv[4] = x * y * y;
  pv[5] = x * y * z;
  pv[6] = x * y;
  pv[7] = x * z * z;
  pv[8] = x * z;
  pv[9] = x;
  pv[10] = y * y * y;
  pv[11] = y * y * z;
  pv[12] = y * y;
  pv[13] = y * z * z;
  pv[14] = y * z;
  pv[15] = y;
  pv[16] = z * z * z;
  pv[17] = z * z;
  pv[18] = z;
  pv[19] = 1;
  return pv;
}

static std::vector<double>
power_vector_dx(const double x, const double y, const double z)
{
  // fill the vector
  std::vector<double> pv(20, 0.0);
  pv[0] = 3 * x * x;
  pv[1] = 2 * x * y;
  pv[2] = 2 * x * z;
  pv[3] = 2 * x;
  pv[4] = y * y;
  pv[5] = y * z;
  pv[6] = y;
  pv[7] = z * z;
  pv[8] = z;
  pv[9] = 1;
  return pv;
}

static std::vector<double>
power_vector_dy(const double x, const double y, const double z)
{
  // fill the vector
  std::vector<double> pv(20, 0.0);
  pv[1] = x * x;
  pv[4] = 2 * x * y;
  pv[5] = x * z;
  pv[6] = x;
  pv[10] = 3 * y * y;
  pv[11] = 2 * y * z;
  pv[12] = 2 * y;
  pv[13] = z * z;
  pv[14] = z;
  pv[15] = 1;
  return pv;
}

static std::vector<double>
power_vector_dz(const double x, const double y, const double z)
{
  // fill the vector
  std::vector<double> pv(20, 0.0);
  pv[2] = x * x;
  pv[5] = x * y;
  pv[7] = 2 * x * z;
  pv[8] = x;
  pv[11] = y * y;
  pv[13] = 2 * y * z;
  pv[14] = y;
  pv[16] = 3 * z * z;
  pv[17] = 2 * z;
  pv[18] = 1;
  return pv;
}

// convert the value of the polynomial
static double
pval(std::vector<double> const & pv, std::vector<double> const & coef)
{
  double sum = 0.0;
  for (unsigned i = 0; i < 20; ++i)
    sum += pv[i] * coef[i];
  return sum;
}

// Find an approximate projective camera that approximates a rational camera
// at the world center.
bool
vpgl_proj_camera_convert::convert(vpgl_rational_camera<double> const & rat_cam,
                                  vgl_point_3d<double> const & world_center,
                                  vpgl_proj_camera<double> & camera)
{
  double x0 = world_center.x(), y0 = world_center.y(), z0 = world_center.z();
  // normalized world center
  double nx0 = rat_cam.scl_off(vpgl_rational_camera<double>::X_INDX).normalize(x0);
  double ny0 = rat_cam.scl_off(vpgl_rational_camera<double>::Y_INDX).normalize(y0);
  double nz0 = rat_cam.scl_off(vpgl_rational_camera<double>::Z_INDX).normalize(z0);

  // get the rational coefficients
  std::vector<std::vector<double>> coeffs = rat_cam.coefficients();
  std::vector<double> neu_u = coeffs[0];
  std::vector<double> den_u = coeffs[1];
  std::vector<double> neu_v = coeffs[2];
  std::vector<double> den_v = coeffs[3];

  // normalize for numerical precision
  double nmax_u = -vnl_numeric_traits<double>::maxval;
  double dmax_u = nmax_u, nmax_v = nmax_u, dmax_v = nmax_u;
  for (unsigned i = 0; i < 20; ++i)
  {
    if (std::fabs(neu_u[i]) > nmax_u)
      nmax_u = std::fabs(neu_u[i]);
    if (std::fabs(den_u[i]) > dmax_u)
      dmax_u = std::fabs(den_u[i]);
    if (std::fabs(neu_v[i]) > nmax_v)
      nmax_v = std::fabs(neu_v[i]);
    if (std::fabs(den_v[i]) > dmax_v)
      dmax_v = std::fabs(den_v[i]);
  }
  // Normalize polys so that ratio of numerator and denominator is unchanged
  double norm_u = nmax_u, norm_v = nmax_v;
  if (norm_u < dmax_u)
    norm_u = dmax_u;
  if (norm_v < dmax_v)
    norm_v = dmax_v;
  for (unsigned i = 0; i < 20; ++i)
  {
    neu_u[i] /= norm_u;
    den_u[i] /= norm_u;
    neu_v[i] /= norm_v;
    den_v[i] /= norm_v;
  }

  // Convert linear approximations to each poly
  // lin_p(x, y, z)=p(0,0,0) + (dp/dx)(x-nx0) + (dp/dy)(y-ny0) + (dp/dz)(z-nz0)
  std::vector<double> pv0 = pvector(nx0, ny0, nz0);
  double neu_u_0 = pval(pv0, neu_u), den_u_0 = pval(pv0, den_u);
  double neu_v_0 = pval(pv0, neu_v), den_v_0 = pval(pv0, den_v);
  std::vector<double> pv_dx0 = power_vector_dx(nx0, ny0, nz0);
  double neu_u_dx0 = pval(pv_dx0, neu_u), den_u_dx0 = pval(pv_dx0, den_u);
  double neu_v_dx0 = pval(pv_dx0, neu_v), den_v_dx0 = pval(pv_dx0, den_v);
  std::vector<double> pv_dy0 = power_vector_dy(nx0, ny0, nz0);
  double neu_u_dy0 = pval(pv_dy0, neu_u), den_u_dy0 = pval(pv_dy0, den_u);
  double neu_v_dy0 = pval(pv_dy0, neu_v), den_v_dy0 = pval(pv_dy0, den_v);
  std::vector<double> pv_dz0 = power_vector_dz(nx0, ny0, nz0);
  double neu_u_dz0 = pval(pv_dz0, neu_u), den_u_dz0 = pval(pv_dz0, den_u);
  double neu_v_dz0 = pval(pv_dz0, neu_v), den_v_dz0 = pval(pv_dz0, den_v);

  // Construct the matrix to convert the center of projection
  vnl_matrix<double> C(4, 4);
  C[0][0] = neu_u_dx0;
  C[0][1] = neu_u_dy0;
  C[0][2] = neu_u_dz0;
  C[0][3] = neu_u_0;
  C[1][0] = den_u_dx0;
  C[1][1] = den_u_dy0;
  C[1][2] = den_u_dz0;
  C[1][3] = den_u_0;
  C[2][0] = neu_v_dx0;
  C[2][1] = neu_v_dy0;
  C[2][2] = neu_v_dz0;
  C[2][3] = neu_v_0;
  C[3][0] = den_v_dx0;
  C[3][1] = den_v_dy0;
  C[3][2] = den_v_dz0;
  C[3][3] = den_v_0;

  vnl_svd<double> svd(C);
  vnl_vector<double> nv = svd.nullvector();
  // assume not at infinity
  nv /= nv[3];
#if 1
  std::cout << "Center of projection\n" << nv << '\n' << "Residual\n" << C * nv << '\n';
#endif
  // Normalize with respect to the principal plane normal (principal ray)
  double ndu = std::sqrt(den_u_dx0 * den_u_dx0 + den_u_dy0 * den_u_dy0 + den_u_dz0 * den_u_dz0);
  double ndv = std::sqrt(den_v_dx0 * den_v_dx0 + den_v_dy0 * den_v_dy0 + den_v_dz0 * den_v_dz0);

  // determine if the projection is affine
  if (ndu / std::fabs(den_u_0) < 1.0e-10 || ndv / std::fabs(den_v_0) < 1.0e-10)
  {
    std::cout << "Camera is nearly affine - approximation not implemented!\n";
    return false;
  }
  // Construct M by joined scale factor vector
  vnl_matrix_fixed<double, 3, 3> M;
  for (unsigned i = 0; i < 3; ++i)
  {
    M[0][i] = C[0][i] / ndu;
    M[1][i] = C[2][i] / ndv;
    M[2][i] = (C[1][i] / ndu + C[3][i] / ndv) / 2;
  }
#if 1
  std::cout << "M matrix\n" << M << '\n';

  vnl_matrix_fixed<double, 3, 3> Mf;
  for (int i = 0; i < 3; ++i)
    for (int j = 0; j < 3; ++j)
      Mf(i, j) = M(2 - j, 2 - i);
  vnl_qr<double> QR(Mf.as_ref());
  vnl_matrix_fixed<double, 3, 3> q, r, Qf, Rf, uq, ur;
  q = QR.Q();
  r = QR.R();
  for (int i = 0; i < 3; ++i)
  {
    for (int j = 0; j < 3; ++j)
    {
      Qf(i, j) = q(2 - j, 2 - i);
      Rf(i, j) = r(2 - j, 2 - i);
    }
  }
  std::cout << "Flipped Rotation\n" << Qf << '\n' << "Flipped Upper Triangular\n" << Rf << '\n';
  vnl_qr<double> uqr(M.as_ref());
  uq = uqr.Q();
  ur = uqr.R();
  std::cout << "UnFlipped Rotation\n"
            << uq << '\n'
            << "UnFlipped Upper Triangular\n"
            << ur << '\n'
            << "Det uq " << vnl_det<double>(uq) << '\n';
  // Normalized denominators
  vnl_vector<double> c1(3), c3(3);
  for (unsigned i = 0; i < 3; ++i)
  {
    c1[i] = C[1][i] / ndu;
    c3[i] = C[3][i] / ndv;
  }

  std::cout << "Denominators\n"
            << "C1 " << c1 << "C3 " << c3;
#endif
  // convert p3 the fourth column of the projection matrix
  vnl_vector_fixed<double, 3> c;
  for (unsigned i = 0; i < 3; ++i)
    c[i] = nv[i];
  vnl_vector_fixed<double, 3> p3 = -M * c;
  // Form the full projection matrix
  vnl_matrix_fixed<double, 3, 4> pmatrix;
  for (unsigned r = 0; r < 3; ++r)
    for (unsigned c = 0; c < 3; ++c)
      pmatrix[r][c] = M[r][c];
  for (unsigned r = 0; r < 3; ++r)
    pmatrix[r][3] = p3[r];
  // account for the image scale and offsets
  double uscale = rat_cam.scale(vpgl_rational_camera<double>::U_INDX);
  double uoff = rat_cam.offset(vpgl_rational_camera<double>::U_INDX);
  double vscale = rat_cam.scale(vpgl_rational_camera<double>::V_INDX);
  double voff = rat_cam.offset(vpgl_rational_camera<double>::V_INDX);
  vnl_matrix_fixed<double, 3, 3> Kr;
  Kr.fill(0.0);
  Kr[0][0] = uscale;
  Kr[0][2] = uoff;
  Kr[1][1] = vscale;
  Kr[1][2] = voff;
  Kr[2][2] = 1.0;
#if 1
  std::cout << "Kr\n" << Kr << '\n';
  vnl_matrix_fixed<double, 3, 3> KRf, KR = Kr * uq;
  for (int i = 0; i < 3; ++i)
    for (int j = 0; j < 3; ++j)
      KRf(i, j) = KR(2 - j, 2 - i);
  vnl_qr<double> krQR(KRf.as_ref());
  vnl_matrix_fixed<double, 3, 3> krq, krr, krQf, krRf;
  krq = krQR.Q();
  krr = krQR.R();
  for (int i = 0; i < 3; ++i)
  {
    for (int j = 0; j < 3; ++j)
    {
      krQf(i, j) = krq(2 - j, 2 - i);
      krRf(i, j) = krr(2 - j, 2 - i);
    }
  }
  std::cout << "Flipped Rotation (KR)\n" << krQf << '\n' << "Flipped Upper Triangular (KR)\n" << krRf << '\n';

  int r0pos = krRf(0, 0) > 0 ? 1 : -1;
  int r1pos = krRf(1, 1) > 0 ? 1 : -1;
  int r2pos = krRf(2, 2) > 0 ? 1 : -1;
  int diag[3] = { r0pos, r1pos, r2pos };
  vnl_matrix_fixed<double, 3, 3> K1, R1;
  for (int i = 0; i < 3; ++i)
  {
    for (int j = 0; j < 3; ++j)
    {
      K1(i, j) = diag[j] * krRf(i, j);
      R1(i, j) = diag[i] * krQf(i, j);
    }
  }
  K1 = K1 / K1(2, 2);
  std::cout << "K1\n" << K1 << '\n' << "R1\n" << R1 << '\n' << "Det R1 " << vnl_det<double>(R1) << '\n';
#endif

  // Need to offset x0, y0 and z0 as well.
  vnl_matrix_fixed<double, 4, 4> T;
  T.fill(0.0);
  T[0][0] = 1.0;
  T[1][1] = 1.0;
  T[2][2] = 1.0;
  T[3][3] = 1.0;
  T[0][3] = -nx0;
  T[1][3] = -ny0;
  T[2][3] = -nz0;
  pmatrix = Kr * pmatrix * T;
#if 0
  std::cout << "P Matrix\n" << pmatrix << '\n';
#endif
  camera.set_matrix(pmatrix);
  return true;
}

//:obtain a scaling transformation to normalize world geographic coordinates
// The resulting values will be on the range [-1, 1]
// The transform is valid anywhere the rational camera is valid
vgl_h_matrix_3d<double>
vpgl_proj_camera_convert::norm_trans(vpgl_rational_camera<double> const & rat_cam)
{
  double xscale = rat_cam.scale(vpgl_rational_camera<double>::X_INDX);
  double xoff = rat_cam.offset(vpgl_rational_camera<double>::X_INDX);
  double yscale = rat_cam.scale(vpgl_rational_camera<double>::Y_INDX);
  double yoff = rat_cam.offset(vpgl_rational_camera<double>::Y_INDX);
  double zscale = rat_cam.scale(vpgl_rational_camera<double>::Z_INDX);
  double zoff = rat_cam.offset(vpgl_rational_camera<double>::Z_INDX);
  vgl_h_matrix_3d<double> T;
  T.set_identity();
  T.set(0, 0, 1 / xscale);
  T.set(1, 1, 1 / yscale);
  T.set(2, 2, 1 / zscale);
  T.set(0, 3, -xoff / xscale);
  T.set(1, 3, -yoff / yscale);
  T.set(2, 3, -zoff / zscale);
  return T;
}


bool
vpgl_perspective_camera_convert::convert(vpgl_rational_camera<double> const & rat_cam,
                                         vgl_box_3d<double> const & approximation_volume,
                                         vpgl_perspective_camera<double> & camera,
                                         vgl_h_matrix_3d<double> & norm_trans)
{
  vpgl_scale_offset<double> sou = rat_cam.scl_off(vpgl_rational_camera<double>::U_INDX);
  vpgl_scale_offset<double> sov = rat_cam.scl_off(vpgl_rational_camera<double>::V_INDX);
  vpgl_scale_offset<double> sox = rat_cam.scl_off(vpgl_rational_camera<double>::X_INDX);
  vpgl_scale_offset<double> soy = rat_cam.scl_off(vpgl_rational_camera<double>::Y_INDX);
  vpgl_scale_offset<double> soz = rat_cam.scl_off(vpgl_rational_camera<double>::Z_INDX);
  auto ni = static_cast<unsigned>(2 * sou.scale()); //# image columns
  auto nj = static_cast<unsigned>(2 * sov.scale()); //# image rows
  norm_trans.set_identity();
  norm_trans.set(0, 0, 1 / sox.scale());
  norm_trans.set(1, 1, 1 / soy.scale());
  norm_trans.set(2, 2, 1 / soz.scale());
  norm_trans.set(0, 3, -sox.offset() / sox.scale());
  norm_trans.set(1, 3, -soy.offset() / soy.scale());
  norm_trans.set(2, 3, -soz.offset() / soz.scale());

  vgl_point_3d<double> minp = approximation_volume.min_point();
  vgl_point_3d<double> maxp = approximation_volume.max_point();
  double xmin = minp.x(), ymin = minp.y(), zmin = minp.z();
  double xrange = maxp.x() - xmin, yrange = maxp.y() - ymin, zrange = maxp.z() - zmin;
  if (xrange < 0 || yrange < 0 || zrange < 0)
    return false;
  // Randomly generate points
  unsigned n = 100;
  std::vector<vgl_point_3d<double>> world_pts;
  unsigned count = 0, ntrials = 0;
  while (count < n)
  {
    ntrials++;
    double rx = xrange * (std::rand() / (RAND_MAX + 1.0));
    double ry = yrange * (std::rand() / (RAND_MAX + 1.0));
    double rz = zrange * (std::rand() / (RAND_MAX + 1.0));
    vgl_point_3d<double> wp(xmin + rx, ymin + ry, zmin + rz);
    vgl_point_2d<double> ip = rat_cam.project(wp);
    if (ip.x() < 0 || ip.x() > ni || ip.y() < 0 || ip.y() > nj)
      continue;
    world_pts.push_back(wp);
    count++;
  }
  std::cout << "Ntrials " << ntrials << '\n';

  // Normalize world and image points to the range [-1,1]
  std::vector<vgl_point_3d<double>> norm_world_pts;
  std::vector<vgl_point_2d<double>> image_pts, norm_image_pts;
  auto N = static_cast<unsigned int>(world_pts.size());
  for (unsigned i = 0; i < N; ++i)
  {
    vgl_point_3d<double> wp = world_pts[i];
    vgl_point_2d<double> ip = rat_cam.project(wp);
    image_pts.push_back(ip);
    vgl_point_2d<double> nip(sou.normalize(ip.x()), sov.normalize(ip.y()));
    norm_image_pts.push_back(nip);
    vgl_point_3d<double> nwp(sox.normalize(wp.x()), soy.normalize(wp.y()), soz.normalize(wp.z()));
    norm_world_pts.push_back(nwp);
  }
  // Assume identity calibration matrix initially, since image point
  // normalization remove any scale and offset from image coordinates
  vnl_matrix_fixed<double, 3, 3> kk;
  kk.fill(0);
  kk[0][0] = 1.0;
  kk[1][1] = 1.0;
  kk[2][2] = 1.0;
  // Convert solution for rotation and translation and calibration matrix of
  // the perspective camera
  vpgl_calibration_matrix<double> K(kk);
  if (!vpgl_perspective_camera_compute::compute(norm_image_pts, norm_world_pts, K, camera))
    return false;
  std::cout << camera << '\n';
  // form the full camera by premultiplying by the image normalization
  vpgl_calibration_matrix<double> Kmin = camera.get_calibration();
  vnl_matrix_fixed<double, 3, 3> kk_min;
  kk_min = Kmin.get_matrix();
  kk[0][0] = sou.scale();
  kk[0][2] = sou.offset();
  kk[1][1] = sov.scale();
  kk[1][2] = sov.offset();
  kk *= kk_min;
  camera.set_calibration(kk);

  // project the points approximated
  double err_max = 0, err_min = 1e10;
  vgl_point_3d<double> min_pt, max_pt;
  for (unsigned i = 0; i < N; ++i)
  {
    vgl_point_3d<double> nwp = norm_world_pts[i];
    double U, V;
    camera.project(nwp.x(), nwp.y(), nwp.z(), U, V);
    vgl_point_2d<double> ip = image_pts[i];
    double error = std::sqrt((ip.x() - U) * (ip.x() - U) + (ip.y() - V) * (ip.y() - V));
    if (error > err_max)
    {
      err_max = error;
      max_pt = world_pts[i];
    }
    if (error < err_min)
    {
      err_min = error;
      min_pt = world_pts[i];
    }
  }
  std::cout << "Max Error = " << err_max << " at " << max_pt << '\n'
            << "Min Error = " << err_min << " at " << min_pt << '\n'
            << "final cam\n"
            << camera << '\n';
  return true;
}


bool
vpgl_perspective_camera_convert::convert_local(vpgl_rational_camera<double> const & rat_cam,
                                               vgl_box_3d<double> const & approximation_volume,
                                               vpgl_perspective_camera<double> & camera,
                                               vgl_h_matrix_3d<double> & norm_trans)
{
  // Set up the geo converter.
  double lon_low = approximation_volume.min_x();
  double lat_low = approximation_volume.min_y();
#ifdef DEBUG
  double lon_high = approximation_volume.max_x();
  double lat_high = approximation_volume.max_y();
  assert(lat_low < lat_high && lon_low < lon_high);
#endif
  vpgl_lvcs lvcs_converter(lat_low,
                           lon_low,
                           .5 * (approximation_volume.min_z() + approximation_volume.max_z()),
                           vpgl_lvcs::wgs84,
                           vpgl_lvcs::DEG);

  // Get a new local bounding box.
  double min_lx = 1e99, min_ly = 1e99, min_lz = 1e99, max_lx = 0, max_ly = 0, max_lz = 0;
  for (int cx = 0; cx < 2; ++cx)
  {
    for (int cy = 0; cy < 2; ++cy)
    {
      for (int cz = 0; cz < 2; ++cz)
      {
        vgl_point_3d<double> wc(approximation_volume.min_x() * cx + approximation_volume.max_x() * (1 - cx),
                                approximation_volume.min_y() * cy + approximation_volume.max_y() * (1 - cy),
                                approximation_volume.min_z() * cz + approximation_volume.max_z() * (1 - cz));
        double lcx, lcy, lcz;
        lvcs_converter.global_to_local(wc.x(), wc.y(), wc.z(), vpgl_lvcs::wgs84, lcx, lcy, lcz);
        vgl_point_3d<double> wc_loc(lcx, lcy, lcz);
        if (cx == 0 && cy == 0 && cz == 0)
        {
          min_lx = wc_loc.x();
          max_lx = wc_loc.x();
          min_ly = wc_loc.y();
          max_ly = wc_loc.y();
          min_lz = wc_loc.z();
          max_lz = wc_loc.z();
          continue;
        }
        if (wc_loc.x() < min_lx)
          min_lx = wc_loc.x();
        if (wc_loc.y() < min_ly)
          min_ly = wc_loc.y();
        if (wc_loc.z() < min_lz)
          min_lz = wc_loc.z();
        if (wc_loc.x() > max_lx)
          max_lx = wc_loc.x();
        if (wc_loc.y() > max_ly)
          max_ly = wc_loc.y();
        if (wc_loc.z() > max_lz)
          max_lz = wc_loc.z();
      }
    }
  }
  double dlx = max_lx - min_lx, dly = max_ly - min_ly, dlz = max_lz - min_lz;

  norm_trans.set_identity();
  norm_trans.set(0, 0, 2 / dlx);
  norm_trans.set(1, 1, 2 / dly);
  norm_trans.set(2, 2, 2 / dlz);
  norm_trans.set(0, 3, -1 - 2 * min_lx / dlx);
  norm_trans.set(1, 3, -1 - 2 * min_ly / dly);
  norm_trans.set(2, 3, -1 - 2 * min_lz / dlz);

  vpgl_scale_offset<double> sou = rat_cam.scl_off(vpgl_rational_camera<double>::U_INDX);
  vpgl_scale_offset<double> sov = rat_cam.scl_off(vpgl_rational_camera<double>::V_INDX);
#if 0 // unused
  vpgl_scale_offset<double> sox =
    rat_cam.scl_off(vpgl_rational_camera<double>::X_INDX);
  vpgl_scale_offset<double> soy =
    rat_cam.scl_off(vpgl_rational_camera<double>::Y_INDX);
  vpgl_scale_offset<double> soz =
    rat_cam.scl_off(vpgl_rational_camera<double>::Z_INDX);
#endif
  auto ni = static_cast<unsigned>(2 * sou.scale()); //# image columns
  auto nj = static_cast<unsigned>(2 * sov.scale()); //# image rows

  vgl_point_3d<double> minp = approximation_volume.min_point();
  vgl_point_3d<double> maxp = approximation_volume.max_point();
  double xmin = minp.x(), ymin = minp.y(), zmin = minp.z();
  double xrange = maxp.x() - xmin, yrange = maxp.y() - ymin, zrange = maxp.z() - zmin;
  if (xrange < 0 || yrange < 0 || zrange < 0)
    return false;
  // Randomly generate points
  unsigned n = 100;
  std::vector<vgl_point_3d<double>> world_pts;
  unsigned count = 0, ntrials = 0;
  while (count < n)
  {
    ++ntrials;
    double rx = xrange * (std::rand() / (RAND_MAX + 1.0));
    double ry = yrange * (std::rand() / (RAND_MAX + 1.0));
    double rz = zrange * (std::rand() / (RAND_MAX + 1.0));
    vgl_point_3d<double> wp(xmin + rx, ymin + ry, zmin + rz);
    vgl_point_2d<double> ip = rat_cam.project(wp);
    if (ip.x() < 0 || ip.x() > ni || ip.y() < 0 || ip.y() > nj)
      continue;
    world_pts.push_back(wp);
    count++;
  }
  std::cout << "Ntrials " << ntrials << '\n';

  // Normalize world and image points to the range [-1,1]
  std::vector<vgl_point_3d<double>> norm_world_pts;
  std::vector<vgl_point_2d<double>> image_pts, norm_image_pts;
  auto N = static_cast<unsigned int>(world_pts.size());
  for (unsigned i = 0; i < N; ++i)
  {
    vgl_point_3d<double> wp = world_pts[i];
    vgl_point_2d<double> ip = rat_cam.project(wp);
    image_pts.push_back(ip);
    vgl_point_2d<double> nip(sou.normalize(ip.x()), sov.normalize(ip.y()));
    norm_image_pts.push_back(nip);

    // Convert to local coords.
    double lcx, lcy, lcz;
    lvcs_converter.global_to_local(wp.x(), wp.y(), wp.z(), vpgl_lvcs::wgs84, lcx, lcy, lcz);
    vgl_homg_point_3d<double> wp_loc(lcx, lcy, lcz);

    vgl_homg_point_3d<double> nwp = norm_trans * wp_loc;
    assert(std::fabs(nwp.x()) <= 1 && std::fabs(nwp.y()) <= 1 && std::fabs(nwp.z()) <= 1);
    norm_world_pts.emplace_back(nwp);
  }
  // Assume identity calibration matrix initially, since image point
  // normalization remove any scale and offset from image coordinates
  vnl_matrix_fixed<double, 3, 3> kk;
  kk.fill(0);
  kk[0][0] = 1.0;
  kk[1][1] = 1.0;
  kk[2][2] = 1.0;
  // Convert solution for rotation and translation and calibration matrix of
  // the perspective camera
  vpgl_calibration_matrix<double> K(kk);
  if (!vpgl_perspective_camera_compute::compute(norm_image_pts, norm_world_pts, K, camera))
    return false;
  std::cout << camera << '\n';
  // form the full camera by premultiplying by the image normalization
  vpgl_calibration_matrix<double> Kmin = camera.get_calibration();
  vnl_matrix_fixed<double, 3, 3> kk_min;
  kk_min = Kmin.get_matrix();
  kk[0][0] = sou.scale();
  kk[0][2] = sou.offset();
  kk[1][1] = sov.scale();
  kk[1][2] = sov.offset();
  kk *= kk_min;
  camera.set_calibration(kk);

  // project the points approximated
  double err_max = 0, err_min = 1e10;
  vgl_point_3d<double> min_pt, max_pt;
  for (unsigned i = 0; i < N; ++i)
  {
    vgl_point_3d<double> nwp = norm_world_pts[i];
    double U, V;
    camera.project(nwp.x(), nwp.y(), nwp.z(), U, V);
    vgl_point_2d<double> ip = image_pts[i];
    double error = std::sqrt((ip.x() - U) * (ip.x() - U) + (ip.y() - V) * (ip.y() - V));
    if (error > err_max)
    {
      err_max = error;
      max_pt = world_pts[i];
    }
    if (error < err_min)
    {
      err_min = error;
      min_pt = world_pts[i];
    }
  }
  std::cout << "Max Error = " << err_max << " at " << max_pt << '\n'
            << "Min Error = " << err_min << " at " << min_pt << '\n'
            << "final cam\n"
            << camera << '\n';
  return true;
}

//
// linear interpolation based on a set of 4 neighboring rays
//          X
//        X r X
//          X
// ray r at the center pixel is interpolated from the neighboring X's
// as shown. This method is used to test if ray interpolation
// is sufficiently accurate
//
static bool
interp_ray(std::vector<vgl_ray_3d<double>> const & ray_nbrs, vgl_ray_3d<double> & intrp_ray)
{
  auto nrays = static_cast<unsigned int>(ray_nbrs.size());
  if (nrays != 4)
    return false;
  vgl_ray_3d<double> r0 = ray_nbrs[0], r1 = ray_nbrs[1];
  vgl_ray_3d<double> r2 = ray_nbrs[2], r3 = ray_nbrs[3];
  vgl_point_3d<double> org0 = r0.origin(), org1 = r1.origin();
  vgl_point_3d<double> org2 = r2.origin(), org3 = r3.origin();
  vgl_vector_3d<double> dir0 = r0.direction(), dir1 = r1.direction();
  vgl_vector_3d<double> dir2 = r2.direction(), dir3 = r3.direction();
  // first order partial derivatives
  vgl_vector_3d<double> dodu = 0.5 * (org2 - org1);
  vgl_vector_3d<double> dodv = 0.5 * (org3 - org0);
  vgl_vector_3d<double> dddu = 0.5 * (dir2 - dir1);
  vgl_vector_3d<double> dddv = 0.5 * (dir3 - dir0);
  vgl_point_3d<double> temp = org1 + dodu + dodv;
  double ox = 0.5 * (org0.x() + temp.x());
  double oy = 0.5 * (org0.y() + temp.y());
  double oz = 0.5 * (org0.z() + temp.z());
  vgl_point_3d<double> iorg(ox, oy, oz);
  vgl_vector_3d<double> idir = 0.5 * (dir1 + dddu + dir0 + dddv);
  intrp_ray.set(iorg, idir);
  return true;
}

// convert tolerances on ray origin and ray direction to test interpolation
static bool
ray_tol(vpgl_local_rational_camera<double> const & rat_cam,
        double mid_u,
        double mid_v,
        vgl_plane_3d<double> const & high,
        vgl_point_3d<double> const & high_guess,
        vgl_plane_3d<double> const & low,
        vgl_point_3d<double> const & low_guess,
        double & org_tol,
        double & dir_tol)
{
  vgl_point_2d<double> ip(mid_u, mid_v);
  vgl_point_3d<double> high_pt, low_pt, low_pt_pix;
  if (!vpgl_backproject::bproj_plane(rat_cam, ip, high, high_guess, high_pt))
    return false;
  if (!vpgl_backproject::bproj_plane(rat_cam, ip, low, low_guess, low_pt))
    return false;
  // move 1 pixel
  ip.set(mid_u + 1.0, mid_v + 1.0);
  if (!vpgl_backproject::bproj_plane(rat_cam, ip, low, low_pt, low_pt_pix))
    return false;
  // Ground Sampling Distance
  double gsd = (low_pt - low_pt_pix).length();
  // tolerance
  double tfact = 0.001;
  org_tol = tfact * gsd;
  vgl_vector_3d<double> dir0 = low_pt - high_pt;
  vgl_vector_3d<double> dir1 = low_pt_pix - high_pt;
  double ang = angle(dir0, dir1);
  dir_tol = tfact * ang;
  return true;
}

// produce rays at sub pixel locations by interpolating four neighbors
//
//            X  <- r0
//         _______
//        | o   o |
// r1-> X |       | X <-r2
//        | o   o |
//         -------
//            X <- r3
// the interporlated rays are indicated by the o's
// the neighbor rays are shown as X's
// This method is used to populate higher resolution layers of the
// ray pyramid

bool
vpgl_generic_camera_convert::upsample_rays(std::vector<vgl_ray_3d<double>> const & ray_nbrs,
                                           vgl_ray_3d<double> const & ray,
                                           std::vector<vgl_ray_3d<double>> & interp_rays)
{
  auto nrays = static_cast<unsigned int>(ray_nbrs.size());
  if (nrays != 4)
    return false;
  vgl_ray_3d<double> r00 = ray_nbrs[0], r01 = ray_nbrs[1];
  vgl_ray_3d<double> r10 = ray_nbrs[2], r11 = ray_nbrs[3];
  vgl_point_3d<double> org00 = r00.origin(), org01 = r01.origin();
  vgl_point_3d<double> org10 = r10.origin(), org11 = r11.origin();
  vgl_vector_3d<double> dir00 = r00.direction(), dir01 = r01.direction();
  vgl_vector_3d<double> dir10 = r10.direction(), dir11 = r11.direction();

  // first sub ray
  interp_rays[0] = ray;

  // second sub ray
  vgl_point_3d<double> iorg = org00 + (org01 - org00) * 0.5;
  vgl_vector_3d<double> idir = dir00 * 0.5 + dir01 * 0.5;
  interp_rays[1].set(iorg, idir);

  // third sub ray
  iorg = org00 + (org10 - org00) * 0.5;
  idir = 0.5 * dir00 + 0.5 * dir10;
  interp_rays[2].set(iorg, idir);

  // fourth sub ray
  iorg = org00 + 0.25 * (org01 - org00) + 0.25 * (org10 - org00) + 0.25 * (org11 - org00);
  idir = 0.25 * dir00 + 0.25 * dir01 + 0.25 * dir10 + 0.25 * dir11;
  interp_rays[3].set(iorg, idir);

  return true;
}

// r0 and r1 are rays spaced a unit grid distane apart (either row or col)
// r is the interpolated ray at n_grid unit distances past r1
vgl_ray_3d<double>
vpgl_generic_camera_convert::interp_pair(vgl_ray_3d<double> const & r0, vgl_ray_3d<double> const & r1, double n_grid)
{
  vgl_vector_3d<double> v01 = r1.origin() - r0.origin();
  vgl_point_3d<double> intp_org = r1.origin() + n_grid * v01;
  vgl_vector_3d<double> d01 = r1.direction() - r0.direction();
  vgl_vector_3d<double> intp_dir = r1.direction() + n_grid * d01;
  return vgl_ray_3d<double>(intp_org, intp_dir);
}


//
// convert a generic camera from a local rational camera
// the approach is to form a pyramid and convert rays by
// back-projecting to top and bottom planes of the valid
// elevations of the rational camera. Successive higher resolution
// layers of the pyramid are populated until the interpolation of
// a ray by the four adjacent neighbors is sufficiently accurate
// The remaining layers of the pyramid are filled in by interpolation
//
bool
vpgl_generic_camera_convert::convert(vpgl_local_rational_camera<double> const & rat_cam,
                                     int ni,
                                     int nj,
                                     vpgl_generic_camera<double> & gen_cam,
                                     unsigned level)
{
  // get z bounds
  double zoff = rat_cam.offset(vpgl_rational_camera<double>::Z_INDX);
  double zscl = rat_cam.scale(vpgl_rational_camera<double>::Z_INDX);
  // construct high and low planes
  // NOTE: z_scale seems to usually be much larger than actual dimensions of scene, which
  //       sometimes causes trouble for vpgl_backproj_plane, which causes the conversion to fail.
  //       Using half scale value for now, but maybe we should consider taking "top" and "bottom"
  //       z values as user-specified inputs.  -dec 15 Nov 2011
  double el_low = zoff - zscl / 2;
  double el_high = zoff + zscl / 2;
  double lon = rat_cam.offset(vpgl_rational_camera<double>::X_INDX);
  double lat = rat_cam.offset(vpgl_rational_camera<double>::Y_INDX);
  double x, y, z_low, z_high;
  // convert high and low elevations to local z values
  rat_cam.lvcs().global_to_local(lon, lat, el_low, vpgl_lvcs::wgs84, x, y, z_low, vpgl_lvcs::DEG);
  rat_cam.lvcs().global_to_local(lon, lat, el_high, vpgl_lvcs::wgs84, x, y, z_high, vpgl_lvcs::DEG);
  return convert(rat_cam, ni, nj, gen_cam, z_low, z_high, level);
}

//
// convert a generic camera from a local rational camera
// the approach is to form a pyramid and convert rays by
// back-projecting to top and bottom planes of the valid
// elevations of the rational camera. Successive higher resolution
// layers of the pyramid are populated until the interpolation of
// a ray by the four adjacent neighbors is sufficiently accurate
// The remaining layers of the pyramid are filled in by interpolation
//
#if 0
bool vpgl_generic_camera_convert::
convert( vpgl_local_rational_camera<double> const& rat_cam,
         int ni, int nj, vpgl_generic_camera<double> & gen_cam,
         double local_z_min, double local_z_max, unsigned level)
{
  vgl_plane_3d<double> high(0.0, 0.0, 1.0, -local_z_max);
  vgl_plane_3d<double> low(0.0, 0.0, 1.0, -local_z_min);

  // initial guess for backprojection to planes
  vgl_point_3d<double> org(0.0, 0.0, local_z_max), endpt(0.0, 0.0, local_z_min);
  // initialize the ray pyramid
  // convert the required number of levels
  double dim = ni;
  if (nj<ni)
    dim = nj;
  double lv = std::log(dim)/std::log(2.0);
  int n_levels = static_cast<int>(lv+1.0);// round up
  if (dim*std::pow(0.5, static_cast<double>(n_levels-1)) < 3.0) n_levels--;
  // construct pyramid of ray indices
  // the row and column dimensions at each level
  std::vector<int> nr(n_levels,0), nc(n_levels,0);
  // the scale factor at each level (to transform back to level 0)
  std::vector<double> scl(n_levels,1.0);
  std::vector<vbl_array_2d<vgl_ray_3d<double> > > ray_pyr(n_levels);
  ray_pyr[0].resize(nj, ni);
  ray_pyr[0].fill(vgl_ray_3d<double>(vgl_point_3d<double>(0,0,0),vgl_vector_3d<double>(0,0,1)));
  nr[0]=nj;   nc[0]=ni; scl[0]=1.0;
  std::cout<<"(ni,nj)"<<ni<<","<<nj<<std::endl;
  int di = (ni+1)/2 +1,
      dj = (nj+1)/2 +1;

  for (int i=1; i<n_levels; ++i)
  {
    std::cout<<"(di,dj)"<<di<<","<<dj<<std::endl;
    ray_pyr[i].resize(dj,di);
    ray_pyr[i].fill(vgl_ray_3d<double>(vgl_point_3d<double>(0,0,0),
                                       vgl_vector_3d<double>(0,0,1)));
    nr[i]=dj;
    nc[i]=di;
    scl[i]=2.0*scl[i-1];
    di = (di+1)/2 +1 ;
    dj = (dj+1)/2 +1;

  }
  // convert the ray interpolation tolerances
  double org_tol = 0.0;
  double ang_tol = 0.0;
  double max_org_err = 0.0, max_ang_err = 0.0;
  if (!ray_tol(rat_cam, ni/2.0, nj/2.0, high,
               org, low, endpt, org_tol, ang_tol))
    return false;
  bool need_interp = true;
  int lev = n_levels-1;
  for (; lev>=0&&need_interp; --lev) {
    // set rays at current pyramid level
    for (int j =0; j<nr[lev]; ++j) {
      int sj = static_cast<int>(scl[lev]*j);
      if (sj>=nj)
          sj = nj-1;
      for (int i =0;i<nc[lev]; ++i)
      {
        int si = static_cast<int>(scl[lev]*i);
        if (si>=ni)
            si = ni-1;
        vgl_point_2d<double> ip(si,sj);
        vgl_point_3d<double> prev_org(0.0,0.0,local_z_max);
        vgl_point_3d<double> prev_endpt(0.0, 0.0, local_z_min);
        // initialize guess with
        if (lev < n_levels-1) {
          double rel_scale = scl[lev]/scl[lev+1];
          int i_above = static_cast<int>(rel_scale * i);
          int j_above = static_cast<int>(rel_scale * j);

          prev_org = ray_pyr[lev+1][j_above][i_above].origin();
          vgl_vector_3d<double> prev_dir = ray_pyr[lev+1][j_above][i_above].direction();
          // find endpoint
          double ray_len = (local_z_min - prev_org.z()) / prev_dir.z();
          prev_endpt = prev_org + (prev_dir * ray_len);
        }
        constexpr double error_tol = 0.25; // allow projection error of 0.25 pixel
        if (!vpgl_backproject::bproj_plane(&rat_cam, ip, high, prev_org, org, error_tol))
          return false;
        if (!vpgl_backproject::bproj_plane(&rat_cam, ip, low, prev_endpt, endpt, error_tol))
          return false;

        vgl_vector_3d<double> dir = endpt-org;
        ray_pyr[lev][j][i].set(org, dir);
      }
    }

    // check for interpolation accuracy at the current level
    // scan through the array and find largest discrepancy in
    // ray origin and ray direction
    need_interp = false;
    max_org_err = 0.0; max_ang_err = 0.0;
    std::vector<vgl_ray_3d<double> > ray_nbrs(4);

    for (int j =1; (j<nr[lev]-1)&&!need_interp; ++j) {
      for (int i =1;(i<nc[lev]-1)&&!need_interp; ++i) {
        vgl_ray_3d<double> ray = ray_pyr[lev][j][i];
        //
        //collect 4-neighbors of ray
        //
        //        0
        //      1 x 2
        //        3
        //
        ray_nbrs[0]=ray_pyr[lev][j-1][i];
        ray_nbrs[1]=ray_pyr[lev][j][i-1];
        ray_nbrs[2]=ray_pyr[lev][j][i+1];
        ray_nbrs[3]=ray_pyr[lev][j+1][i];
        //interpolate using neighbors
        vgl_ray_3d<double> intp_ray;
        if (!interp_ray(ray_nbrs, intp_ray))
          return false;
        double dorg = (ray.origin()-intp_ray.origin()).length();
        double dang = angle(ray.direction(), intp_ray.direction());
        if (dorg>max_org_err) max_org_err = dorg;
        if (dang>max_ang_err) max_ang_err = dang;
        need_interp = max_org_err>org_tol || max_ang_err>ang_tol;
        if(need_interp)
            std::cout<<lev<<", "<<i<<","<<j<<std::endl;
      }
    }
  }
  // found level where interpolation is within tolerance
  // fill in values at lower levels
  for (++lev; lev>0; --lev) {
    unsigned int ncr = nc[lev];
    unsigned int nrb = nr[lev];
    vbl_array_2d<vgl_ray_3d<double> >& clev = ray_pyr[lev];
    vbl_array_2d<vgl_ray_3d<double> >& nlev = ray_pyr[lev-1];
    std::vector<vgl_ray_3d<double> > ray_nbrs(4);
    std::vector<vgl_ray_3d<double> > interp_rays(4);
    for (unsigned int j = 0; j<nrb; ++j)
      for (unsigned int i = 0; i<ncr; ++i) {
        ray_nbrs[0] = clev[j][i];
        ray_nbrs[1] = clev[j][i];
        ray_nbrs[2] = clev[j][i];
        ray_nbrs[3] = clev[j][i];
        if (i+1<ncr) ray_nbrs[1] = clev[j][i+1];
        if (j+1<nrb) ray_nbrs[2] = clev[j+1][i];
        if (i+1<ncr && j+1<nrb) ray_nbrs[3] = clev[j+1][i+1];
        if (!upsample_rays(ray_nbrs, clev[j][i], interp_rays))
          return false;
        if (2*i<nlev.cols() && 2*j<nlev.rows())
        {
          nlev[2*j][2*i]    =interp_rays[0];
          if (2*i+1<nlev.cols())                      nlev[2*j][2*i+1]  =interp_rays[1];
          if (2*j+1<nlev.rows())                      nlev[2*j+1][2*i]  =interp_rays[2];
          if (2*i+1<nlev.cols() && 2*j+1<nlev.rows()) nlev[2*j+1][2*i+1]=interp_rays[3];
        }
      }
  }
  if ((int)level < n_levels) {
    gen_cam = vpgl_generic_camera<double>(ray_pyr,nr,nc);
    return true;
  }
  else
    return false;
}
#endif


bool
vpgl_generic_camera_convert::pyramid_est(vpgl_local_rational_camera<double> const & rat_cam,
                                         int ni,
                                         int nj,
                                         int offseti,
                                         int offsetj,
                                         double local_z_min,
                                         double local_z_max,
                                         int n_levels,
                                         std::vector<int> nr,
                                         std::vector<int> nc,
                                         std::vector<unsigned int> scl,
                                         std::vector<vbl_array_2d<vgl_ray_3d<double>>> & ray_pyr)

{
  vgl_plane_3d<double> high(0.0, 0.0, 1.0, -local_z_max);
  vgl_plane_3d<double> low(0.0, 0.0, 1.0, -local_z_min);

  // initial guess for backprojection to planes
  vgl_point_3d<double> org(0.0, 0.0, local_z_max), endpt(0.0, 0.0, local_z_min);
  // convert the ray interpolation tolerances
  double org_tol = 0.0;
  double ang_tol = 0.0;
  double max_org_err = 0.0, max_ang_err = 0.0;
  if (!ray_tol(rat_cam, offseti + ni / 2.0, offsetj + nj / 2.0, high, org, low, endpt, org_tol, ang_tol))
    return false;

  bool need_interp = true;
  int lev = n_levels - 1;
  // std::cout<<" lev "<<lev<<" ";
  for (; lev >= 0 && need_interp; --lev)
  {
    // set rays at current pyramid level
    for (int j = 0; j < nr[lev]; ++j)
    {
      int sj = offsetj + static_cast<int>(scl[lev] * j);
      // sj = (j == 0)? sj : sj -1;
      // if (sj>=gnj)
      //    sj =gnj;
      for (int i = 0; i < nc[lev]; ++i)
      {
        int si = offseti + static_cast<int>(scl[lev] * i);
        // si = (i == 0)? si : si -1;
        // if (si>=gni)
        //    si = gni;
        vgl_point_2d<double> ip(si, sj);
        vgl_point_3d<double> prev_org(0.0, 0.0, local_z_max);
        vgl_point_3d<double> prev_endpt(0.0, 0.0, local_z_min);
        // initialize guess with
        if (lev < n_levels - 1)
        {
          double rel_scale = scl[lev] / scl[lev + 1];
          int i_above = static_cast<int>(rel_scale * i);
          int j_above = static_cast<int>(rel_scale * j);
          prev_org = ray_pyr[lev + 1][j_above][i_above].origin();
          vgl_vector_3d<double> prev_dir = ray_pyr[lev + 1][j_above][i_above].direction();
          // find endpoint
          double ray_len = (local_z_min - prev_org.z()) / prev_dir.z();
          prev_endpt = prev_org + (prev_dir * ray_len);
        }
        constexpr double error_tol = 0.5; // allow projection error of 0.25 pixel
        if (!vpgl_backproject::bproj_plane(rat_cam, ip, high, prev_org, org, error_tol))
          return false;
        if (!vpgl_backproject::bproj_plane(rat_cam, ip, low, prev_endpt, endpt, error_tol))
          return false;
        vgl_vector_3d<double> dir = endpt - org;
        ray_pyr[lev][j][i].set(org, dir);
      }
    }
    // check for interpolation accuracy at the current level
    // scan through the array and find largest discrepancy in
    // ray origin and ray direction
    need_interp = false;
    max_org_err = 0.0;
    max_ang_err = 0.0;
    std::vector<vgl_ray_3d<double>> ray_nbrs(4);

    for (int j = 1; (j < nr[lev] - 1) && !need_interp; ++j)
    {
      for (int i = 1; (i < nc[lev] - 1) && !need_interp; ++i)
      {
        vgl_ray_3d<double> ray = ray_pyr[lev][j][i];
        //
        // collect 4-neighbors of ray
        //
        //        0
        //      1 x 2
        //        3
        //
        ray_nbrs[0] = ray_pyr[lev][j - 1][i];
        ray_nbrs[1] = ray_pyr[lev][j][i - 1];
        ray_nbrs[2] = ray_pyr[lev][j][i + 1];
        ray_nbrs[3] = ray_pyr[lev][j + 1][i];
        // interpolate using neighbors
        vgl_ray_3d<double> intp_ray;
        if (!interp_ray(ray_nbrs, intp_ray))
          return false;
        double dorg = (ray.origin() - intp_ray.origin()).length();
        double dang = angle(ray.direction(), intp_ray.direction());
        if (dorg > max_org_err)
          max_org_err = dorg;
        if (dang > max_ang_err)
          max_ang_err = dang;
        need_interp = max_org_err > org_tol || max_ang_err > ang_tol;
#if 0
              if(need_interp)
                  std::cout<<lev<<", "<<i<<","<<j<<std::endl;
#endif
      }
    }
  }
  // found level where interpolation is within tolerance
  // fill in values at lower levels
  for (++lev; lev > 0; --lev)
  {
    unsigned int ncr = nc[lev];
    unsigned int nrb = nr[lev];
    vbl_array_2d<vgl_ray_3d<double>> & clev = ray_pyr[lev];
    vbl_array_2d<vgl_ray_3d<double>> & nlev = ray_pyr[lev - 1];
    std::vector<vgl_ray_3d<double>> ray_nbrs(4);
    std::vector<vgl_ray_3d<double>> interp_rays(4);
    for (unsigned int j = 0; j < nrb; ++j)
    {
      for (unsigned int i = 0; i < ncr; ++i)
      {
        ray_nbrs[0] = clev[j][i];
        ray_nbrs[1] = clev[j][i];
        ray_nbrs[2] = clev[j][i];
        ray_nbrs[3] = clev[j][i];
        if (i + 1 < ncr)
          ray_nbrs[1] = clev[j][i + 1];
        if (j + 1 < nrb)
          ray_nbrs[2] = clev[j + 1][i];
        if (i + 1 < ncr && j + 1 < nrb)
          ray_nbrs[3] = clev[j + 1][i + 1];
        if (!upsample_rays(ray_nbrs, clev[j][i], interp_rays))
          return false;
        if (2 * i < nlev.cols() && 2 * j < nlev.rows())
        {
          nlev[2 * j][2 * i] = interp_rays[0];
          if (2 * i + 1 < nlev.cols())
            nlev[2 * j][2 * i + 1] = interp_rays[1];
          if (2 * j + 1 < nlev.rows())
            nlev[2 * j + 1][2 * i] = interp_rays[2];
          if (2 * i + 1 < nlev.cols() && 2 * j + 1 < nlev.rows())
            nlev[2 * j + 1][2 * i + 1] = interp_rays[3];
        }
      }
    }
  }
  return true;
}
#if 1
//: Implementation of breaking up images in 256x256 blocks
bool
vpgl_generic_camera_convert::convert(vpgl_local_rational_camera<double> const & rat_cam,
                                     int gni,
                                     int gnj,
                                     vpgl_generic_camera<double> & gen_cam,
                                     double local_z_min,
                                     double local_z_max,
                                     unsigned level)
{
  vgl_plane_3d<double> high(0.0, 0.0, 1.0, -local_z_max);
  vgl_plane_3d<double> low(0.0, 0.0, 1.0, -local_z_min);

  // initial guess for backprojection to planes
  vgl_point_3d<double> org(0.0, 0.0, local_z_max), endpt(0.0, 0.0, local_z_min);
  // initialize the ray pyramid
  // convert the required number of levels

  unsigned int mindim = gni < gnj ? gni : gnj;
  unsigned int factor = 256;
  unsigned int n_levels = 6;

  while (mindim < factor)
  {
    factor /= 2;
    n_levels--;

    std::cout << "." << std::endl;
  }
  unsigned int numi = gni / factor;
  unsigned int numj = gnj / factor;

  // construct pyramid of ray indices
  // the row and column dimensions at each level
  std::vector<int> nr(n_levels, 0), nc(n_levels, 0);
  vbl_array_2d<vgl_ray_3d<double>> finalrays(gnj, gni);

  std::vector<unsigned int> scl(n_levels);
  unsigned int running_scale = 1;
  std::vector<vbl_array_2d<vgl_ray_3d<double>>> ray_pyr(n_levels);
  for (unsigned int i = 0; i < n_levels; i++)
  {
    nr[i] = factor / running_scale + 1;
    nc[i] = factor / running_scale + 1;
    ray_pyr[i].resize(nr[i], nc[i]);
    ray_pyr[i].fill(vgl_ray_3d<double>(vgl_point_3d<double>(0, 0, 0), vgl_vector_3d<double>(0, 0, 1)));
    scl[i] = running_scale;
    running_scale *= 2;
    finalrays.fill(vgl_ray_3d<double>(vgl_point_3d<double>(0, 0, 0), vgl_vector_3d<double>(0, 0, 1)));
  }
  for (unsigned int bigi = 0; bigi <= numi; bigi++)
  {
    unsigned int ni = factor;
    unsigned int offseti = bigi * factor;
    if (bigi == numi)
      offseti = gni - factor;
    for (unsigned int bigj = 0; bigj <= numj; bigj++)
    {
      unsigned int nj = factor;
      unsigned int offsetj = bigj * factor;
      if (bigj == numj)
        offsetj = gnj - factor;
      // vpgl_generic_camera expects pixels centered at integer values (pyramid_est is agnostic;
      // although, as uints, offseti and offsetj will center pixels on integers)
      if (!vpgl_generic_camera_convert::pyramid_est(
            rat_cam, ni, nj, offseti, offsetj, local_z_min, local_z_max, n_levels, nr, nc, scl, ray_pyr))
        return false;
      for (unsigned int i = 0; i < factor; i++)
      {
        for (unsigned int j = 0; j < factor; j++)
        {
          if (j + offsetj < static_cast<unsigned>(gnj) && i + offseti < static_cast<unsigned>(gni))
            finalrays[j + offsetj][i + offseti] = ray_pyr[0][j][i];
        }
      }
    }
  }
  if (level < n_levels)
  {
    gen_cam = vpgl_generic_camera<double>(finalrays);
    return true;
  }
  else
    return false;
}
#endif
#if 1
bool
vpgl_generic_camera_convert::convert_bruteforce(vpgl_local_rational_camera<double> const & rat_cam,
                                                int gni,
                                                int gnj,
                                                vpgl_generic_camera<double> & gen_cam,
                                                double local_z_min,
                                                double local_z_max,
                                                unsigned /*level*/)
{
  vgl_plane_3d<double> high(0.0, 0.0, 1.0, -local_z_max);
  vgl_plane_3d<double> low(0.0, 0.0, 1.0, -local_z_min);

  // initial guess for backprojection to planes
  vgl_point_3d<double> org(0.0, 0.0, local_z_max), endpt(0.0, 0.0, local_z_min);
  // initialize the ray pyramid
  // convert the required number of levels
  vbl_array_2d<vgl_ray_3d<double>> finalrays(gnj, gni);
  for (unsigned i = 0; i < static_cast<unsigned>(gni); i++)
  {
    for (unsigned j = 0; j < static_cast<unsigned>(gnj); j++)
    {
      vgl_point_2d<double> ip(i, j);
      constexpr double error_tol = 0.5; // allow projection error of 0.25 pixel
      if (!vpgl_backproject::bproj_plane(rat_cam, ip, high, org, org, error_tol))
        return false;
      if (!vpgl_backproject::bproj_plane(rat_cam, ip, low, endpt, endpt, error_tol))
        return false;
      vgl_vector_3d<double> dir = endpt - org;
      finalrays[j][i].set(org, dir);
    }
  }

  gen_cam = vpgl_generic_camera<double>(finalrays);
  return true;
}
#endif
bool
vpgl_generic_camera_convert::convert(vpgl_proj_camera<double> const & prj_cam,
                                     int ni,
                                     int nj,
                                     vpgl_generic_camera<double> & gen_cam,
                                     unsigned level)
{
  vbl_array_2d<vgl_ray_3d<double>> rays(nj, ni);
  vgl_ray_3d<double> ray;
  vgl_homg_point_2d<double> ipt;
  double scale = (level < 32) ? double(1L << level) : std::pow(2.0, static_cast<double>(level));
  for (int j = 0; j < nj; ++j)
    for (int i = 0; i < ni; ++i)
    {
      ipt.set(i * scale, j * scale, 1.0);
      ray = prj_cam.backproject_ray(ipt);
      rays[j][i] = ray;
    }
  gen_cam = vpgl_generic_camera<double>(rays);
  return true;
}

bool
vpgl_generic_camera_convert::convert(vpgl_perspective_camera<double> const & per_cam,
                                     int ni,
                                     int nj,
                                     vpgl_generic_camera<double> & gen_cam,
                                     unsigned level)
{
  vbl_array_2d<vgl_ray_3d<double>> rays(nj, ni);
  vgl_ray_3d<double> ray;
  vgl_homg_point_2d<double> ipt;
  double scale = (level < 32) ? double(1L << level) : std::pow(2.0, static_cast<double>(level));
  for (int j = 0; j < nj; ++j)
    for (int i = 0; i < ni; ++i)
    {
      ipt.set(i * scale, j * scale, 1.0);
      ray = per_cam.backproject_ray(ipt);
      rays[j][i] = ray;
    }
  gen_cam = vpgl_generic_camera<double>(rays);
  return true;
}

bool
vpgl_generic_camera_convert::convert_with_margin(vpgl_perspective_camera<double> const & per_cam,
                                                 int ni,
                                                 int nj,
                                                 vpgl_generic_camera<double> & gen_cam,
                                                 int margin,
                                                 unsigned level)
{
  vbl_array_2d<vgl_ray_3d<double>> rays(nj + 2 * margin, ni + 2 * margin);
  vgl_ray_3d<double> ray;
  vgl_homg_point_2d<double> ipt;
  double scale = (level < 32) ? double(1L << level) : std::pow(2.0, static_cast<double>(level));
  for (int j = -margin; j < nj + margin; ++j)
    for (int i = -margin; i < ni + margin; ++i)
    {
      ipt.set(i * scale, j * scale, 1.0);
      ray = per_cam.backproject_ray(ipt);
      rays[j + margin][i + margin] = ray;
    }
  gen_cam = vpgl_generic_camera<double>(rays);
  return true;
}


// the affine camera defines a principal plane which is
// far enough from the scene origin so that all the scene
// geometry is in front of the plane. The backproject function
// constructs finite ray origins on the principal plane.
bool
vpgl_generic_camera_convert::convert(vpgl_affine_camera<double> const & aff_cam,
                                     int ni,
                                     int nj,
                                     vpgl_generic_camera<double> & gen_cam,
                                     unsigned level)
{
  double scale = (level < 32) ? double(1L << level) : std::pow(2.0, static_cast<double>(level));
  // The ray direction is the camera center (which is at inifnity)
  vgl_homg_point_3d<double> cent = aff_cam.camera_center();
  vgl_vector_3d<double> dir(cent.x(), cent.y(), cent.z());

  // get the principal plane, on which all ray origins will lie
  vgl_homg_plane_3d<double> plane = aff_cam.principal_plane();

  // compute the transformation from image coordinates to X,Y,Z on the principal plane
  vnl_matrix_fixed<double, 3, 4> P = aff_cam.get_matrix();
  double u0 = P(0, 3);
  double v0 = P(1, 3);

  vnl_matrix_fixed<double, 3, 3> A;
  for (unsigned j = 0; j < 3; ++j)
  {
    A(0, j) = P(0, j);
    A(1, j) = P(1, j);
  }
  A(2, 0) = plane.a();
  A(2, 1) = plane.b();
  A(2, 2) = plane.c();

  // invA maps (u-u0, v-v0, -d) to X,Y,Z on the principal plane
  vnl_matrix_fixed<double, 3, 3> invA{ vnl_svd<double>(A.as_ref()).inverse() };

  // construct the array of camera rays
  vgl_point_3d<double> org;
  ;
  vbl_array_2d<vgl_ray_3d<double>> rays(nj, ni);
  for (int j = 0; j < nj; ++j)
  {
    for (int i = 0; i < ni; ++i)
    {
      vnl_vector_fixed<double, 3> ipt(i * scale - u0, j * scale - v0, -plane.d());
      vnl_vector_fixed<double, 3> org_vnl = invA * ipt;
      // convert from vnl_vector to vgl_vector
      org.set(org_vnl[0], org_vnl[1], org_vnl[2]);
      rays[j][i].set(org, dir);
    }
  }
  gen_cam = vpgl_generic_camera<double>(rays);
  return true;
}


bool
vpgl_generic_camera_convert::convert(vpgl_camera_double_sptr const & camera,
                                     int ni,
                                     int nj,
                                     vpgl_generic_camera<double> & gen_cam,
                                     unsigned level)
{
  if (auto * cam = dynamic_cast<vpgl_local_rational_camera<double> *>(camera.ptr()))
    return vpgl_generic_camera_convert::convert(*cam, ni, nj, gen_cam, level);

  if (auto * cam = dynamic_cast<vpgl_perspective_camera<double> *>(camera.ptr()))
  {
    return vpgl_generic_camera_convert::convert(*cam, ni, nj, gen_cam, level);
  }

  if (auto * cam = dynamic_cast<vpgl_affine_camera<double> *>(camera.ptr()))
    return vpgl_generic_camera_convert::convert(*cam, ni, nj, gen_cam, level);

  if (auto * cam = dynamic_cast<vpgl_proj_camera<double> *>(camera.ptr()))
  {
    return vpgl_generic_camera_convert::convert(*cam, ni, nj, gen_cam, level);
  }

  return false;
}


#if HAS_GEOTIFF
//: Convert a geocam (transformtaion matrix read from a geotiff header + an lvcs) to a generic camera
bool
vpgl_generic_camera_convert::convert(vpgl_geo_camera & geocam,
                                     int ni,
                                     int nj,
                                     double height,
                                     vpgl_generic_camera<double> & gen_cam,
                                     unsigned level)
{
  double scale = (level < 32) ? double(1L << level) : std::pow(2.0, static_cast<double>(level));

  //: all rays have the same direction
  vgl_vector_3d<double> dir(0.0, 0.0, -1.0);

  vbl_array_2d<vgl_ray_3d<double>> rays(nj, ni);
  vgl_point_3d<double> org;

  for (int j = 0; j < nj; ++j)
    for (int i = 0; i < ni; ++i)
    {
      double x, y, z;
      geocam.backproject(i * scale, j * scale, x, y, z);
      org.set(x, y, height);
      rays[j][i].set(org, dir);
    }
  gen_cam = vpgl_generic_camera<double>(rays);
  return true;
}

//: Convert a geocam (transformtaion matrix read from a geotiff header + an lvcs) to a generic camera using the
// specified ray direction (not necessarily nadir rays)
//  basically creates a camera with parallel rays but the rays can be in any direction
bool
vpgl_generic_camera_convert::convert(vpgl_geo_camera & geocam,
                                     int ni,
                                     int nj,
                                     double height,
                                     vgl_vector_3d<double> & dir,
                                     vpgl_generic_camera<double> & gen_cam,
                                     unsigned level)
{
  double scale = (level < 32) ? double(1L << level) : std::pow(2.0, static_cast<double>(level));

  vbl_array_2d<vgl_ray_3d<double>> rays(nj, ni);
  vgl_point_3d<double> org;

  for (int j = 0; j < nj; ++j)
    for (int i = 0; i < ni; ++i)
    {
      double x, y, z;
      geocam.backproject(i * scale, j * scale, x, y, z);
      org.set(x, y, height);
      rays[j][i].set(org, dir);
    }
  gen_cam = vpgl_generic_camera<double>(rays);
  return true;
}

bool
vpgl_affine_camera_convert::convert(vpgl_local_rational_camera<double> const & camera_in,
                                    vgl_box_3d<double> const & region_of_interest,
                                    vpgl_affine_camera<double> & camera_out,
                                    unsigned int num_points)
{
  vnl_random rng;
  double min_x = region_of_interest.min_x();
  double min_y = region_of_interest.min_y();
  double min_z = region_of_interest.min_z();
  double max_x = region_of_interest.max_x();
  double max_y = region_of_interest.max_y();
  double max_z = region_of_interest.max_z();

  std::vector<vgl_point_2d<double>> image_pts;
  std::vector<vgl_point_3d<double>> world_pts;
  for (unsigned i = 0; i < num_points; ++i)
  {
    double x = rng.drand64(min_x, max_x); // sample in local coords
    double y = rng.drand64(min_y, max_y);
    double z = rng.drand64(min_z, max_z);
    world_pts.emplace_back(x, y, z);
    double u, v;
    camera_in.project(x, y, z, u, v); // local rational camera has an lvcs, so it handles, local coord to global to
                                      // image point projection internally
    image_pts.emplace_back(u, v);
  }

  bool success = vpgl_affine_camera_compute::compute(image_pts, world_pts, camera_out);
  // it is assumed that the camera is above the region of interest
  camera_out.set_viewing_distance(max_z + region_of_interest.depth() * 10);
  camera_out.orient_ray_direction(vgl_vector_3d<double>(0, 0, -1));
  return success;
}
#else
bool
vpgl_affine_camera_convert::convert(vpgl_local_rational_camera<double> const & camera_in,
                                    vgl_box_3d<double> const & region_of_interest,
                                    vpgl_affine_camera<double> & camera_out,
                                    unsigned int num_points)
{
  return false; // Always report failure if GEOTIFF not available.
}
#endif // HAS_GEOTIFF

#endif // vpgl_camera_convert_cxx_