00001 #include "vpgl_backproject.h"
00002
00003
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
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
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
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
00050 if (err > error_tol)
00051 {
00052 vcl_cerr << "ERROR: backprojection error = " << err << vcl_endl;
00053 return false;
00054 }
00055 return true;
00056 }
00057
00058
00059
00060
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
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
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
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
00106
00107
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
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
00123 return true;
00124 }
00125
00126
00127
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
00133
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
00143 to_camera = point_high - point;
00144
00145 normalize(to_camera);
00146
00147 return true;
00148
00149 }
00150