00001
00002
00003
00004
00005 #if !defined (AQSENSE_SAL3DPP_ZMAP_HPP)
00006 #define AQSENSE_SAL3DPP_ZMAP_HPP
00007
00008 #if defined (_MSC_VER) && (_MSC_VER >= 1020)
00009 #pragma once
00010 #endif // _MSC_VER && _MSC_VER >= 1020
00011
00012 #include <stdexcept>
00013 #include <string>
00014 #include <sal3d/zmap.h>
00015 #include <sal3dpp/Common.hpp>
00016 #include <sal3dpp/COP.hpp>
00017 #include <sal3dpp/Frame.hpp>
00018
00019 namespace sal3d
00020 {
00061 class ZMap
00062 {
00064 typedef Array2D<float>::Row Row;
00065
00066 public:
00083 ZMap (const COP &cop, const ZMapFactors &zFactors,
00084 bool pointCells = false)
00085 {
00086 sal3d::Movement3D identity;
00087 _zmap.p = 0;
00088 Error e;
00089 if (pointCells)
00090 sal3d_zmap_new_from_moved_cop(cop.c_cop(),
00091 identity.c_movement_3d(), zFactors, &_zmap, &e.e);
00092 else
00093 sal3d_zmap_new_interpolated_from_moved_cop(
00094 cop.c_cop(), identity.c_movement_3d(), zFactors,
00095 &_zmap, &e.e);
00096 if ( e.e.value < 0 )
00097 throw e;
00098 }
00099
00117 ZMap (const COP &cop, const Movement3D &movement,
00118 const ZMapFactors &zFactors,
00119 bool pointCells = false)
00120 {
00121 _zmap.p = 0;
00122 Error e;
00123 if (pointCells)
00124 sal3d_zmap_new_from_moved_cop(cop.c_cop(),
00125 movement.c_movement_3d(), zFactors, &_zmap, &e.e);
00126 else
00127 sal3d_zmap_new_interpolated_from_moved_cop(
00128 cop.c_cop(), movement.c_movement_3d(), zFactors,
00129 &_zmap, &e.e);
00130 if ( e.e.value < 0 )
00131 throw e;
00132 }
00133
00144 ZMap (const ZMapFactors &zFactors)
00145 {
00146 _zmap.p = 0;
00147 Error e;
00148 sal3d_zmap_new (zFactors, &_zmap, &e.e);
00149 if ( e.e.value < 0 )
00150 throw e;
00151 }
00152
00162 explicit ZMap (const std::string &fileName)
00163 {
00164 _zmap.p = 0;
00165 Error e;
00166 sal3d_zmap_new_from_file (fileName.c_str (),
00167 &_zmap, &e.e);
00168 if ( e.e.value < 0 )
00169 throw e;
00170 }
00171
00184 explicit ZMap (sal3d_zmap zmap):
00185 _zmap(zmap)
00186 {
00187 Error e;
00188 sal3d_zmap_check_validity(zmap, &e.e);
00189 if ( e.e.value < 0 )
00190 throw e;
00191 }
00192
00200 ZMap (const ZMap &zmap):
00201 _zmap (zmap._zmap)
00202 {
00203 Error e;
00204 sal3d_zmap_share(_zmap, &e.e);
00205 if ( e.e.value < 0 )
00206 throw e;
00207 }
00208
00215 ~ZMap ()
00216 {
00217 sal3d_zmap_release(_zmap);
00218 }
00219
00230 ZMap
00231 copy() const
00232 {
00233 sal3d_zmap zmap;
00234 Error e;
00235 sal3d_zmap_duplicate(_zmap, &zmap, &e.e);
00236 if ( e.e.value < 0 )
00237 throw e;
00238 return ZMap(zmap);
00239 }
00240
00250 sal3d_zmap
00251 c_zmap() const
00252 {
00253 return _zmap;
00254 }
00255
00263 ZMapFactors
00264 getFactors () const
00265 {
00266 ZMapFactors zFactors;
00267
00268 Error e;
00269 sal3d_zmap_get_factors(_zmap, &zFactors, &e.e);
00270 if ( e.e.value < 0 )
00271 throw e;
00272
00273 return zFactors;
00274 }
00275
00307 Frame
00308 getFrame (int dataType = SAL3D_FLOAT_DATA_TYPE(32),
00309 float *zmin = 0, float *zmax = 0) const
00310 {
00311 sal3d_frame c_frame;
00312 Error e;
00313 sal3d_zmap_get_frame(_zmap, dataType, 0, 0, &c_frame, zmin,
00314 zmax, &e.e);
00315 if ( e.e.value < 0 )
00316 throw e;
00317
00318 return Frame(c_frame);
00319 }
00320
00349 Frame
00350 getFrame (int dataType, float zmin, float zmax) const
00351 {
00352 sal3d_frame c_frame;
00353 Error e;
00354 sal3d_zmap_get_frame(_zmap, dataType, &zmin, &zmax, &c_frame,
00355 0, 0, &e.e);
00356 if ( e.e.value < 0 )
00357 throw e;
00358
00359 return Frame(c_frame);
00360 }
00361
00371 float
00372 getX(int iy) const
00373 {
00374 ZMapFactors zFactors = getFactors();
00375 return zFactors.xmin + (zFactors.xmax - zFactors.xmin) *
00376 iy / (zFactors.height - 1);
00377 }
00378
00388 float
00389 getY(int ix) const
00390 {
00391 ZMapFactors zFactors = getFactors();
00392 return zFactors.ymin + (zFactors.ymax - zFactors.ymin) *
00393 ix / (zFactors.width - 1);
00394 }
00395
00407 float
00408 getZ(int ix, int iy) const
00409 {
00410 return (*this)[iy][ix];
00411 }
00412
00425 void
00426 getPoint(int ix, int iy, float &x, float &y, float &z) const
00427 {
00428 x = getX(iy);
00429 y = getY(ix);
00430 z = (*this)[iy][ix];
00431 }
00432
00443 const Row
00444 operator[] (int iy) const
00445 {
00446 float *data;
00447 ptrdiff_t pitch;
00448
00449 Error e;
00450 sal3d_zmap_get_data(_zmap, &data, &pitch, &e.e);
00451 if ( e.e.value < 0 )
00452 throw e;
00453
00454 ZMapFactors zFactors = (*this).getFactors();
00455
00456 return Row(data + pitch * iy, zFactors.width);
00457 }
00458
00467 Row
00468 operator[] (int iy)
00469 {
00470 return static_cast<const ZMap &>(*this)[iy];
00471 }
00472
00480 ZMap &
00481 operator= (const ZMap &rhs)
00482 {
00483 Error e;
00484
00485 sal3d_zmap_share(rhs._zmap, &e.e);
00486 if ( e.e.value < 0 )
00487 throw e;
00488 sal3d_zmap_release(_zmap);
00489 _zmap = rhs._zmap;
00490
00491 return *this;
00492 }
00493
00515 ZMap
00516 getDistanceToZMap(const ZMap &zmap) const
00517 {
00518 sal3d_zmap diff;
00519 Error e;
00520 sal3d_zmap_get_distance_to_zmap(_zmap, zmap._zmap,
00521 &diff, &e.e);
00522 if ( e.e.value < 0 )
00523 throw e;
00524 return ZMap(diff);
00525 }
00526
00553 ZMap
00554 getFineDistanceToZMap(const ZMap &zmap, int windowSize) const
00555 {
00556 sal3d_zmap diff;
00557 Error e;
00558 sal3d_zmap_get_fine_distance_to_zmap(_zmap,
00559 zmap._zmap, windowSize, &diff, &e.e);
00560 if ( e.e.value < 0 )
00561 throw e;
00562 return ZMap(diff);
00563 }
00564
00588 ZMap
00589 getDistanceToMovedCOP(const COP &cop,
00590 const Movement3D &movement) const
00591 {
00592 ZMap copzmap(cop, movement, getFactors());
00593
00594 return getDistanceToZMap(copzmap);
00595 }
00596
00625 ZMap
00626 getFineDistanceToMovedCOP(const COP &cop,
00627 const Movement3D &movement, int windowSize) const
00628 {
00629 ZMap copzmap(cop, movement, getFactors());
00630
00631 return getFineDistanceToZMap(copzmap, windowSize);
00632 }
00633
00643 void
00644 saveToFile (const std::string &fileName) const
00645 {
00646 Error e;
00647 sal3d_zmap_save_to_file(_zmap,
00648 fileName.c_str(), &e.e);
00649 if ( e.e.value < 0 )
00650 throw e;
00651 }
00652
00653 private:
00655 sal3d_zmap _zmap;
00656 };
00657 }
00658
00659 #endif // !AQSENSE_SAL3DPP_ZMAP_HPP
by 
1.7.6.1