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