core/vpgl/algo/vpgl_backproject.cxx
Go to the documentation of this file.
00001 #include "vpgl_backproject.h"
00002 //:
00003 // \file
00004 #include <vpgl/algo/vpgl_invmap_cost_function.h>
00005 #include <vgl/vgl_point_2d.h>
00006 #include <vgl/vgl_point_3d.h>
00007 #include <vgl/vgl_plane_3d.h>
00008 #include <vnl/algo/vnl_amoeba.h>
00009 #include <vgl/vgl_intersection.h>
00010 #include <vpgl/vpgl_generic_camera.h>
00011 
00012 //: Backproject an image point onto a plane, start with initial_guess
00013 bool vpgl_backproject::bproj_plane(const vpgl_camera<double>* cam,
00014                                    vnl_double_2 const& image_point,
00015                                    vnl_double_4 const& plane,
00016                                    vnl_double_3 const& initial_guess,
00017                                    vnl_double_3& world_point,
00018                                    double error_tol)
00019 {
00020   // special case of a generic camera
00021   if (cam->type_name()=="vpgl_generic_camera")
00022   {
00023     vgl_ray_3d<double> ray;
00024     vgl_point_3d<double> ipt;
00025     vgl_plane_3d<double> gplane(plane[0], plane[1], plane[2], plane[3]);
00026     const vpgl_generic_camera<double>* gcam = dynamic_cast<const vpgl_generic_camera<double>*>(cam);
00027     ray = gcam->ray(image_point[0], image_point[1]);
00028     if (!vgl_intersection<double>(ray, gplane, ipt))
00029       return false;
00030     world_point[0]=ipt.x(); world_point[1]=ipt.y(); world_point[2]=ipt.z();
00031     return true;
00032   }
00033   // general case
00034   vpgl_invmap_cost_function cf(image_point, plane, cam);
00035   vnl_double_2 x1(0.0, 0.0);
00036 
00037   cf.set_params(initial_guess, x1);
00038   vnl_amoeba amoeba(cf);
00039   amoeba.set_max_iterations(100000);
00040   amoeba.set_relative_diameter(1.0);
00041   vnl_vector<double> x(&x1[0], 2);
00042   amoeba.minimize(x); x1 = x;
00043   cf.point_3d(x1, world_point);
00044   double u=0, v=0, X=world_point[0], Y=world_point[1], Z=world_point[2];
00045   cam->project(X, Y, Z, u, v);
00046    vnl_double_2 final_proj;
00047    final_proj[0]=u; final_proj[1]=v;
00048   double err = (final_proj-image_point).magnitude();
00049   // was: double err = vcl_sqrt(cf.f(x));
00050   if (err > error_tol) // greater than a 20th of a pixel
00051   {
00052     vcl_cerr << "ERROR: backprojection error = " << err << vcl_endl;
00053     return false;
00054   }
00055   return true;
00056 }
00057 
00058   // vgl interface
00059 
00060 //: Backproject an image point onto a plane, start with initial_guess
00061 bool vpgl_backproject::bproj_plane(const vpgl_camera<double>*  cam,
00062                                    vgl_point_2d<double> const& image_point,
00063                                    vgl_plane_3d<double> const& plane,
00064                                    vgl_point_3d<double> const& initial_guess,
00065                                    vgl_point_3d<double>& world_point,
00066                                    double error_tol)
00067 {
00068   //simply convert to vnl interface
00069   vnl_double_2 ipt;
00070   vnl_double_3 ig, wp;
00071   vnl_double_4 pl;
00072   ipt[0]=image_point.x(); ipt[1]=image_point.y();
00073   pl[0]=plane.a(); pl[1]=plane.b(); pl[2]=plane.c(); pl[3]=plane.d();
00074   ig[0]=initial_guess.x();  ig[1]=initial_guess.y();  ig[2]=initial_guess.z();
00075   bool success = vpgl_backproject::bproj_plane(cam, ipt, pl, ig, wp, error_tol);
00076   world_point.set(wp[0], wp[1], wp[2]);
00077   return success;
00078 }
00079 
00080 
00081 //: Backproject an image point onto a world plane
00082 bool vpgl_backproject::bproj_plane(vpgl_rational_camera<double> const& rcam,
00083                                    vnl_double_2 const& image_point,
00084                                    vnl_double_4 const& plane,
00085                                    vnl_double_3 const& initial_guess,
00086                                    vnl_double_3& world_point,
00087                                    double error_tol)
00088 {
00089   const vpgl_camera<double>*  cam = static_cast<const vpgl_camera<double>* >(&rcam);
00090   return bproj_plane(cam, image_point, plane, initial_guess, world_point, error_tol);
00091 }
00092 
00093 //: Backproject an image point onto a world plane
00094 bool vpgl_backproject::bproj_plane(vpgl_rational_camera<double> const& rcam,
00095                                    vgl_point_2d<double> const& image_point,
00096                                    vgl_plane_3d<double> const& plane,
00097                                    vgl_point_3d<double> const& initial_guess,
00098                                    vgl_point_3d<double>& world_point,
00099                                    double error_tol)
00100 {
00101   const vpgl_camera<double>* const cam = static_cast<const vpgl_camera<double>* >(&rcam);
00102   return bproj_plane(cam, image_point, plane, initial_guess, world_point, error_tol);
00103 }
00104 
00105 //Only the direction of the vector is important so it can be
00106 //normalized to a unit vector. Two rays can be constructed, one through
00107 //point and one through a point formed by adding the vector to the point
00108 bool
00109 vpgl_backproject::bproj_point_vector(vpgl_proj_camera<double> const& cam,
00110                                vgl_point_2d<double> const& point,
00111                                vgl_vector_2d<double> const& vect,
00112                                vgl_plane_3d<double>& plane)
00113 {
00114   vgl_homg_point_2d<double> hp(point);
00115   //get a second point
00116   vgl_vector_2d<double> vu = vect/vect.length();
00117   vgl_point_2d<double> point_plus_vect = point + vu;
00118   vgl_homg_point_2d<double> hp1(point_plus_vect);
00119   vgl_homg_line_2d<double> hline(hp, hp1);
00120   vgl_homg_plane_3d<double> hpl = cam.backproject(hline);
00121   plane = vgl_plane_3d<double>(hpl);
00122   //might add checks for ideal plane later
00123   return true;
00124 }
00125 
00126 
00127 //: Use backprojection to determine direction to camera from 3-d point
00128 bool vpgl_backproject::direction_to_camera(vpgl_local_rational_camera<double> const& cam,
00129                                            vgl_point_3d<double> const& point,
00130                                            vgl_vector_3d<double> &to_camera)
00131 {
00132   // assumes that camera is above point of interest
00133   // project point to image, and backproject to another z-plane, vector points to sensor
00134   vgl_point_2d<double> img_pt = cam.project(point);
00135   const double z_off = 10.0;
00136   vgl_plane_3d<double> plane_high(0,0,1,-(point.z()+z_off));
00137   vgl_point_3d<double> point_high;
00138   vgl_point_3d<double> guess(point.x(),point.y(),point.z() + z_off);
00139   if (!bproj_plane(cam, img_pt, plane_high, guess, point_high)) {
00140     return false;
00141   }
00142   // this assumes camera z > point.z
00143   to_camera = point_high - point;
00144   // normalize vector
00145   normalize(to_camera);
00146 
00147   return true;
00148 
00149 }
00150