vpgl_backproject_dem.cxx

Go to the documentation of this file.

// This is core/vpgl/algo/vpgl_backproject_dem.cxx
#include <limits>
#include <algorithm>
#include "vpgl_backproject.h"
#include "vpgl_backproject_dem.h"
#include <vpgl/file_formats/vpgl_geo_camera.h>
#include <vgl/vgl_point_2d.h>
#include <vgl/vgl_point_3d.h>
#include <vgl/vgl_plane_3d.h>
#include <vgl/vgl_ray_3d.h>
#include <vpgl/algo/vpgl_ray.h>
#include <vil/vil_image_resource.h>
#include <vil/vil_math.h>
#include <vnl/vnl_cost_function.h>
#include <vnl/algo/vnl_brent_minimizer.h>
class dem_bproj_cost_function : public vnl_cost_function{
public:
  dem_bproj_cost_function(vil_image_view<float> const& dem_view, vpgl_geo_camera* geo_cam, vgl_ray_3d<double> const& ray, bool verbose=false) :
    vnl_cost_function(1), ray_(ray), geo_cam_(geo_cam), dview_(&dem_view), verbose_(verbose){}
  //: x is the parameter that runs along the ray, with x=0 at the ray origin
  double f(vnl_vector<double> const& x) override{
    //get the lon and lat values for a given parameter value
    vgl_point_3d<double> org = ray_.origin(), rayp;
    vgl_vector_3d<double> dir = ray_.direction();
    rayp = org + x[0]*dir;
    double lon = rayp.x(), lat = rayp.y(), elev = rayp.z();
    double ud, vd;
    //determine the dem pixel coordinates and check bounds
    geo_cam_->project(lon, lat, elev, ud, vd);
    int u = static_cast<int>(ud+0.5), v = static_cast<int>(vd+0.5);
    int ni = dview_->ni(), nj = dview_->nj();
    if(u<0||u>=ni||v<0||v>=nj){
      if(verbose_) std::cout << "warning: dem backprojection cost function - outside DEM bounds" << std::endl;
      return std::numeric_limits<double>::max();
    }
    // within bounds so get squared distance between dem z and ray z
    double z = (*dview_)(u,v);
    return (elev-z)*(elev-z);
  }

private:
  vgl_ray_3d<double> ray_;
  vpgl_geo_camera* geo_cam_;
  const vil_image_view<float>* dview_;
  bool verbose_;
};
vpgl_backproject_dem::vpgl_backproject_dem( vil_image_resource_sptr const& dem, double zmin, double zmax):verbose_(false), min_samples_(5000.0), tail_fract_(0.025), dem_(dem)
{
  //construct a geo_camera for the dem (an orthographic view looking straight down)
  if(!vpgl_geo_camera::init_geo_camera(dem_, geo_cam_)){
    if(verbose_) std::cout << "WARNING! - units unknown in vpgl_backproject_dem - assume degrees meters" << std::endl;
  }
  if(!geo_cam_)
    return;
  //get the image of elevations
  dem_view_ = dem_->get_view();
  unsigned ni = dem_view_.ni(), nj = dem_view_.nj();

  // get the center of the dem
  unsigned nhi = ni/2, nhj = nj/2;
  double lon, lat;
  geo_cam_->img_to_global(nhi, nhj, lon, lat);
  double elev = dem_view_(nhi, nhj);
  geo_center_.set(lon, lat, elev);

  // get the corners of the dem
  geo_cam_->img_to_global(0, 0, lon, lat);
  elev = dem_view_(0, 0);
  dem_corners_.emplace_back(lon, lat, elev);

  geo_cam_->img_to_global(ni-1, 0, lon, lat);
  elev = dem_view_(ni-1, 0);
  dem_corners_.emplace_back(lon, lat, elev);

  geo_cam_->img_to_global(ni-1, nj-1, lon, lat);
  elev = dem_view_(ni-1, nj-1);
  dem_corners_.emplace_back(lon, lat, elev);

  geo_cam_->img_to_global(0, nj-1, lon, lat);
  elev = dem_view_(0, nj-1);
  dem_corners_.emplace_back(lon, lat, elev);

  // check for appropriate zmin/zmax inputs
  if (zmax > zmin) {
    z_min_ = zmin;
    z_max_ = zmax;

  } else {
    if(verbose_) std::cout << "Calculating Z-range from DEM..." << std::endl;
    //get the bounds on elevation by sampling according to a fraction of the dem area
    //compute the pixel interval (stride) for sampling the fraction
    double area = ni*nj;
    double stride_area = area/min_samples_;
    auto stride_interval = static_cast<unsigned>(std::sqrt(stride_area));

    // sample elevations
    std::vector<double> z_samples;
    float zmin_calc=std::numeric_limits<float>::max(), zmax_calc=-zmin_calc;
    for(unsigned j = 0; j<nj; j+=stride_interval)
      for(unsigned i = 0; i<ni; i+=stride_interval){
        float z = dem_view_(i,j);
        if(z <= 0.0)
          continue;
        z_samples.push_back(z);
      }
    // sort the samples to remove tails due to DEM errors
    std::sort(z_samples.begin(), z_samples.end());
    // remove the tails and compute min max elevations
    auto ns = static_cast<double>(z_samples.size());
    auto band_size = static_cast<unsigned>(ns*tail_fract_);
    for(unsigned k = band_size; k<(ns-band_size); ++k){
      double z = z_samples[k];
      if(z<zmin_calc) zmin_calc = static_cast<float>(z);
      if(z>zmax_calc) zmax_calc = z;
    }
    //the final elevation bounds
    z_min_ = zmin_calc;
    z_max_ = zmax_calc;
  }

  if(verbose_) std::cout << "[ZMIN,ZMAX]=[" << z_min_ << "," << z_max_ << "]" << std::endl;
}
vpgl_backproject_dem::~vpgl_backproject_dem(){
  if(geo_cam_)
    delete geo_cam_;
  geo_cam_ = nullptr;
}
// the function to backproject onto the dem using vgl objects
bool vpgl_backproject_dem::bproj_dem(const vpgl_camera<double>* cam,
                 vgl_point_2d<double> const& image_point,
                 double max_z, double min_z,
                 vgl_point_3d<double> const& initial_guess,
                 vgl_point_3d<double> & world_point,
                 double error_tol){
if(verbose_) std::cout << "vpgl_backproj_dem " << image_point << " max_z " << max_z << " min_z " << min_z << " init_guess " << initial_guess << " error tol " << error_tol << std::endl;
  //compute the ray corresponding to the image point
  double dz = (max_z - min_z);
  vgl_point_2d<double> initial_xy(initial_guess.x(), initial_guess.y());
  vgl_ray_3d<double> ray;
  if(!vpgl_ray::ray(cam, image_point, initial_xy, max_z, dz, ray)){
    if(verbose_) std::cout << " compute camera ray failed - Fatal!" << std::endl;
    return false;
  }
  vgl_point_3d<double> origin = ray.origin();
  //find min parameter on ray
  vgl_vector_3d<double> dir = ray.direction();
  if(std::fabs(dir.z())<0.001){
    if(verbose_) std::cout << "Ray parallel to XY plane - Fatal!" << std::endl;
    return false;
  }
  //inital guess at ray parameter
  double t = (initial_guess.z() - max_z)/dir.z();

  //check if guess is inside DEM - could be too conservative for very oblique cameras
  vgl_point_3d<double> guess_point = origin + t*dir;
  double lon = guess_point.x(), lat = guess_point.y(),elev = guess_point.z();
  double ud, vd;
  //Note that elevation has no effect in the project function - just required to meet the interface of an abstract camera
  geo_cam_->project(lon, lat, elev, ud, vd);
  int u = static_cast<int>(ud+0.5), v = static_cast<int>(vd+0.5);
  int ni = dem_view_.ni(), nj = dem_view_.nj();
  if(u<0||u>=ni||v<0||v>=nj){
    if(verbose_) std::cout << "Initial guess for DEM intersection is outside DEM bounds - Fatal!" << std::endl;
    return false;
  }
  // construct the brent minimizer
  // note the brent minimzier is not fully integrated with the vil_nonlinear_minimizer interface
  // thus the non standard call for error below
  dem_bproj_cost_function c(dem_view_, geo_cam_, ray, verbose_);
  vnl_brent_minimizer brm(c);
  double tmin = brm.minimize(t);
  double error = brm.f_at_last_minimum();
  if(error>error_tol)
    return false;

  world_point = origin + tmin*dir;

  if(verbose_) std::cout << "success! ray/dem intersection " << world_point  << std::endl;
  return true;
}

