00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "PSCalib/GeometryAccess.h"
00017 #include "PSCalib/GeometryObject.h"
00018
00019
00020
00021
00022
00023 #include <iostream>
00024 #include <fstream>
00025 #include <sstream>
00026 #include <iomanip>
00027
00028
00029
00030
00031
00032 #include "MsgLogger/MsgLogger.h"
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042 namespace PSCalib {
00043
00044
00045
00046
00047
00048 GeometryAccess::GeometryAccess (const std::string& path, unsigned pbits)
00049 : m_path(path)
00050 , m_pbits(pbits)
00051 , p_iX(0)
00052 , p_iY(0)
00053 , p_image(0)
00054 , p_XatZ(0)
00055 , p_YatZ(0)
00056 {
00057 if(m_pbits & 1) MsgLog(name(), info, "m_pbits = " << m_pbits);
00058
00059 load_pars_from_file();
00060
00061 if(m_pbits & 2) print_list_of_geos();
00062 if(m_pbits & 4) print_list_of_geos_children();
00063 if(m_pbits & 8) print_comments_from_dict();
00064 }
00065
00066
00067
00068
00069
00070 GeometryAccess::~GeometryAccess ()
00071 {
00072 delete [] p_iX;
00073 delete [] p_iY;
00074 delete p_image;
00075 delete [] p_XatZ;
00076 delete [] p_YatZ;
00077 }
00078
00079
00080 void GeometryAccess::load_pars_from_file(const std::string& path)
00081 {
00082 m_dict_of_comments.clear();
00083 v_list_of_geos.clear();
00084 v_list_of_geos.reserve(100);
00085
00086 if(! path.empty()) m_path = path;
00087
00088 std::ifstream f(m_path.c_str());
00089
00090 if(not f.good()) { MsgLog(name(), error, "Calibration file " << m_path << " does not exist"); }
00091 else { if(m_pbits & 1) MsgLog(name(), info, "load_pars_from_file(): " << m_path); }
00092
00093 std::string line;
00094 while (std::getline(f, line)) {
00095 if(m_pbits & 256) std::cout << line << '\n';
00096 if(line.empty()) continue;
00097 if(line[0] == '#') {
00098 add_comment_to_dict(line);
00099 continue;
00100 }
00101
00102 v_list_of_geos.push_back( parse_line(line) );
00103 }
00104
00105 f.close();
00106
00107 set_relations();
00108 }
00109
00110
00111
00112 void GeometryAccess::save_pars_in_file(const std::string& path)
00113 {
00114 if(m_pbits & 1) MsgLog(name(), info, "Save pars in file " << path.c_str());
00115
00116 std::stringstream ss;
00117
00118
00119 std::map<int, std::string>::iterator iter;
00120 for (iter = m_dict_of_comments.begin(); iter != m_dict_of_comments.end(); ++iter) {
00121
00122 ss << "# " << iter->second << '\n';
00123 }
00124
00125 ss << '\n';
00126
00127
00128 for(std::vector<GeometryAccess::shpGO>::iterator it = v_list_of_geos.begin();
00129 it != v_list_of_geos.end(); ++it) {
00130 if( (*it)->get_parent_name().empty() ) continue;
00131 ss << (*it)->str_data() << '\n';
00132 }
00133
00134 std::ofstream out(path.c_str());
00135 out << ss.str();
00136 out.close();
00137
00138 if(m_pbits & 64) std::cout << ss.str();
00139 }
00140
00141
00142
00143 void GeometryAccess::add_comment_to_dict(const std::string& line)
00144 {
00145 std::size_t p1 = line.find_first_not_of("#");
00146 std::size_t p2 = line.find_first_not_of(" ", p1);
00147
00148
00149
00150
00151
00152
00153
00154 if (p1 == std::string::npos) return;
00155
00156 int ind = m_dict_of_comments.size();
00157
00158 if (p2 == std::string::npos) {
00159 m_dict_of_comments[ind] = std::string();
00160 return;
00161 }
00162
00163 std::string endline(line, p2);
00164 m_dict_of_comments[ind] = endline;
00165 }
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198 GeometryAccess::shpGO GeometryAccess::parse_line(const std::string& line)
00199 {
00200 std::string pname;
00201 unsigned pindex;
00202 std::string oname;
00203 unsigned oindex;
00204 double x0;
00205 double y0;
00206 double z0;
00207 double rot_z;
00208 double rot_y;
00209 double rot_x;
00210 double tilt_z;
00211 double tilt_y;
00212 double tilt_x;
00213
00214 std::stringstream ss(line);
00215
00216 if(ss >> pname >> pindex >> oname >> oindex >> x0 >> y0 >> z0
00217 >> rot_z >> rot_y >> rot_x >> tilt_z >> tilt_y >> tilt_x) {
00218 GeometryAccess::shpGO shp( new GeometryObject (pname,
00219 pindex,
00220 oname,
00221 oindex,
00222 x0,
00223 y0,
00224 z0,
00225 rot_z,
00226 rot_y,
00227 rot_x,
00228 tilt_z,
00229 tilt_y,
00230 tilt_x
00231 ));
00232 if(m_pbits & 256) shp->print_geo();
00233 return shp;
00234 }
00235 else {
00236 std::string msg = "parse_line(...) can't parse line: " + line;
00237
00238 MsgLog(name(), error, msg);
00239 return GeometryAccess::shpGO();
00240 }
00241 }
00242
00243
00244
00245 GeometryAccess::shpGO GeometryAccess::find_parent(const GeometryAccess::shpGO& geobj)
00246 {
00247 for(std::vector<GeometryAccess::shpGO>::iterator it = v_list_of_geos.begin();
00248 it != v_list_of_geos.end(); ++it) {
00249 if(*it == geobj) continue;
00250 if( (*it)->get_geo_index() == geobj->get_parent_index()
00251 && (*it)->get_geo_name() == geobj->get_parent_name() ) {
00252 return (*it);
00253 }
00254 }
00255
00256
00257
00258
00259 if(m_pbits & 256) std::cout << " GeometryAccess::find_parent(...): parent is not found..."
00260 << geobj->get_parent_name() << " idx:" << geobj->get_parent_index() << '\n';
00261
00262 if( ! geobj->get_parent_name().empty() ) {
00263
00264 if(m_pbits & 256) std::cout << " create one with name:" << geobj->get_parent_name()
00265 << " idx:" << geobj->get_parent_index() << '\n';
00266
00267 GeometryAccess::shpGO shp_top_parent( new GeometryObject (std::string(),
00268 0,
00269 geobj->get_parent_name(),
00270 geobj->get_parent_index()));
00271
00272 v_list_of_geos.push_back( shp_top_parent );
00273 return shp_top_parent;
00274 }
00275
00276 if(m_pbits & 256) std::cout << " return empty shpGO() for the very top parent\n";
00277 return GeometryAccess::shpGO();
00278 }
00279
00280
00281
00282 void GeometryAccess::set_relations()
00283 {
00284 if(m_pbits & 16) MsgLog(name(), info, "Begin set_relations(): size of the list:" << v_list_of_geos.size());
00285 for(std::vector<GeometryAccess::shpGO>::iterator it = v_list_of_geos.begin();
00286 it != v_list_of_geos.end(); ++it) {
00287
00288 GeometryAccess::shpGO shp_parent = find_parent(*it);
00289
00290
00291 if( shp_parent == GeometryAccess::shpGO() ) continue;
00292
00293 (*it)->set_parent(shp_parent);
00294 shp_parent->add_child(*it);
00295
00296 if(m_pbits & 16)
00297 std::cout << "\n geo:" << std::setw(10) << (*it) -> get_geo_name()
00298 << " : " << (*it) -> get_geo_index()
00299 << " has parent:" << std::setw(10) << shp_parent -> get_geo_name()
00300 << " : " << shp_parent -> get_geo_index()
00301 << '\n';
00302 }
00303 }
00304
00305
00306
00307 GeometryAccess::shpGO GeometryAccess::get_geo(const std::string& oname, const unsigned& oindex)
00308 {
00309 for(std::vector<GeometryAccess::shpGO>::iterator it = v_list_of_geos.begin();
00310 it != v_list_of_geos.end(); ++it) {
00311 if( (*it)->get_geo_index() == oindex
00312 && (*it)->get_geo_name() == oname )
00313 return (*it);
00314 }
00315 return GeometryAccess::shpGO();
00316 }
00317
00318
00319
00320 GeometryAccess::shpGO GeometryAccess::get_top_geo()
00321 {
00322 return v_list_of_geos.back();
00323 }
00324
00325
00326
00327 void
00328 GeometryAccess::get_pixel_coords(const double*& X,
00329 const double*& Y,
00330 const double*& Z,
00331 unsigned& size,
00332 const std::string& oname,
00333 const unsigned& oindex,
00334 const bool do_tilt,
00335 const bool do_eval)
00336 {
00337 GeometryAccess::shpGO geo = (oname.empty()) ? get_top_geo() : get_geo(oname, oindex);
00338 if(m_pbits & 32) {
00339 std::stringstream ss; ss << "get_pixel_coords(...) for geo:\n" << geo -> string_geo_children()
00340 << " do_tilt: " << do_tilt << " do_eval: " << do_eval;
00341 MsgLog(name(), info, ss.str());
00342 }
00343 geo -> get_pixel_coords(X, Y, Z, size, do_tilt, do_eval);
00344 }
00345
00346
00347
00348 void
00349 GeometryAccess::get_pixel_xy_at_z(const double*& XatZ,
00350 const double*& YatZ,
00351 unsigned& size,
00352 const double& Zplain,
00353 const std::string& oname,
00354 const unsigned& oindex)
00355 {
00356 const double* X;
00357 const double* Y;
00358 const double* Z;
00359
00360 const bool do_tilt=true;
00361 const bool do_eval=true;
00362 get_pixel_coords(X,Y,Z,size,oname,oindex,do_tilt,do_eval);
00363
00364 if (!size) {
00365 MsgLog(name(), error, "get_pixel_coords returns ZERO size coordinate array...");
00366 abort();
00367 }
00368
00369 if (p_XatZ) delete [] p_XatZ;
00370 if (p_YatZ) delete [] p_YatZ;
00371
00372 p_XatZ = new double[size];
00373 p_YatZ = new double[size];
00374
00375
00376 double Z0 = 0;
00377 if (Zplain) Z0 = Zplain;
00378 else {
00379 for(unsigned i=0; i<size; ++i) Z0 += Z[i];
00380 Z0 = Z0/size;
00381 }
00382
00383 if(fabs(Z0) < 1000) {
00384 XatZ = X;
00385 YatZ = Y;
00386 return;
00387 }
00388
00389 double R;
00390 for(unsigned i=0; i<size; ++i) {
00391 if (Z[i]) {
00392 R = Z0/Z[i];
00393 p_XatZ[i] = X[i]*R;
00394 p_YatZ[i] = Y[i]*R;
00395 } else {
00396 p_XatZ[i] = X[i];
00397 p_YatZ[i] = Y[i];
00398 }
00399 }
00400 XatZ = p_XatZ;
00401 YatZ = p_YatZ;
00402 }
00403
00404
00405
00406 void
00407 GeometryAccess::get_pixel_areas(const double*& A,
00408 unsigned& size,
00409 const std::string& oname,
00410 const unsigned& oindex)
00411 {
00412 GeometryAccess::shpGO geo = (oname.empty()) ? get_top_geo() : get_geo(oname, oindex);
00413 if(m_pbits & 32) {
00414 std::string msg = "get_pixel_areas(...) for geo:\n" + geo -> string_geo_children();
00415 MsgLog(name(), info, msg);
00416 }
00417 geo -> get_pixel_areas(A, size);
00418 }
00419
00420
00421
00422 void
00423 GeometryAccess::get_pixel_mask(const int*& mask,
00424 unsigned& size,
00425 const std::string& oname,
00426 const unsigned& oindex,
00427 const unsigned& mbits)
00428 {
00429
00430
00431 GeometryAccess::shpGO geo = (oname.empty()) ? get_top_geo() : get_geo(oname, oindex);
00432 if(m_pbits & 32) {
00433 std::string msg = "get_pixel_areas(...) for geo:\n" + geo -> string_geo_children();
00434 MsgLog(name(), info, msg);
00435 }
00436 geo -> get_pixel_mask(mask, size, mbits);
00437 }
00438
00439
00440
00441 double
00442 GeometryAccess::get_pixel_scale_size(const std::string& oname,
00443 const unsigned& oindex)
00444 {
00445 GeometryAccess::shpGO geo = (oname.empty()) ? get_top_geo() : get_geo(oname, oindex);
00446 return geo -> get_pixel_scale_size();
00447 }
00448
00449
00450
00451 void
00452 GeometryAccess::set_geo_pars(const std::string& oname,
00453 const unsigned& oindex,
00454 const double& x0,
00455 const double& y0,
00456 const double& z0,
00457 const double& rot_z,
00458 const double& rot_y,
00459 const double& rot_x,
00460 const double& tilt_z,
00461 const double& tilt_y,
00462 const double& tilt_x
00463 )
00464 {
00465 GeometryAccess::shpGO geo = (oname.empty()) ? get_top_geo() : get_geo(oname, oindex);
00466 geo -> set_geo_pars(x0, y0, z0, rot_z, rot_y, rot_x, tilt_z, tilt_y, tilt_x);
00467 }
00468
00469
00470
00471 void
00472 GeometryAccess::move_geo(const std::string& oname,
00473 const unsigned& oindex,
00474 const double& dx,
00475 const double& dy,
00476 const double& dz
00477 )
00478 {
00479 GeometryAccess::shpGO geo = (oname.empty()) ? get_top_geo() : get_geo(oname, oindex);
00480 geo -> move_geo(dx, dy, dz);
00481 }
00482
00483
00484
00485 void
00486 GeometryAccess::tilt_geo(const std::string& oname,
00487 const unsigned& oindex,
00488 const double& dt_x,
00489 const double& dt_y,
00490 const double& dt_z
00491 )
00492 {
00493 GeometryAccess::shpGO geo = (oname.empty()) ? get_top_geo() : get_geo(oname, oindex);
00494 geo -> tilt_geo(dt_x, dt_y, dt_z);
00495 }
00496
00497
00498
00499 void GeometryAccess::print_list_of_geos()
00500 {
00501 std::stringstream ss; ss << "print_list_of_geos():";
00502 if( v_list_of_geos.empty() ) ss << "List of geos is empty...";
00503 for(std::vector<GeometryAccess::shpGO>::iterator it = v_list_of_geos.begin();
00504 it != v_list_of_geos.end(); ++it) {
00505 ss << '\n' << (*it)->string_geo();
00506 }
00507
00508 MsgLog(name(), info, ss.str());
00509 }
00510
00511
00512
00513 void GeometryAccess::print_list_of_geos_children()
00514 {
00515 std::stringstream ss; ss << "print_list_of_geos_children(): ";
00516 if( v_list_of_geos.empty() ) ss << "List of geos is empty...";
00517
00518 for(std::vector<GeometryAccess::shpGO>::iterator it = v_list_of_geos.begin();
00519 it != v_list_of_geos.end(); ++it) {
00520 ss << '\n' << (*it)->string_geo_children();
00521 }
00522
00523 MsgLog(name(), info, ss.str());
00524 }
00525
00526
00527
00528 void GeometryAccess::print_comments_from_dict()
00529 {
00530 std::stringstream ss; ss << "print_comments_from_dict():\n";
00531
00532 std::map<int, std::string>::iterator iter;
00533
00534 for (iter = m_dict_of_comments.begin(); iter != m_dict_of_comments.end(); ++iter) {
00535 ss << " key:" << std::setw(4) << std::left << iter->first;
00536 ss << " val:" << iter->second << '\n';
00537 }
00538
00539 MsgLog(name(), info, ss.str());
00540 }
00541
00542
00543 void
00544 GeometryAccess::print_pixel_coords(const std::string& oname,
00545 const unsigned& oindex)
00546 {
00547 const double* X;
00548 const double* Y;
00549 const double* Z;
00550 unsigned size;
00551 const bool do_tilt=true;
00552 get_pixel_coords(X,Y,Z,size,oname,oindex,do_tilt);
00553
00554 std::stringstream ss; ss << "print_pixel_coords():\n"
00555 << "size=" << size << '\n' << std::fixed << std::setprecision(1);
00556 ss << "X: "; for(unsigned i=0; i<10; ++i) ss << std::setw(10) << X[i] << ", "; ss << "...\n";
00557 ss << "Y: "; for(unsigned i=0; i<10; ++i) ss << std::setw(10) << Y[i] << ", "; ss << "...\n";
00558 ss << "Z: "; for(unsigned i=0; i<10; ++i) ss << std::setw(10) << Z[i] << ", "; ss << "...\n";
00559
00560 MsgLog(name(), info, ss.str());
00561 }
00562
00563
00564
00565
00566 void
00567 GeometryAccess::get_pixel_coord_indexes(const unsigned *& iX,
00568 const unsigned *& iY,
00569 unsigned& size,
00570 const std::string& oname,
00571 const unsigned& oindex,
00572 const double& pix_scale_size_um,
00573 const int* xy0_off_pix,
00574 const bool do_tilt)
00575 {
00576 const double* X;
00577 const double* Y;
00578 const double* Z;
00579
00580 get_pixel_coords(X,Y,Z,size,oname,oindex,do_tilt);
00581
00582 double pix_size = (pix_scale_size_um) ? pix_scale_size_um : get_pixel_scale_size(oname, oindex);
00583
00584 if (p_iX) delete [] p_iX;
00585 if (p_iY) delete [] p_iY;
00586
00587 p_iX = new unsigned[size];
00588 p_iY = new unsigned[size];
00589
00590 if (xy0_off_pix) {
00591
00592 double x_off_um = xy0_off_pix[0] * pix_size;
00593 double y_off_um = xy0_off_pix[1] * pix_size;
00594
00595 double x_min=0; for(unsigned i=0; i<size; ++i) { if (X[i] + x_off_um < x_min) x_min = X[i] + x_off_um; } x_off_um -= x_min - pix_size/2;
00596 double y_min=0; for(unsigned i=0; i<size; ++i) { if (Y[i] + y_off_um < y_min) y_min = Y[i] + y_off_um; } y_off_um -= y_min - pix_size/2;
00597
00598 for(unsigned i=0; i<size; ++i) {
00599 p_iX[i] = (unsigned)((X[i] + x_off_um) / pix_size);
00600 p_iY[i] = (unsigned)((Y[i] + y_off_um) / pix_size);
00601 }
00602 }
00603 else {
00604
00605 double x_min=X[0]; for(unsigned i=0; i<size; ++i) { if (X[i] < x_min) x_min = X[i]; } x_min -= pix_size/2;
00606 double y_min=Y[0]; for(unsigned i=0; i<size; ++i) { if (Y[i] < y_min) y_min = Y[i]; } y_min -= pix_size/2;
00607 for(unsigned i=0; i<size; ++i) {
00608 p_iX[i] = (unsigned)((X[i] - x_min) / pix_size);
00609 p_iY[i] = (unsigned)((Y[i] - y_min) / pix_size);
00610 }
00611 }
00612
00613 iX = p_iX;
00614 iY = p_iY;
00615 }
00616
00617
00618
00619 void
00620 GeometryAccess::get_pixel_xy_inds_at_z(const unsigned *& iX,
00621 const unsigned *& iY,
00622 unsigned& size,
00623 const double& Zplain,
00624 const std::string& oname,
00625 const unsigned& oindex,
00626 const double& pix_scale_size_um,
00627 const int* xy0_off_pix)
00628 {
00629 const double* X;
00630 const double* Y;
00631
00632 get_pixel_xy_at_z(X,Y,size,Zplain,oname,oindex);
00633
00634 double pix_size = (pix_scale_size_um) ? pix_scale_size_um : get_pixel_scale_size(oname, oindex);
00635
00636 if (p_iX) delete [] p_iX;
00637 if (p_iY) delete [] p_iY;
00638
00639 p_iX = new unsigned[size];
00640 p_iY = new unsigned[size];
00641
00642 if (xy0_off_pix) {
00643
00644 double x_off_um = xy0_off_pix[0] * pix_size;
00645 double y_off_um = xy0_off_pix[1] * pix_size;
00646
00647 double x_min=0; for(unsigned i=0; i<size; ++i) { if (X[i] + x_off_um < x_min) x_min = X[i] + x_off_um; } x_off_um -= x_min - pix_size/2;
00648 double y_min=0; for(unsigned i=0; i<size; ++i) { if (Y[i] + y_off_um < y_min) y_min = Y[i] + y_off_um; } y_off_um -= y_min - pix_size/2;
00649
00650 for(unsigned i=0; i<size; ++i) {
00651 p_iX[i] = (unsigned)((X[i] + x_off_um) / pix_size);
00652 p_iY[i] = (unsigned)((Y[i] + y_off_um) / pix_size);
00653 }
00654 }
00655 else {
00656
00657 double x_min=X[0]; for(unsigned i=0; i<size; ++i) { if (X[i] < x_min) x_min = X[i]; } x_min -= pix_size/2;
00658 double y_min=Y[0]; for(unsigned i=0; i<size; ++i) { if (Y[i] < y_min) y_min = Y[i]; } y_min -= pix_size/2;
00659 for(unsigned i=0; i<size; ++i) {
00660 p_iX[i] = (unsigned)((X[i] - x_min) / pix_size);
00661 p_iY[i] = (unsigned)((Y[i] - y_min) / pix_size);
00662 }
00663 }
00664
00665 iX = p_iX;
00666 iY = p_iY;
00667 }
00668
00669
00670
00671
00672 ndarray<GeometryAccess::image_t, 2> &
00673 GeometryAccess::ref_img_from_pixel_arrays(const unsigned*& iX,
00674 const unsigned*& iY,
00675 const double* W,
00676 const unsigned& size)
00677 {
00678 unsigned ix_max=iX[0]; for(unsigned i=0; i<size; ++i) { if (iX[i] > ix_max) ix_max = iX[i]; } ix_max++;
00679 unsigned iy_max=iY[0]; for(unsigned i=0; i<size; ++i) { if (iY[i] > iy_max) iy_max = iY[i]; } iy_max++;
00680
00681 if(p_image) delete p_image;
00682
00683 unsigned shape[2] = {ix_max, iy_max};
00684 p_image = new ndarray<GeometryAccess::image_t, 2> (shape);
00685 ndarray<GeometryAccess::image_t, 2> img = *p_image;
00686
00687 for(ndarray<GeometryAccess::image_t, 2>::iterator it=img.begin(); it!=img.end(); it++) { *it = 0; }
00688
00689 if (W) for(unsigned i=0; i<size; ++i) img[iX[i]][iY[i]] = (GeometryAccess::image_t) W[i];
00690 else for(unsigned i=0; i<size; ++i) img[iX[i]][iY[i]] = 1;
00691 return *p_image;
00692 }
00693
00694
00695
00696
00697
00698
00699
00700 ndarray<GeometryAccess::image_t, 2>
00701 GeometryAccess::img_from_pixel_arrays(const unsigned*& iX,
00702 const unsigned*& iY,
00703 const double* W,
00704 const unsigned& size)
00705 {
00706 unsigned ix_max=iX[0]; for(unsigned i=0; i<size; ++i) { if (iX[i] > ix_max) ix_max = iX[i]; } ix_max++;
00707 unsigned iy_max=iY[0]; for(unsigned i=0; i<size; ++i) { if (iY[i] > iy_max) iy_max = iY[i]; } iy_max++;
00708
00709 ndarray<GeometryAccess::image_t, 2> img = make_ndarray<GeometryAccess::image_t>(ix_max, iy_max);
00710
00711 for(ndarray<GeometryAccess::image_t, 2>::iterator it=img.begin(); it!=img.end(); it++) { *it = 0; }
00712
00713 if (W) for(unsigned i=0; i<size; ++i) img[iX[i]][iY[i]] = (GeometryAccess::image_t) W[i];
00714 else for(unsigned i=0; i<size; ++i) img[iX[i]][iY[i]] = 1;
00715 return img;
00716 }
00717
00718
00719
00720
00721 }
00722
00723
00724
00725