1 // This is vpgl/vpgl_tri_focal_tensor.h 2 #ifndef vpgl_tri_focal_tensor_h_ 3 #define vpgl_tri_focal_tensor_h_ 4 #ifdef VCL_NEEDS_PRAGMA_INTERFACE 5 #pragma interface 6 #endif 7 //: 8 // \file 9 // \brief The trifocal tensor 10 // 11 // A class to hold a Trifocal Tensor and perform common operations, such as 12 // point and line transfer, coordinate-frame transformation and I/O. 13 // 14 // \author 15 // Paul Beardsley, 29.03.96 16 // Oxford University, UK 17 // 18 // \verbatim 19 // Modifications: 20 // AWF - Added composition, transformation, homography generation. 21 // Peter Vanroose - 11 Mar 97 - added operator== 22 // Peter Vanroose - 22 Jun 03 - added vgl interface 23 // JL Mundy - 30 Mar 18 - moved to vpgl, templated 24 // \endverbatim 25 // 26 //------------------------------------------------------------------------------ 27 28 #include <vector> 29 #include <iostream> 30 #include <iosfwd> 31 #include <memory> 32 #include <stdexcept> 33 #include <vbl/vbl_array_3d.h> 34 #include <vnl/vnl_matrix.h> 35 #include <vnl/vnl_matrix_fixed.h> 36 #include <vgl/vgl_fwd.h> 37 #include <vgl/algo/vgl_algo_fwd.h> 38 #include "vpgl_proj_camera.h" 39 #include "vpgl_fundamental_matrix.h" 40 41 template <class Type> 42 class vpgl_tri_focal_tensor 43 { 44 // Data Members------------------------------------------------------------ 45 protected: 46 vbl_array_3d<Type> T_; 47 48 // Epipoles 49 // e12 is the epipole corresponding to the center of camera 1 projected into image 2 50 // e13 is the epipole corresponding to the center of camera 1 projected into image 3 51 bool epipoles_valid_; 52 vgl_homg_point_2d<Type> e12_; 53 vgl_homg_point_2d<Type> e13_; 54 55 // cameras 56 bool cameras_valid_{false}; 57 vpgl_proj_camera<Type> c1_; 58 vpgl_proj_camera<Type> c2_; 59 vpgl_proj_camera<Type> c3_; 60 61 // fundamental matrices 62 // f12 is the fundamental matrix mapping a point in image 1 into a line in image 2 63 // f13 is the fundamental matrix mapping a point in image 1 into a line in image 3 64 // (note this order is opposite to H&Z but seems more intuitive) 65 // 66 bool f_matrices_1213_valid_; 67 vpgl_fundamental_matrix<Type> f12_; 68 vpgl_fundamental_matrix<Type> f13_; 69 bool f_matrix_23_valid_; 70 vpgl_fundamental_matrix<Type> f23_; 71 72 // set flags false and results invalid 73 void init(); 74 // make the Frobenius norm == 1 75 void normalize(); 76 public: 77 78 // Constructors/Initializers/Destructors----------------------------------- 79 vpgl_tri_focal_tensor()80 vpgl_tri_focal_tensor() : T_(vbl_array_3d<Type>(3, 3, 3, Type(0))) { 81 for (size_t i = 0; i < 3; ++i) 82 T_[i][i][i] = Type(1); 83 this->init(); 84 } vpgl_tri_focal_tensor(const vbl_array_3d<Type> & T)85 vpgl_tri_focal_tensor(const vbl_array_3d<Type>& T): T_(T),cameras_valid_(false) 86 { 87 this->init(); 88 } 89 //: Construct from 27-element vector vpgl_tri_focal_tensor(const Type * tri_focal_tensor_array)90 vpgl_tri_focal_tensor(const Type *tri_focal_tensor_array): T_(vbl_array_3d<Type>(3, 3, 3, tri_focal_tensor_array)),cameras_valid_(false){ 91 this->init(); 92 } 93 94 //: Construct from three cameras vpgl_tri_focal_tensor(const vpgl_proj_camera<Type> & c1,const vpgl_proj_camera<Type> & c2,const vpgl_proj_camera<Type> & c3)95 vpgl_tri_focal_tensor(const vpgl_proj_camera<Type>& c1, const vpgl_proj_camera<Type>& c2, const vpgl_proj_camera<Type>& c3): 96 T_(vbl_array_3d<Type>(3, 3, 3, Type(0))){ 97 set(c1, c2, c3); 98 this->init();// init must be second to avoid writing over cameras 99 } 100 101 //: Construct from two remaining cameras, the first camera is already canonical, i.e. [I | 0] vpgl_tri_focal_tensor(const vpgl_proj_camera<Type> & c2,const vpgl_proj_camera<Type> & c3)102 vpgl_tri_focal_tensor(const vpgl_proj_camera<Type>& c2, const vpgl_proj_camera<Type>& c3): 103 T_(vbl_array_3d<Type>(3, 3, 3, Type(0))){ 104 set( vpgl_proj_camera<Type>(), c2, c3); 105 this->init();// init must be second to avoid writing over cameras 106 } 107 108 //: Construct from three camera matrices vpgl_tri_focal_tensor(const vnl_matrix_fixed<Type,3,4> & m1,const vnl_matrix_fixed<Type,3,4> & m2,const vnl_matrix_fixed<Type,3,4> & m3)109 vpgl_tri_focal_tensor(const vnl_matrix_fixed<Type,3,4>& m1, const vnl_matrix_fixed<Type,3,4>& m2, const vnl_matrix_fixed<Type,3,4>& m3) 110 { 111 set(vpgl_proj_camera<Type>(m1), vpgl_proj_camera<Type>(m2), vpgl_proj_camera<Type>(m3)); 112 this->init();// init must be second to avoid writing over cameras 113 } 114 115 //: Construct from two camera matrices vpgl_tri_focal_tensor(const vnl_matrix_fixed<Type,3,4> & m2,const vnl_matrix_fixed<Type,3,4> & m3)116 vpgl_tri_focal_tensor(const vnl_matrix_fixed<Type,3,4>& m2, const vnl_matrix_fixed<Type,3,4>& m3){ 117 set(vpgl_proj_camera<Type>(), vpgl_proj_camera<Type>(m2), vpgl_proj_camera<Type>(m3)); 118 this->init();// init must be second to avoid writing over cameras 119 } 120 121 virtual ~vpgl_tri_focal_tensor() = default; 122 123 //: compute all derivative quantities compute()124 virtual bool compute() { 125 return (this->compute_epipoles() && this->compute_f_matrices() && 126 this->compute_proj_cameras() && this->compute_f_matrix_23()); 127 } 128 // Data Access------------------------------------------------------------- 129 130 // vpgl_tri_focal_tensor<Type>& operator=(const vpgl_tri_focal_tensor<Type>& T); 131 virtual bool operator==(vpgl_tri_focal_tensor<Type> const& T) const { 132 for (size_t i=0;i<3;++i) for (size_t j=0;j<3;++j) for (size_t k=0;k<3;++k) if (T_(i,j,k)!=T(i,j,k)) return false; 133 return true; } operator()134 Type& operator() (size_t i1, size_t i2, size_t i3) { return T_(i1,i2,i3); } operator()135 Type operator() (size_t i1, size_t i2, size_t i3) const { return T_(i1,i2,i3); } 136 set(size_t i1,size_t i2,size_t i3,Type value)137 void set(size_t i1, size_t i2, size_t i3, Type value){T_(i1, i2, i3) = value;} 138 set(vbl_array_3d<Type> & array)139 void set(vbl_array_3d<Type>& array){ 140 *this = vpgl_tri_focal_tensor<Type>(array); 141 } 142 void set(const vpgl_proj_camera<Type>& c1, const vpgl_proj_camera<Type>& c2, const vpgl_proj_camera<Type>& c3); set(const vpgl_proj_camera<Type> & c2,const vpgl_proj_camera<Type> & c3)143 void set(const vpgl_proj_camera<Type>& c2, const vpgl_proj_camera<Type>& c3){ 144 vpgl_proj_camera<Type> canon; this->set(canon, c2, c3); 145 } set(const vnl_matrix_fixed<Type,3,4> & M1,const vnl_matrix_fixed<Type,3,4> & M2,const vnl_matrix_fixed<Type,3,4> & M3)146 void set(const vnl_matrix_fixed<Type,3,4>& M1, const vnl_matrix_fixed<Type,3,4>& M2, const vnl_matrix_fixed<Type,3,4>& M3){ 147 this->set(vpgl_proj_camera<Type>(M1), vpgl_proj_camera<Type>(M2), vpgl_proj_camera<Type>(M3)); 148 } 149 150 // Data Control------------------------------------------------------------ 151 //: tri focal tensor point constraint (should be a 3x3 array of all zeros if points correspond) 152 vnl_matrix_fixed<Type, 3, 3> point_constraint_3x3(vgl_homg_point_2d<Type> const& point1, 153 vgl_homg_point_2d<Type> const& point2, 154 vgl_homg_point_2d<Type> const& point3); 155 156 157 //:tri focal tensor scalar point constraint (should == 0 if points correspond) 158 Type point_constraint(vgl_homg_point_2d<Type> const& point1, 159 vgl_homg_point_2d<Type> const& point2, 160 vgl_homg_point_2d<Type> const& point3); 161 162 //: tri focal tensor line constraint (should be a 3 vector all zeros if lines correspond) 163 vnl_vector_fixed<Type, 3> line_constraint_3(vgl_homg_line_2d<Type> const& line1, 164 vgl_homg_line_2d<Type> const& line2, 165 vgl_homg_line_2d<Type> const& line3); 166 //: point transfer 167 // point in image 1 corresponding to points in images 2 and 3 and etc. 168 virtual vgl_homg_point_2d<Type> image1_transfer(vgl_homg_point_2d<Type> const& point2, 169 vgl_homg_point_2d<Type> const& point3) const; 170 virtual vgl_homg_point_2d<Type> image2_transfer(vgl_homg_point_2d<Type> const& point1, 171 vgl_homg_point_2d<Type> const& point3) const; 172 virtual vgl_homg_point_2d<Type> image3_transfer(vgl_homg_point_2d<Type> const& point1, 173 vgl_homg_point_2d<Type> const& point2) const; 174 //: line transfer 175 // line in image 1 corresponding to lines in images 2 and 3 and etc. 176 virtual vgl_homg_line_2d<Type> image1_transfer(vgl_homg_line_2d<Type> const& line2, 177 vgl_homg_line_2d<Type> const& line3) const; 178 virtual vgl_homg_line_2d<Type> image2_transfer(vgl_homg_line_2d<Type> const& line1, 179 vgl_homg_line_2d<Type> const& line3) const; 180 virtual vgl_homg_line_2d<Type> image3_transfer(vgl_homg_line_2d<Type> const& line1, 181 vgl_homg_line_2d<Type> const& line2) const; 182 //: homographies induced by a line 183 // homography between images 3 and 1 given a line in image 2 and etc. 184 virtual vgl_h_matrix_2d<Type> hmatrix_13(vgl_homg_line_2d<Type> const& line2) const; 185 virtual vgl_h_matrix_2d<Type> hmatrix_12(vgl_homg_line_2d<Type> const& line3) const; 186 187 get_epipoles(vgl_homg_point_2d<Type> & e12,vgl_homg_point_2d<Type> & e13)188 virtual bool get_epipoles(vgl_homg_point_2d<Type>& e12, vgl_homg_point_2d<Type>& e13) { 189 if (!epipoles_valid_) compute_epipoles(); e12 = e12_; e13 = e13_; return epipoles_valid_; 190 } 191 192 bool compute_epipoles(); 193 epipole_12()194 virtual vgl_homg_point_2d<Type> epipole_12() {if(!epipoles_valid_) compute_epipoles(); return e12_;} epipole_13()195 virtual vgl_homg_point_2d<Type> epipole_13() {if(!epipoles_valid_) compute_epipoles(); return e13_;} 196 197 // The fundamental matrix between image 1 and image 2 is given by: 198 // ${\tt F12}_{jk} = \left [e12 \right ]_\times T_{ijk} e13_k$. //note the use of Einstein notation for summation 199 // and between image 1 and image 3 is given by: 200 // ${\tt F13}_{jk} = \left [e13 \right]_\times T_{ijk} e12_j$. 201 // Note that $\left [ e13 \right ]_\times$ is just the skew-symmetric matrix corresponding to e13. 202 // 203 bool compute_f_matrices(); 204 bool compute_f_matrix_23(); fmatrix_12()205 vpgl_fundamental_matrix<Type> fmatrix_12(){if(!f_matrices_1213_valid_) compute_f_matrices(); return f12_;} fmatrix_13()206 vpgl_fundamental_matrix<Type> fmatrix_13(){if(!f_matrices_1213_valid_) compute_f_matrices(); return f13_;} fmatrix_23()207 vpgl_fundamental_matrix<Type> fmatrix_23(){if(!f_matrix_23_valid_) compute_f_matrix_23(); return f23_;} 208 209 bool compute_proj_cameras(); 210 proj_camera_1()211 vpgl_proj_camera<Type> proj_camera_1(){if(!cameras_valid_) compute_proj_cameras(); return c1_;} proj_camera_2()212 vpgl_proj_camera<Type> proj_camera_2(){if(!cameras_valid_) compute_proj_cameras(); return c2_;} proj_camera_3()213 vpgl_proj_camera<Type> proj_camera_3(){if(!cameras_valid_) compute_proj_cameras(); return c3_;} 214 215 216 // Utility Methods--------------------------------------------------------- 217 218 void get_constraint_lines_image1(vgl_homg_point_2d<Type> const& p2, 219 vgl_homg_point_2d<Type> const& p3, 220 std::vector<vgl_homg_line_2d<Type> >& lines) const; 221 222 void get_constraint_lines_image2(vgl_homg_point_2d<Type> const& p1, 223 vgl_homg_point_2d<Type> const& p3, 224 std::vector<vgl_homg_line_2d<Type> >& lines) const; 225 226 void get_constraint_lines_image3(vgl_homg_point_2d<Type> const& p1, 227 vgl_homg_point_2d<Type> const& p2, 228 std::vector<vgl_homg_line_2d<Type> >& lines) const; 229 230 231 //: Contract Tensor axis tensor_axis with first component of Matrix M. 232 // That is: 233 // For tensor_axis = 1, Compute T_ijk = T_pjk M_pi 234 // For tensor_axis = 2, Compute T_ijk = T_ipk M_pj 235 // For tensor_axis = 3, Compute T_ijk = T_ijp M_pk 236 vpgl_tri_focal_tensor<Type> postmultiply(size_t tensor_axis, const vnl_matrix<Type>& M) const; 237 238 //: Contract Tensor axis tensor_axis with second component of Matrix M. 239 // That is: 240 // For tensor_axis = 1, Compute T_ijk = M_ip T_pjk 241 // For tensor_axis = 2, Compute T_ijk = M_jp T_ipk 242 // For tensor_axis = 3, Compute T_ijk = M_kp T_ijp 243 vpgl_tri_focal_tensor<Type> premultiply(size_t tensor_axis, const vnl_matrix<Type>& M) const; 244 245 //: implementations for individual axes 246 vpgl_tri_focal_tensor<Type> postmultiply1(const vnl_matrix<Type>& M) const; 247 vpgl_tri_focal_tensor<Type> postmultiply2(const vnl_matrix<Type>& M) const; 248 vpgl_tri_focal_tensor<Type> postmultiply3(const vnl_matrix<Type>& M) const; 249 250 vpgl_tri_focal_tensor<Type> premultiply1(const vnl_matrix<Type>& M) const; 251 vpgl_tri_focal_tensor<Type> premultiply2(const vnl_matrix<Type>& M) const; 252 vpgl_tri_focal_tensor<Type> premultiply3(const vnl_matrix<Type>& M) const; 253 254 // contractions involving vectors 255 //: ${\tt M}_{jk} = T_{ijk} v_i$ 256 vnl_matrix_fixed<Type,3,3> dot1(const vnl_vector_fixed<Type,3>& v) const; 257 //: ${\tt M}_{ik} = T_{ijk} v_j$ 258 vnl_matrix_fixed<Type,3,3> dot2(const vnl_vector_fixed<Type,3>& v) const; 259 //: ${\tt M}_{ij} = T_{ijk} v_k$ 260 vnl_matrix_fixed<Type,3,3> dot3(const vnl_vector_fixed<Type, 3>& v) const; 261 //: ${\tt M}_{kj} = T_{ijk} v_i$ (The transpose of dot1) 262 vnl_matrix_fixed<Type,3,3> dot1t(const vnl_vector_fixed<Type,3>& v) const; 263 //: ${\tt M}_{ki} = T_{ijk} v_j$ (The transpose of dot2). 264 vnl_matrix_fixed<Type,3,3> dot2t(const vnl_vector_fixed<Type,3>& v) const; 265 //: ${\tt M}_{ji} = T_{ijk} v_k$ (The transpose of dot3) 266 vnl_matrix_fixed<Type,3,3> dot3t(const vnl_vector_fixed<Type,3>& v) const; 267 268 // INTERNALS--------------------------------------------------------------- 269 270 private: 271 272 }; 273 //: stream operators 274 template<class Type> 275 std::ostream& operator << (std::ostream&, const vpgl_tri_focal_tensor<Type>& T); 276 template<class Type> 277 std::istream& operator >> (std::istream&, vpgl_tri_focal_tensor<Type>& T); 278 template<class Type> 279 280 //: are two tensors within a scale factor of each other, i.e. T1 ~ T2. 281 bool within_scale(const vpgl_tri_focal_tensor<Type>& T1, const vpgl_tri_focal_tensor<Type>& T2); 282 283 #endif // vpgl_tri_focal_tensor_h_ 284