1 // This is core/vpgl/algo/vpgl_backproject.h
2 #ifndef vpgl_backproject_h_
3 #define vpgl_backproject_h_
4 //:
5 // \file
6 // \brief Methods for back_projecting from cameras to 3-d geometric structures
7 // \author J. L. Mundy
8 // \date Oct 29, 2006
9 //
10 // \verbatim
11 // Modifications
12 // Yi Dong Jun-2015 added relative diameter as one argument, with default value 1.0 (same as before)
13 // \endverbatim
14
15 #include <vpgl/vpgl_camera.h>
16 #include <vpgl/vpgl_generic_camera.h>
17 #include <vpgl/vpgl_proj_camera.h>
18 #include <vgl/vgl_fwd.h>
19 #include <vnl/vnl_double_2.h>
20 #include <vnl/vnl_double_3.h>
21 #include <vnl/vnl_double_4.h>
22 #include <vgl/vgl_plane_3d.h>
23
24 class vpgl_backproject
25 {
26 public:
27 ~vpgl_backproject();
28
29 //: Generic camera interfaces (pointer for abstract class)
30 // An iterative solution using forward projection
31 // vnl interface
32
33 //:Backproject an image point onto a plane, start with initial_guess
34 static bool bproj_plane(vpgl_camera<double> const& cam,
35 vnl_double_2 const& image_point,
36 vnl_double_4 const& plane,
37 vnl_double_3 const& initial_guess,
38 vnl_double_3& world_point,
39 double error_tol = 0.05,
40 double relative_diameter = 1.0);
41
42
43 //: wrapper function to keep backwards-compatibility with previous API
44 static bool bproj_plane(const vpgl_camera<double> *cam,
45 vnl_double_2 const& image_point,
46 vnl_double_4 const& plane,
47 vnl_double_3 const& initial_guess,
48 vnl_double_3& world_point,
49 double error_tol = 0.05,
50 double relative_diameter = 1.0)
51 { return bproj_plane(*cam, image_point, plane, initial_guess, world_point, error_tol, relative_diameter); }
52
53
54 // +++ concrete camera interfaces +++
55
56 //:Backproject an image point onto a plane, start with initial_guess
57 static bool bproj_plane(vpgl_generic_camera<double> const& cam,
58 vnl_double_2 const& image_point,
59 vnl_double_4 const& plane,
60 vnl_double_3 const& initial_guess,
61 vnl_double_3& world_point,
62 double error_tol = 0.05,
63 double relative_diameter = 1.0);
64
65 //:Backproject an image point onto a plane, start with initial_guess
66 static bool bproj_plane(vpgl_proj_camera<double> const& cam,
67 vnl_double_2 const& image_point,
68 vnl_double_4 const& plane,
69 vnl_double_3 const& initial_guess,
70 vnl_double_3& world_point,
71 double error_tol = 0.05,
72 double relative_diameter = 1.0);
73
74
75 // ==== vgl interface ===
76
77 //: Backproject an image point onto a plane, start with initial_guess
78 template <class CAM_T>
79 static bool bproj_plane(CAM_T const& cam,
80 vgl_point_2d<double> const& image_point,
81 vgl_plane_3d<double> const& plane,
82 vgl_point_3d<double> const& initial_guess,
83 vgl_point_3d<double>& world_point,
84 double error_tol = 0.05,
85 double relative_diameter = 1.0);
86
87
88 //: wrapper function to keep backwards compatibility with the previous API
89 static bool bproj_plane(const vpgl_camera<double> *cam,
90 vgl_point_2d<double> const& image_point,
91 vgl_plane_3d<double> const& plane,
92 vgl_point_3d<double> const& initial_guess,
93 vgl_point_3d<double>& world_point,
94 double error_tol = 0.05,
95 double relative_diameter = 1.0)
96 { return bproj_plane(*cam, image_point, plane, initial_guess, world_point, error_tol, relative_diameter); }
97
98 //:Backproject a point with associated direction vector in the image to a plane in 3-d, passing through the center of projection and containing the point and vector.
99 // ** Defined only for a projective camera **
100 static bool bproj_point_vector(vpgl_proj_camera<double> const& cam,
101 vgl_point_2d<double> const& point,
102 vgl_vector_2d<double> const& vect,
103 vgl_plane_3d<double>& plane);
104
105 //: Use backprojection to determine direction to camera from 3-d point
106 template <class CAM_T>
107 static bool direction_to_camera(CAM_T const& cam,
108 vgl_point_3d<double> const& point,
109 vgl_vector_3d<double> &to_camera,
110 double error_tol = 0.05,
111 double relative_diameter = 1.0);
112
113 private:
114 //: constructor private - static methods only
115 vpgl_backproject();
116 };
117
118
119
120 //--- Template Definitions ---//
121
122 template <class CAM_T>
bproj_plane(CAM_T const & cam,vgl_point_2d<double> const & image_point,vgl_plane_3d<double> const & plane,vgl_point_3d<double> const & initial_guess,vgl_point_3d<double> & world_point,double error_tol,double relative_diameter)123 bool vpgl_backproject::bproj_plane(CAM_T const& cam,
124 vgl_point_2d<double> const& image_point,
125 vgl_plane_3d<double> const& plane,
126 vgl_point_3d<double> const& initial_guess,
127 vgl_point_3d<double>& world_point,
128 double error_tol,
129 double relative_diameter)
130 {
131 //simply convert to vnl interface
132 vnl_double_2 ipt;
133 vnl_double_3 ig, wp;
134 vnl_double_4 pl;
135 ipt[0]=image_point.x(); ipt[1]=image_point.y();
136 pl[0]=plane.a(); pl[1]=plane.b(); pl[2]=plane.c(); pl[3]=plane.d();
137 ig[0]=initial_guess.x(); ig[1]=initial_guess.y(); ig[2]=initial_guess.z();
138 bool success = vpgl_backproject::bproj_plane(cam, ipt, pl, ig, wp, error_tol, relative_diameter);
139 world_point.set(wp[0], wp[1], wp[2]);
140 return success;
141 }
142
143 template<class CAM_T>
direction_to_camera(CAM_T const & cam,vgl_point_3d<double> const & point,vgl_vector_3d<double> & to_camera,double error_tol,double relative_diameter)144 bool vpgl_backproject::direction_to_camera(CAM_T const& cam,
145 vgl_point_3d<double> const& point,
146 vgl_vector_3d<double> &to_camera,
147 double error_tol,
148 double relative_diameter)
149 {
150 // assumes that camera is above point of interest
151 // project point to image, and backproject to another z-plane, vector points to sensor
152 vgl_point_2d<double> img_pt = cam.project(point);
153 constexpr double z_off = 10.0;
154 vgl_plane_3d<double> plane_high(0,0,1,-(point.z()+z_off));
155 vgl_point_3d<double> point_high;
156 vgl_point_3d<double> guess(point.x(),point.y(),point.z() + z_off);
157 if (!bproj_plane(cam, img_pt, plane_high, guess, point_high, error_tol, relative_diameter)) {
158 return false;
159 }
160 // this assumes camera z > point.z
161 to_camera = point_high - point;
162 // normalize vector
163 normalize(to_camera);
164
165 return true;
166 }
167
168 #endif // vpgl_backproject_h_
169