1 // This is core/vpgl/vpgl_rational_camera.h 2 #ifndef vpgl_rational_camera_h_ 3 #define vpgl_rational_camera_h_ 4 //: 5 // \file 6 // \brief A camera model based on ratios of cubic polynomials 7 // \author Joseph Mundy 8 // \date Oct 2006 9 // 10 // 11 // A camera that projects 3-d world points according to ratios of 12 // cubic polynomials. That is, 13 // \verbatim 14 // neu_u(X,Y,Z) neu_v(X,Y,Z) 15 // u = ------------ v = ------------ 16 // den_u(X,Y,Z) den_v(X,Y,X) 17 // \endverbatim 18 // where u is the image column index and v is the image row index. 19 // 20 // neu_u(X,Y,Z),den_u(X,Y,Z), neu_v(X,Y,Z), den_v(X,Y,Z) are 21 // cubic polynomials in three variables and there are 20 coefficients each, 22 // e.g., 23 // 24 // R(X,Y,Z) = a300 x^3 + a210 x^2y + a201 x^2z + a200 x^2 + ... + a001z + a000 25 // 26 // The normal ordering of multi-variate polynomial coefficients is as 27 // shown above, the highest powers in x followed by highest powers in y, 28 // followed by powers in z. The full monomial sequence is: 29 // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 30 // x^3 x^2y x^2z x^2 xy^2 xyz xy xz^2 xz x y^3 y^2z y^2 yz^2 yz y z^3 z^2 z 1 31 // The highest powers are in the lowest index of the coefficient vector. 32 // 33 // Polynomial calculations are often ill-conditioned if the variables are not 34 // normalized. Common practice is to normalize all variables to the 35 // range [-1, 1]. This normalization requires 10 additional offset and scale 36 // parameters for a total of 90 parameters. 37 // 38 // The scale and offset transformation is applied to (X,Y,Z) 39 // before applying the polynomial mapping. The resulting (u,v) is normalized 40 // and must be mapped back (un-normalized) to the image coordinate 41 // system to obtain the actual projection. 42 // 43 // In order to facilitate the scale offset transformation process, a helper 44 // class, scale_offset, is defined to process the forward and reverse 45 // normalizations. 46 // 47 // While the internal ordering of rational coefficients does not change, 48 // coefficients can be input and output several different orders. 49 // 50 // currently supported ordering schemas are: 51 // VXL: VXL internal ordering 52 // RPC00B 53 // RPC00A 54 // 55 // RPC00A ordering is defined in the public domain document 56 // "RPC00A vs RPC00B White Paper.docx" 57 // Available at the following links: 58 // https://github.com/ngageoint/Rational-Polynomial-Coefficients-Mapper/raw/master/RPC00A%20vs%20RPC00B%20White%20Paper.docx 59 // http://www.gwg.nga.mil/ntb/baseline/docs/RPC/RPC00A%20vs%20RPC00B%20White%20Paper.docx 60 // 61 // conversion between ordering schema 62 // x = longitude, L 63 // y = latitude, P 64 // z = elevation, H 65 // 66 // VXL RPC00B RPC00A 67 // xxx 0 11 11 68 // xxy 1 14 12 69 // xxz 2 17 13 70 // xx 3 7 8 71 // xyy 4 12 14 72 // xyz 5 10 7 73 // xy 6 4 4 74 // xzz 7 13 17 75 // xz 8 5 5 76 // x 9 1 1 77 // yyy 10 15 15 78 // yyz 11 18 16 79 // yy 12 8 9 80 // yzz 13 16 18 81 // yz 14 6 6 82 // y 15 2 2 83 // zzz 16 19 19 84 // zz 17 9 10 85 // z 18 3 3 86 // 1 19 0 0 87 // 88 #include <iostream> 89 #include <string> 90 #include <utility> 91 #include <vector> 92 #include <stdexcept> 93 #include <array> 94 95 #ifdef _MSC_VER 96 # include <vcl_msvc_warnings.h> 97 #endif 98 #include <vgl/vgl_fwd.h> 99 #include <vnl/vnl_matrix_fixed.h> 100 #include <vnl/vnl_vector_fixed.h> 101 #include "vpgl_camera.h" 102 103 104 // ---------------------------------------- 105 // rational camera order for input/output operations 106 enum class vpgl_rational_order { 107 VXL, 108 RPC00B, 109 RPC00A, 110 }; 111 112 // conversion to other forms (see vpgl_rational_camera.cxx) 113 class vpgl_rational_order_func { 114 public: 115 static std::vector<unsigned> to_vector(vpgl_rational_order choice); 116 static std::string to_string(vpgl_rational_order choice); 117 static vpgl_rational_order from_string(std::string const& buf); 118 119 // initializer_list for iteration 120 // e.g. "for (auto item : vpgl_rational_order_func::initializer_list) {...}" 121 static constexpr std::array<vpgl_rational_order,3> initializer_list{{ 122 vpgl_rational_order::VXL, 123 vpgl_rational_order::RPC00B, 124 vpgl_rational_order::RPC00A 125 }}; 126 127 private: 128 vpgl_rational_order_func() = delete; 129 }; 130 131 132 133 134 // ---------------------------------------- 135 // Represent scale and offset transformations used in normalization 136 // 137 template <class T> 138 class vpgl_scale_offset 139 { 140 public: vpgl_scale_offset()141 vpgl_scale_offset() : 142 scale_(1), offset_(0) {} vpgl_scale_offset(const T scale,const T offset)143 vpgl_scale_offset(const T scale, const T offset) : 144 scale_(scale), offset_(offset) {} 145 146 //mutators/accessors set_scale(const T scale)147 void set_scale(const T scale) {scale_ = scale;} set_offset(const T offset)148 void set_offset(const T offset) {offset_ = offset;} scale()149 T scale() const {return scale_;} offset()150 T offset() const {return offset_;} 151 152 // normalize a coordinate value normalize(const T value)153 T normalize(const T value) const 154 { 155 if (scale_==0) 156 return 0; 157 else 158 return (value-offset_)/scale_; 159 } 160 161 // un-normalize a coordinate value un_normalize(const T value)162 T un_normalize(const T value) const 163 { 164 T temp = value*scale_; 165 return temp + offset_; 166 } 167 //: Equality test 168 inline bool operator==(vpgl_scale_offset<T> const &that) const 169 { return this == &that || 170 (this->scale()==that.scale() && 171 this->offset() == that.offset() ); 172 } 173 private: 174 //members 175 T scale_; 176 T offset_; 177 }; 178 179 180 //--------------------=== rational camera ===--------------------------- 181 // 182 template <class T> 183 class vpgl_rational_camera : public vpgl_camera<T> 184 { 185 public: 186 //: enumeration for indexing coordinates 187 enum coor_index{X_INDX = 0, Y_INDX, Z_INDX, U_INDX, V_INDX}; 188 //: enumeration for indexing polynomials 189 enum poly_index{NEU_U = 0, DEN_U, NEU_V, DEN_V}; 190 191 //: default constructor 192 vpgl_rational_camera(); 193 194 //: Constructor from 4 coefficient vectors and 5 scale, offset pairs. 195 vpgl_rational_camera(std::vector<T> const& neu_u, 196 std::vector<T> const& den_u, 197 std::vector<T> const& neu_v, 198 std::vector<T> const& den_v, 199 const T x_scale, const T x_off, 200 const T y_scale, const T y_off, 201 const T z_scale, const T z_off, 202 const T u_scale, const T u_off, 203 const T v_scale, const T v_off, 204 vpgl_rational_order input_order = vpgl_rational_order::VXL 205 ); 206 207 //: Constructor from 4 coefficient arrays and 5 scale, offset pairs. 208 vpgl_rational_camera(const double* neu_u, 209 const double* den_u, 210 const double* neu_v, 211 const double* den_v, 212 const T x_scale, const T x_off, 213 const T y_scale, const T y_off, 214 const T z_scale, const T z_off, 215 const T u_scale, const T u_off, 216 const T v_scale, const T v_off, 217 vpgl_rational_order input_order = vpgl_rational_order::VXL 218 ); 219 220 //: Constructor with everything wrapped up in an array and vector. 221 vpgl_rational_camera(std::vector<std::vector<T> > const& rational_coeffs, 222 std::vector<vpgl_scale_offset<T> > const& scale_offsets, 223 vpgl_rational_order input_order = vpgl_rational_order::VXL 224 ); 225 226 //: Constructor with a coefficient matrix 227 vpgl_rational_camera(vnl_matrix_fixed<T, 4, 20> const& rational_coeffs, 228 std::vector<vpgl_scale_offset<T> > const& scale_offsets, 229 vpgl_rational_order input_order = vpgl_rational_order::VXL 230 ); 231 232 ~vpgl_rational_camera() override = default; 233 type_name()234 std::string type_name() const override { return "vpgl_rational_camera"; } 235 236 //: Clone `this': creation of a new object and initialization 237 // legal C++ because the return type is covariant with vpgl_camera<T>* 238 vpgl_rational_camera<T> *clone() const override; 239 240 //: Equality test 241 inline bool operator==(vpgl_rational_camera<T> const &that) const 242 { return this == &that || 243 ((this->coefficient_matrix()==that.coefficient_matrix())&& 244 (this->scale_offsets() == that.scale_offsets()) );} 245 246 // --- Mutators/Accessors --- 247 248 //: set rational polynomial coefficients 249 void set_coefficients( 250 std::vector<T> const& neu_u, 251 std::vector<T> const& den_u, 252 std::vector<T> const& neu_v, 253 std::vector<T> const& den_v, 254 vpgl_rational_order input_order = vpgl_rational_order::VXL 255 ); 256 257 void set_coefficients( 258 const double* neu_u, 259 const double* den_u, 260 const double* neu_v, 261 const double* den_v, 262 vpgl_rational_order input_order = vpgl_rational_order::VXL 263 ); 264 265 void set_coefficients( 266 std::vector<std::vector<T> > const& rational_coeffs, 267 vpgl_rational_order input_order = vpgl_rational_order::VXL 268 ); 269 270 void set_coefficients( 271 vnl_matrix_fixed<T, 4, 20> const& rational_coeffs, 272 vpgl_rational_order input_order = vpgl_rational_order::VXL 273 ); 274 275 //: get the rational polynomial coefficients in a vnl matrix 276 vnl_matrix_fixed<T, 4, 20> coefficient_matrix( 277 vpgl_rational_order output_order = vpgl_rational_order::VXL 278 ) const; 279 280 //: get the rational polynomial coefficients in std vector of vectors 281 std::vector<std::vector<T> > coefficients( 282 vpgl_rational_order output_order = vpgl_rational_order::VXL 283 ) const; 284 285 //: set all coordinate scale and offsets 286 void set_scale_offsets(const T x_scale, const T x_off, 287 const T y_scale, const T y_off, 288 const T z_scale, const T z_off, 289 const T u_scale, const T u_off, 290 const T v_scale, const T v_off 291 ); 292 293 void set_scale_offsets(std::vector<vpgl_scale_offset<T> > const& scale_offsets); 294 295 //: get the scale and offsets in a vector scale_offsets()296 std::vector<vpgl_scale_offset<T> > scale_offsets() const 297 {return scale_offsets_;} 298 299 //:set a specific scale value set_scale(const coor_index coor_index,const T scale)300 void set_scale(const coor_index coor_index, const T scale) 301 {scale_offsets_[coor_index].set_scale(scale);} 302 303 //:set a specific offset value set_offset(const coor_index coor_index,const T offset)304 void set_offset(const coor_index coor_index, const T offset) 305 {scale_offsets_[coor_index].set_offset(offset);} 306 307 //: get a specific scale value scale(const coor_index coor_index)308 T scale(const coor_index coor_index) const 309 {return scale_offsets_[coor_index].scale();} 310 311 //: get a specific offset value offset(const coor_index coor_index)312 T offset(const coor_index coor_index) const 313 {return scale_offsets_[coor_index].offset();} 314 315 //: get a specific scale_offset scl_off(const coor_index coor_index)316 vpgl_scale_offset<T> scl_off(const coor_index coor_index) const 317 {return scale_offsets_[coor_index];} 318 319 // --- Often useful for adjusting the camera --- 320 321 //:set u-v translation offset set_image_offset(const T u_off,const T v_off)322 void set_image_offset(const T u_off, const T v_off) 323 { scale_offsets_[U_INDX].set_offset(u_off); 324 scale_offsets_[V_INDX].set_offset(v_off); } 325 326 //:get u-v translation offset image_offset(T & u_off,T & v_off)327 void image_offset(T& u_off, T& v_off) const 328 {u_off = offset(U_INDX); v_off = offset(V_INDX);} 329 330 //:set u-v scale set_image_scale(const T u_scale,const T v_scale)331 void set_image_scale(const T u_scale, const T v_scale) 332 { scale_offsets_[U_INDX].set_scale(u_scale); 333 scale_offsets_[V_INDX].set_scale(v_scale); } 334 335 //:get u-v scale image_scale(T & u_scale,T & v_scale)336 void image_scale(T& u_scale, T& v_scale) 337 {u_scale = scale(U_INDX); v_scale = scale(V_INDX);} 338 339 // --- project 3D world point to 2D image point -- 340 341 //: The generic camera interface. u represents image column, v image row. 342 void project(const T x, const T y, const T z, T &u, T &v) const override; 343 344 //: Project a world point onto the image (vnl interface) 345 vnl_vector_fixed<T, 2> project(vnl_vector_fixed<T, 3> const& world_point) const; 346 347 //: Project a world point onto the image (vgl interface) 348 vgl_point_2d<T> project(vgl_point_3d<T> world_point) const; 349 350 // --- print & save camera --- 351 352 //: print camera parameters 353 virtual void print( 354 std::ostream& s = std::cout, 355 vpgl_rational_order output_order = vpgl_rational_order::VXL 356 ) const; 357 358 //: save camera parameters to a file 359 virtual bool save( 360 std::string cam_path, 361 vpgl_rational_order output_order = vpgl_rational_order::RPC00B 362 ) const; 363 364 //: write PVL (parameter) to output stream 365 virtual void write_pvl(std::ostream& s, vpgl_rational_order output_order) const; 366 367 // --- read camera --- 368 369 //: read from PVL (parameter value language) file/stream 370 bool read_pvl(std::string cam_path); 371 virtual bool read_pvl(std::istream& istr); 372 373 //: read from TXT file/stream 374 bool read_txt(std::string cam_path); 375 virtual bool read_txt(std::istream& istr); 376 377 protected: 378 // utilities 379 vnl_vector_fixed<T, 20> power_vector(const T x, const T y, const T z) const; 380 381 // members 382 vnl_matrix_fixed<T, 4, 20> rational_coeffs_; 383 std::vector<vpgl_scale_offset<T> > scale_offsets_; 384 }; 385 386 //: Write to stream 387 // \relatesalso vpgl_rational_camera 388 template <class T> 389 std::ostream& operator<<(std::ostream& s, const vpgl_rational_camera<T>& p); 390 391 //: Read from stream 392 // \relatesalso vpgl_rational_camera 393 template <class T> 394 std::istream& operator>>(std::istream& is, vpgl_rational_camera<T>& p); 395 396 //: Creates a rational camera from a PVL file 397 // \relatesalso vpgl_rational_camera 398 template <class T> 399 vpgl_rational_camera<T>* read_rational_camera(std::string cam_path); 400 401 //: Creates a rational camera from a PVL input stream 402 // \relatesalso vpgl_rational_camera 403 template <class T> 404 vpgl_rational_camera<T>* read_rational_camera(std::istream& istr); 405 406 //: Creates a rational camera from a TXT file 407 // \relatesalso vpgl_rational_camera 408 template <class T> 409 vpgl_rational_camera<T>* read_rational_camera_from_txt(std::string cam_path); 410 411 //: Creates a rational camera from a TXT input stream 412 // \relatesalso vpgl_rational_camera 413 template <class T> 414 vpgl_rational_camera<T>* read_rational_camera_from_txt(std::istream& istr); 415 416 417 #define VPGL_RATIONAL_CAMERA_INSTANTIATE(T) extern "please include vgl/vpgl_rational_camera.hxx first" 418 419 420 #endif // vpgl_rational_camera_h_ 421