bool vpgl_backproject_dem::bproj_dem(const vpgl_camera<double>* cam, vnl_double_2 const& image_point,
                                     double max_z, double min_z, vnl_double_3 const& initial_guess, vnl_double_3& world_point,
                                     double error_tol){
  vgl_point_2d<double> img_pt;
  vgl_point_3d<double> init_guess;
  vgl_point_3d<double> wrld_pt;
  img_pt.set(image_point[0], image_point[1]);
  init_guess.set(initial_guess[0],initial_guess[1],initial_guess[2]);
  bool good = this->bproj_dem(cam, img_pt, max_z, min_z, init_guess, wrld_pt, error_tol);

  if(!good){
    return false;
  }
  world_point[0]=wrld_pt.x(); world_point[1]=wrld_pt.y(); world_point[2]=wrld_pt.z();
  return true;
}

bool vpgl_backproject_dem::bproj_dem(vpgl_rational_camera<double> const& rcam, vnl_double_2 const& image_point, double max_z, double min_z,
                                     vnl_double_3 const& initial_guess, vnl_double_3& world_point, double error_tol){

  const auto* cam = dynamic_cast<const vpgl_camera<double>* >(&rcam);
  return this->bproj_dem(cam, image_point, max_z, min_z, initial_guess, world_point, error_tol);
}

bool vpgl_backproject_dem::bproj_dem(vpgl_rational_camera<double> const& rcam, vgl_point_2d<double> const& image_point, double max_z, double min_z,
                                     vgl_point_3d<double> const& initial_guess, vgl_point_3d<double>& world_point, double error_tol){

  const auto* cam = dynamic_cast<const vpgl_camera<double>* >(&rcam);
  return this->bproj_dem(cam, image_point, max_z, min_z, initial_guess, world_point, error_tol);
}