From 357975f78ed33c73a7de3a77598dd80d313e4c63 Mon Sep 17 00:00:00 2001 From: Razvan Mihalyi Date: Mon, 19 Nov 2012 13:34:43 +0100 Subject: [PATCH] update svn to r776 --- ...c08e35fb37efbfe34b574c1b43217904e.svn-base | 118 ++ ...2028f2a8b91166bea8d02b8459483fe48.svn-base | 893 ++++++++++++++ ...2e3484f4b8a583e474ef9d100a4e18b87.svn-base | 386 ++++++ ...798e851dc40765533de4663e756f74607.svn-base | 39 + ...a2b9c7b114a8d0cbbeb1af1d6aa426f6e.svn-base | 68 ++ ...5b8ef2733188c331eba089f8ff064bfcc.svn-base | 186 +++ ...af16ec9f8e2193fd95818a6115330471c.svn-base | 42 + ...1c2c72a73e576e447d85ba661b728eff3.svn-base | 55 + ...cdfed9982db5b177391c5ecb352cfb0d5.svn-base | 851 +++++++++++++ ...097e810110fac54c59f2cdf6621e44165.svn-base | 1077 +++++++++++++++++ ...8738858d3b9331e25d7914925c340b330.svn-base | 13 + ...4ee8c1c126617fb7272fc790d76de6aae.svn-base | 129 ++ ...360d62e6f8cd63f8d04565e8f5bc0841c.svn-base | 35 + ...1704e7bc495aa3c6879088dcbbf71c484.svn-base | 7 + ...6b9fcba7dbe7d7b8a1c555930862af850.svn-base | 37 + ...f69a7dcb1d3eb01b3259fea032402080f.svn-base | 155 +++ ...740c95b88c9588a5244f8156b4c4d63fd.svn-base | 194 +++ ...4d39a4e57120a8ee05465615fa460a92f.svn-base | 48 + ...1afe9990bebcc4fb43e0efaf52fd2deb9.svn-base | 227 ++++ ...2aa9b57ff566abb39d885906a01ed2ce8.svn-base | 26 + ...d7cba6c3b4bdd91895a682bc62d2efba7.svn-base | 275 +++++ ...a612dff3c260324085b9e15e86162ebdb.svn-base | 996 +++++++++++++++ ...f445696a09d44974b7fbc00582316ea46.svn-base | 424 +++++++ ...a356300cae6565726ccc1c7d05b04365a.svn-base | 610 ++++++++++ ...2546d685f14bc88c1b412a66c2e069b85.svn-base | 64 + .svn/wc.db | Bin 631808 -> 637952 bytes CMakeLists.txt | 27 +- README.scanserver | 2 +- include/normals/normals.h | 155 +++ include/scanio/scan_io_ply.h | 39 + include/scanio/scan_io_xyzr.h | 42 + include/slam6d/basicScan.h | 1 + include/slam6d/data_types.h | 1 + include/slam6d/globals.icc | 248 ++-- include/slam6d/icp6D.h | 7 +- include/slam6d/kd.h | 22 +- include/slam6d/kdManaged.h | 2 + include/slam6d/kdMeta.h | 2 + include/slam6d/kdTreeImpl.h | 174 ++- include/slam6d/kdparams.h | 18 +- include/slam6d/managedScan.h | 1 + include/slam6d/metaScan.h | 1 + include/slam6d/pairingMode.h | 10 + include/slam6d/point.h | 6 + include/slam6d/point_type.h | 17 + include/slam6d/scan.h | 37 +- include/slam6d/searchTree.h | 10 +- know_issues.txt | 35 + src/normals/CMakeLists.txt | 12 +- src/normals/calc_normals.cc | 275 +++++ src/normals/normals.cc | 901 ++++++-------- src/scanio/CMakeLists.txt | 2 +- src/scanio/scan_io_ply.cc | 227 ++++ src/scanio/scan_io_xyzr.cc | 194 +++ src/segmentation/CMakeLists.txt | 8 +- src/show/CMakeLists.txt | 2 +- src/show/compacttree.cc | 4 +- src/show/scancolormanager.cc | 12 - src/show/show_common.cc | 38 +- src/slam6d/CMakeLists.txt | 8 +- src/slam6d/basicScan.cc | 12 +- src/slam6d/fbr/CMakeLists.txt | 10 +- src/slam6d/icp6D.cc | 20 +- src/slam6d/kd.cc | 68 +- src/slam6d/kdManaged.cc | 10 + src/slam6d/kdMeta.cc | 10 + src/slam6d/point_type.cc | 52 +- src/slam6d/scan.cc | 325 ++--- src/slam6d/searchTree.cc | 79 +- src/slam6d/slam6D.cc | 796 ++++++------ src/thermo/CMakeLists.txt | 1 - 71 files changed, 9556 insertions(+), 1322 deletions(-) create mode 100644 .svn/pristine/0f/0f93101c08e35fb37efbfe34b574c1b43217904e.svn-base create mode 100644 .svn/pristine/30/30442352028f2a8b91166bea8d02b8459483fe48.svn-base create mode 100644 .svn/pristine/35/35452722e3484f4b8a583e474ef9d100a4e18b87.svn-base create mode 100644 .svn/pristine/3b/3bb9b87798e851dc40765533de4663e756f74607.svn-base create mode 100644 .svn/pristine/47/479c520a2b9c7b114a8d0cbbeb1af1d6aa426f6e.svn-base create mode 100644 .svn/pristine/4b/4baca755b8ef2733188c331eba089f8ff064bfcc.svn-base create mode 100644 .svn/pristine/57/57865b7af16ec9f8e2193fd95818a6115330471c.svn-base create mode 100644 .svn/pristine/58/583c8601c2c72a73e576e447d85ba661b728eff3.svn-base create mode 100644 .svn/pristine/58/584b0b2cdfed9982db5b177391c5ecb352cfb0d5.svn-base create mode 100644 .svn/pristine/5d/5d4ec32097e810110fac54c59f2cdf6621e44165.svn-base create mode 100644 .svn/pristine/6e/6ee9fe28738858d3b9331e25d7914925c340b330.svn-base create mode 100644 .svn/pristine/78/78a71ec4ee8c1c126617fb7272fc790d76de6aae.svn-base create mode 100644 .svn/pristine/88/88689f7360d62e6f8cd63f8d04565e8f5bc0841c.svn-base create mode 100644 .svn/pristine/91/91cd31e1704e7bc495aa3c6879088dcbbf71c484.svn-base create mode 100644 .svn/pristine/92/92778e36b9fcba7dbe7d7b8a1c555930862af850.svn-base create mode 100644 .svn/pristine/92/92ef8a9f69a7dcb1d3eb01b3259fea032402080f.svn-base create mode 100644 .svn/pristine/97/97bbbc7740c95b88c9588a5244f8156b4c4d63fd.svn-base create mode 100644 .svn/pristine/97/97df0014d39a4e57120a8ee05465615fa460a92f.svn-base create mode 100644 .svn/pristine/9e/9e481491afe9990bebcc4fb43e0efaf52fd2deb9.svn-base create mode 100644 .svn/pristine/aa/aa64aec2aa9b57ff566abb39d885906a01ed2ce8.svn-base create mode 100644 .svn/pristine/b0/b03ec22d7cba6c3b4bdd91895a682bc62d2efba7.svn-base create mode 100644 .svn/pristine/be/becc3bca612dff3c260324085b9e15e86162ebdb.svn-base create mode 100644 .svn/pristine/be/bed5706f445696a09d44974b7fbc00582316ea46.svn-base create mode 100644 .svn/pristine/df/dfe7a87a356300cae6565726ccc1c7d05b04365a.svn-base create mode 100644 .svn/pristine/f1/f16e6c52546d685f14bc88c1b412a66c2e069b85.svn-base create mode 100644 include/normals/normals.h create mode 100644 include/scanio/scan_io_ply.h create mode 100644 include/scanio/scan_io_xyzr.h create mode 100644 include/slam6d/pairingMode.h create mode 100644 know_issues.txt create mode 100644 src/normals/calc_normals.cc create mode 100644 src/scanio/scan_io_ply.cc create mode 100644 src/scanio/scan_io_xyzr.cc diff --git a/.svn/pristine/0f/0f93101c08e35fb37efbfe34b574c1b43217904e.svn-base b/.svn/pristine/0f/0f93101c08e35fb37efbfe34b574c1b43217904e.svn-base new file mode 100644 index 0000000..a595d0e --- /dev/null +++ b/.svn/pristine/0f/0f93101c08e35fb37efbfe34b574c1b43217904e.svn-base @@ -0,0 +1,118 @@ +### TOOLS + +IF(WITH_TOOLS) + ### SCAN_RED + add_executable(scan_red scan_red.cc fbr/fbr_global.cc fbr/panorama.cc fbr/scan_cv.cc) + + IF(UNIX) + target_link_libraries(scan_red scan dl ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS} ${Boost_LIBRARIES}) + ENDIF(UNIX) + + IF (WIN32) + target_link_libraries(scan_red scan ANN XGetopt) + ENDIF(WIN32) + + ### SCAN_DIFF + add_executable(scan_diff scan_diff.cc) + # add_executable(scan_diff2d scan_diff2d.cc ../show/colormanager.cc) + + IF(UNIX) + target_link_libraries(scan_diff scan dl ANN) + # target_link_libraries(scan_diff2d scan dl ANN) + ENDIF(UNIX) + + IF (WIN32) + target_link_libraries(scan_diff scan ANN XGetopt) + # target_link_libraries(scan_diff2d scan ANN XGetopt) + ENDIF(WIN32) + + add_executable(frame_to_graph frame_to_graph.cc) + add_executable(convergence convergence.cc) + add_executable(graph_balancer graph_balancer.cc) + add_executable(exportPoints exportPoints.cc) + add_executable(frames2riegl frames2riegl.cc) + add_executable(frames2pose frames2pose.cc) + add_executable(pose2frames pose2frames.cc) + add_executable(riegl2frames riegl2frames.cc) + add_executable(toGlobal toGlobal.cc) + + IF(UNIX) + target_link_libraries(graph_balancer scan ${Boost_GRAPH_LIBRARY} ${Boost_SERIALIZATION_LIBRARY} ${Boost_REGEX_LIBRARY}) + target_link_libraries(exportPoints scan dl ANN) + target_link_libraries(toGlobal scan) + ENDIF(UNIX) + + + IF (WIN32) + target_link_libraries(frame_to_graph XGetopt ${Boost_LIBRARIES}) + target_link_libraries(convergence XGetopt ${Boost_LIBRARIES}) + target_link_libraries(graph_balancer scan XGetopt ${Boost_LIBRARIES}) + target_link_libraries(exportPoints scan ANN XGetopt ${Boost_LIBRARIES}) + target_link_libraries(frames2pose XGetopt ${Boost_LIBRARIES}) + target_link_libraries(pose2frames XGetopt ${Boost_LIBRARIES}) + target_link_libraries(frames2riegl XGetopt ${Boost_LIBRARIES}) + target_link_libraries(riegl2frames XGetopt ${Boost_LIBRARIES}) + target_link_libraries(toGlobal XGetopt ${Boost_LIBRARIES}) + ENDIF(WIN32) + +ENDIF(WITH_TOOLS) + +### SCANLIB + +SET(SCANLIB_SRCS + kd.cc kdManaged.cc kdMeta.cc graphSlam6D.cc + graph.cc icp6D.cc icp6Dapx.cc icp6Dsvd.cc + icp6Dortho.cc icp6Dquat.cc icp6Dhelix.cc icp6Dlumeuler.cc + icp6Dlumquat.cc icp6Ddual.cc lum6Deuler.cc lum6Dquat.cc + ghelix6DQ2.cc gapx6D.cc ann_kd.cc elch6D.cc + elch6Dquat.cc elch6DunitQuat.cc elch6Dslerp.cc elch6Deuler.cc + point_type.cc icp6Dquatscale.cc searchTree.cc Boctree.cc + scan.cc basicScan.cc managedScan.cc metaScan.cc + io_types.cc io_utils.cc pointfilter.cc allocator.cc + ) + +if(WITH_METRICS) + set(SCANLIB_SRCS ${SCANLIB_SRCS} metrics.cc) +endif(WITH_METRICS) + +add_library(scan STATIC ${SCANLIB_SRCS}) + +FIND_PACKAGE(OpenCV REQUIRED) + +target_link_libraries(scan scanclient scanio normals) + +IF(UNIX) + target_link_libraries(scan dl) +ENDIF(UNIX) + +### EXPORT SHARED LIBS + +IF(EXPORT_SHARED_LIBS) +add_library(scan_s SHARED ${SCANLIB_SRCS}) +#target_link_libraries(scan_s ${Boost_LIBRARIES} newmat) +target_link_libraries(scan_s newmat_s sparse_s ANN_s ) +ENDIF(EXPORT_SHARED_LIBS) + +### SLAM6D + +IF(WITH_CUDA) + CUDA_COMPILE(CUDA_FILES cuda/CIcpGpuCuda.cu ) + add_executable(slam6D slam6D.cc cuda/icp6Dcuda.cc ${CUDA_FILES}) + target_link_libraries(slam6D ${CUDA_LIBRARIES} ANN cudpp64) + CUDA_ADD_CUBLAS_TO_TARGET(slam6D) + CUDA_ADD_CUTIL_TO_TARGET(slam6D) +ELSE(WITH_CUDA) + add_executable(slam6D slam6D.cc) +ENDIF(WITH_CUDA) + +IF(UNIX) + target_link_libraries(slam6D scan newmat sparse ANN) +ENDIF(UNIX) + +IF(WIN32) + target_link_libraries(slam6D scan newmat sparse ANN XGetopt ${Boost_LIBRARIES}) +ENDIF(WIN32) + +#IF(MSVC) +# INSTALL(TARGETS slam6D RUNTIME DESTINATION ${CMAKE_SOURCE_DIR}/windows) +#ENDIF(MSVC) diff --git a/.svn/pristine/30/30442352028f2a8b91166bea8d02b8459483fe48.svn-base b/.svn/pristine/30/30442352028f2a8b91166bea8d02b8459483fe48.svn-base new file mode 100644 index 0000000..54dea9e --- /dev/null +++ b/.svn/pristine/30/30442352028f2a8b91166bea8d02b8459483fe48.svn-base @@ -0,0 +1,893 @@ +/* + * scan implementation + * + * Copyright (C) Andreas Nuechter, Kai Lingemann, Dorit Borrmann, Jan Elseberg, Thomas Escher + * + * Released under the GPL version 3. + * + */ + +#include "slam6d/scan.h" + +#include "slam6d/basicScan.h" +#include "slam6d/managedScan.h" +#include "slam6d/metaScan.h" +#include "slam6d/searchTree.h" +#include "slam6d/kd.h" +#include "slam6d/Boctree.h" +#include "slam6d/globals.icc" + +#include "normals/normals.h" + +#ifdef WITH_METRICS +#include "slam6d/metrics.h" +#endif + +#ifdef _MSC_VER +#define _NO_PARALLEL_READ +#endif + +#ifdef __APPLE__ +#define _NO_PARALLEL_READ +#endif + +using std::vector; + + +vector Scan::allScans; +bool Scan::scanserver = false; + + +void Scan::openDirectory(bool scanserver, const std::string& path, IOType type, + int start, int end) +{ + Scan::scanserver = scanserver; + if(scanserver) + ManagedScan::openDirectory(path, type, start, end); + else + BasicScan::openDirectory(path, type, start, end); +} + +void Scan::closeDirectory() +{ + if(scanserver) + ManagedScan::closeDirectory(); + else + BasicScan::closeDirectory(); +} + +Scan::Scan() +{ + unsigned int i; + + // pose and transformations + for(i = 0; i < 3; ++i) rPos[i] = 0; + for(i = 0; i < 3; ++i) rPosTheta[i] = 0; + for(i = 0; i < 4; ++i) rQuat[i] = 0; + M4identity(transMat); + M4identity(transMatOrg); + M4identity(dalignxf); + + // trees and reduction methods + cuda_enabled = false; + nns_method = -1; + kd = 0; + ann_kd_tree = 0; + + // reduction on-demand + reduction_voxelSize = 0.0; + reduction_nrpts = 0; + reduction_pointtype = PointType(); + + // flags + m_has_reduced = false; + + // octtree + octtree_reduction_voxelSize = 0.0; + octtree_voxelSize = 0.0; + octtree_pointtype = PointType(); + octtree_loadOct = false; + octtree_saveOct = false; +} + +Scan::~Scan() +{ + if(kd) delete kd; +} + +void Scan::setReductionParameter(double voxelSize, int nrpts, PointType pointtype) +{ + reduction_voxelSize = voxelSize; + reduction_nrpts = nrpts; + reduction_pointtype = pointtype; +} + +void Scan::setSearchTreeParameter(int nns_method, bool cuda_enabled) +{ + searchtree_nnstype = nns_method; + searchtree_cuda_enabled = cuda_enabled; +} + +void Scan::setOcttreeParameter(double reduction_voxelSize, double voxelSize, PointType pointtype, bool loadOct, bool saveOct) +{ + octtree_reduction_voxelSize = reduction_voxelSize; + octtree_voxelSize = voxelSize; + octtree_pointtype = pointtype; + octtree_loadOct = loadOct; + octtree_saveOct = saveOct; +} + +void Scan::clear(unsigned int types) +{ + if(types & DATA_XYZ) clear("xyz"); + if(types & DATA_RGB) clear("rgb"); + if(types & DATA_REFLECTANCE) clear("reflectance"); + if(types & DATA_TEMPERATURE) clear("temperature"); + if(types & DATA_AMPLITUDE) clear("amplitude"); + if(types & DATA_TYPE) clear("type"); + if(types & DATA_DEVIATION) clear("deviation"); +} + +SearchTree* Scan::getSearchTree() +{ + // if the search tree hasn't been created yet, calculate everything + if(kd == 0) { + createSearchTree(); + } + return kd; +} + +void Scan::toGlobal() { + calcReducedPoints(); + transform(transMatOrg, INVALID); +} + +/** + * Computes a search tree depending on the type. + */ +void Scan::createSearchTree() +{ + // multiple threads will call this function at the same time because they + // all work on one pair of Scans, just let the first one (who sees a nullpointer) + // do the creation + boost::lock_guard lock(m_mutex_create_tree); + if(kd != 0) return; + + // make sure the original points are created before starting the measurement + DataXYZ xyz_orig(get("xyz reduced original")); + +#ifdef WITH_METRICS + Timer tc = ClientMetric::create_tree_time.start(); +#endif //WITH_METRICS + + createSearchTreePrivate(); + +#ifdef WITH_METRICS + ClientMetric::create_tree_time.end(tc); +#endif //WITH_METRICS +} + +void Scan::calcReducedOnDemand() +{ + // multiple threads will call this function at the same time + // because they all work on one pair of Scans, + // just let the first one (who sees count as zero) do the reduction + boost::lock_guard lock(m_mutex_reduction); + if(m_has_reduced) return; + +#ifdef WITH_METRICS + Timer t = ClientMetric::on_demand_reduction_time.start(); +#endif //WITH_METRICS + + calcReducedOnDemandPrivate(); + + m_has_reduced = true; + +#ifdef WITH_METRICS + ClientMetric::on_demand_reduction_time.end(t); +#endif //WITH_METRICS +} + +void Scan::calcNormalsOnDemand() +{ + // multiple threads will call this function at the same time + // because they all work on one pair of Scans, + // just let the first one (who sees count as zero) do the reduction + boost::lock_guard lock(m_mutex_normals); + if(m_has_normals) return; + calcNormalsOnDemandPrivate(); + m_has_normals = true; +} + +void Scan::copyReducedToOriginal() +{ +#ifdef WITH_METRICS + Timer t = ClientMetric::copy_original_time.start(); +#endif //WITH_METRICS + + DataXYZ xyz_reduced(get("xyz reduced")); + unsigned int size = xyz_reduced.size(); + DataXYZ xyz_reduced_orig(create("xyz reduced original", sizeof(double)*3*size)); + for(unsigned int i = 0; i < size; ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_reduced_orig[i][j] = xyz_reduced[i][j]; + } + } + +#ifdef WITH_METRICS + ClientMetric::copy_original_time.end(t); +#endif //WITH_METRICS +} + +void Scan::copyOriginalToReduced() +{ +#ifdef WITH_METRICS + Timer t = ClientMetric::copy_original_time.start(); +#endif //WITH_METRICS + + DataXYZ xyz_reduced_orig(get("xyz reduced original")); + unsigned int size = xyz_reduced_orig.size(); + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); + for(unsigned int i = 0; i < size; ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_reduced[i][j] = xyz_reduced_orig[i][j]; + } + } + +#ifdef WITH_METRICS + ClientMetric::copy_original_time.end(t); +#endif //WITH_METRICS +} + + +/** + * Computes normals for all points + */ +void Scan::calcNormals() +{ + cout << "calcNormals" << endl; + DataXYZ xyz(get("xyz")); + DataNormal xyz_normals(create("normal", sizeof(double)*3*xyz.size())); + if(xyz.size() == 0) + throw runtime_error("Could not calculate reduced points, XYZ data is empty"); + + vector points; + points.reserve(xyz.size()); + vector normals; + normals.reserve(xyz.size()); + for(unsigned int j = 0; j < xyz.size(); j++) { + points.push_back(Point(xyz[j][0], xyz[j][1], xyz[j][2])); + } + const int K_NEIGHBOURS = 10; + calculateNormalsApxKNN(normals, points, K_NEIGHBOURS, get_rPos(), 1.0); + for (unsigned int i = 0; i < normals.size(); ++i) { + xyz_normals[i][0] = normals[i].x; + xyz_normals[i][1] = normals[i].y; + xyz_normals[i][2] = normals[i].z; + } +} + +/** + * Computes an octtree of the current scan, then getting the + * reduced points as the centers of the octree voxels. + */ +void Scan::calcReducedPoints() +{ +#ifdef WITH_METRICS + Timer t = ClientMetric::scan_load_time.start(); +#endif //WITH_METRICS + + + // get xyz to start the scan load, separated here for time measurement + DataXYZ xyz(get("xyz")); + DataXYZ xyz_normals(get("")); + if (reduction_pointtype.hasNormal()) { + DataXYZ my_xyz_normals(get("normal")); + xyz_normals = my_xyz_normals; + } + DataReflectance reflectance(get("")); + if (reduction_pointtype.hasReflectance()) { + DataReflectance my_reflectance(get("reflectance")); + reflectance = my_reflectance; + } + +#ifdef WITH_METRICS + ClientMetric::scan_load_time.end(t); + Timer tl = ClientMetric::calc_reduced_points_time.start(); +#endif //WITH_METRICS + + if(reduction_voxelSize <= 0.0) { + // copy the points + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size())); + for(unsigned int i = 0; i < xyz.size(); ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_reduced[i][j] = xyz[i][j]; + } + } + if (reduction_pointtype.hasReflectance()) { + DataReflectance reflectance_reduced(create("reflectance reduced", sizeof(float)*reflectance.size())); + for(unsigned int i = 0; i < xyz.size(); ++i) { + reflectance_reduced[i] = reflectance[i]; + } + } + if (reduction_pointtype.hasNormal()) { + DataNormal normal_reduced(create("normal reduced", sizeof(double)*3*xyz.size())); + for(unsigned int i = 0; i < xyz.size(); ++i) { + for(unsigned int j = 0; j < 3; ++j) { + normal_reduced[i][j] = xyz_normals[i][j]; + } + } + } + + } else { + + double **xyz_in = new double*[xyz.size()]; + for (unsigned int i = 0; i < xyz.size(); ++i) { + xyz_in[i] = new double[reduction_pointtype.getPointDim()]; + unsigned int j = 0; + for (; j < 3; ++j) + xyz_in[i][j] = xyz[i][j]; + if (reduction_pointtype.hasReflectance()) + xyz_in[i][j++] = reflectance[i]; + if (reduction_pointtype.hasNormal()) + for (unsigned int l = 0; l < 3; ++l) + xyz_in[i][j] = xyz_normals[i][l]; + } + + // start reduction + // build octree-tree from CurrentScan + // put full data into the octtree + BOctTree *oct = new BOctTree(xyz_in, + xyz.size(), + reduction_voxelSize, + reduction_pointtype); + + vector center; + center.clear(); + if (reduction_nrpts > 0) { + if (reduction_nrpts == 1) { + oct->GetOctTreeRandom(center); + } else { + oct->GetOctTreeRandom(center, reduction_nrpts); + } + } else { + oct->GetOctTreeCenter(center); + } + + // storing it as reduced scan + unsigned int size = center.size(); + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); + DataReflectance reflectance_reduced(get("")); + DataNormal normal_reduced(get("")); + if (reduction_pointtype.hasReflectance()) { + DataReflectance my_reflectance_reduced(create("reflectance reduced", + sizeof(float)*size)); + reflectance_reduced = my_reflectance_reduced; + } + if (reduction_pointtype.hasNormal()) { + DataNormal my_normal_reduced(create("normal reduced", sizeof(double)*3*size)); + normal_reduced = my_normal_reduced; + } + for(unsigned int i = 0; i < size; ++i) { + unsigned int j = 0; + for (; j < 3; ++j) + xyz_reduced[i][j] = center[i][j]; + if (reduction_pointtype.hasReflectance()) + reflectance_reduced[i] = center[i][j++]; + if (reduction_pointtype.hasNormal()) + for (unsigned int l = 0; l < 3; ++l) + normal_reduced[i][l] = center[i][j++]; + } + } + +#ifdef WITH_METRICS + ClientMetric::calc_reduced_points_time.end(tl); +#endif //WITH_METRICS +} + + +/** + * Merges the scan's intrinsic coordinates with the robot position. + * @param prevScan The scan that's transformation is extrapolated, + * i.e., odometry extrapolation + * + * For additional information see the following paper (jfr2007.pdf): + * + * Andreas Nüchter, Kai Lingemann, Joachim Hertzberg, and Hartmut Surmann, + * 6D SLAM - 3D Mapping Outdoor Environments Journal of Field Robotics (JFR), + * Special Issue on Quantitative Performance Evaluation of Robotic and Intelligent + * Systems, Wiley & Son, ISSN 1556-4959, Volume 24, Issue 8-9, pages 699 - 722, + * August/September, 2007 + * + */ +void Scan::mergeCoordinatesWithRoboterPosition(Scan* prevScan) +{ + double tempMat[16], deltaMat[16]; + M4inv(prevScan->get_transMatOrg(), tempMat); + MMult(prevScan->get_transMat(), tempMat, deltaMat); + transform(deltaMat, INVALID); //apply delta transformation of the previous scan +} + +/** + * The method transforms all points with the given transformation matrix. + */ +void Scan::transformAll(const double alignxf[16]) +{ + DataXYZ xyz(get("xyz")); + unsigned int i=0 ; + // #pragma omp parallel for + for(; i < xyz.size(); ++i) { + transform3(alignxf, xyz[i]); + } + // TODO: test for ManagedScan compability, may need a touch("xyz") to mark saving the new values +} + +//! Internal function of transform which alters the reduced points +void Scan::transformReduced(const double alignxf[16]) +{ +#ifdef WITH_METRICS + Timer t = ClientMetric::transform_time.start(); +#endif //WITH_METRICS + + DataXYZ xyz_reduced(get("xyz reduced")); + unsigned int i=0; + // #pragma omp parallel for + for( ; i < xyz_reduced.size(); ++i) { + transform3(alignxf, xyz_reduced[i]); + } + + DataNormal normal_reduced(get("normal reduced")); + for (unsigned int i = 0; i < normal_reduced.size(); ++i) { + transform3normal(alignxf, normal_reduced[i]); + } + + +#ifdef WITH_METRICS + ClientMetric::transform_time.end(t); +#endif //WITH_METRICS +} + +//! Internal function of transform which handles the matrices +void Scan::transformMatrix(const double alignxf[16]) +{ + double tempxf[16]; + + // apply alignxf to transMat and update pose vectors + MMult(alignxf, transMat, tempxf); + memcpy(transMat, tempxf, sizeof(transMat)); + Matrix4ToEuler(transMat, rPosTheta, rPos); + Matrix4ToQuat(transMat, rQuat); + +#ifdef DEBUG + cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", " + << rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ")" << endl; + + cerr << transMat << endl; +#endif + + // apply alignxf to dalignxf + MMult(alignxf, dalignxf, tempxf); + memcpy(dalignxf, tempxf, sizeof(transMat)); +} + +/** + * Transforms the scan by a given transformation and writes a new frame. The idea + * is to write for every transformation in all files, such that the show program + * is able to determine, whcih scans have to be drawn in which color. Hidden scans + * (or later processed scans) are written with INVALID. + * + * @param alignxf Transformation matrix + * @param colour Specifies which colour should the written to the frames file + * @param islum Is the transformtion part of LUM, i.e., all scans are transformed? + * In this case only LUM transformation is stored, otherwise all scans are processed + * -1 no transformation is stored + * 0 ICP transformation + * 1 LUM transformation, all scans except last scan + * 2 LUM transformation, last scan only + */ +void Scan::transform(const double alignxf[16], const AlgoType type, int islum) +{ + MetaScan* meta = dynamic_cast(this); + + if(meta) { + for(unsigned int i = 0; i < meta->size(); ++i) { + meta->getScan(i)->transform(alignxf, type, -1); + } + } + +#ifdef TRANSFORM_ALL_POINTS + transformAll(alignxf); +#endif //TRANSFORM_ALL_POINTS + +#ifdef DEBUG + cerr << alignxf << endl; + cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", " + << rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ") ---> "; +#endif + + // transform points + transformReduced(alignxf); + + // update matrices + transformMatrix(alignxf); + + // store transformation in frames + if(type != INVALID) { +#ifdef WITH_METRICS + Timer t = ClientMetric::add_frames_time.start(); +#endif //WITH_METRICS + bool in_meta; + MetaScan* meta = dynamic_cast(this); + int found = 0; + unsigned int scans_size = allScans.size(); + + switch (islum) { + case -1: + // write no tranformation + break; + case 0: + for(unsigned int i = 0; i < scans_size; ++i) { + Scan* scan = allScans[i]; + in_meta = false; + if(meta) { + for(unsigned int j = 0; j < meta->size(); ++j) { + if(meta->getScan(j) == scan) { + found = i; + in_meta = true; + } + } + } + + if(scan == this || in_meta) { + found = i; + scan->addFrame(type); + } else { + if(found == 0) { + scan->addFrame(ICPINACTIVE); + } else { + scan->addFrame(INVALID); + } + } + } + break; + case 1: + addFrame(type); + break; + case 2: + for(unsigned int i = 0; i < scans_size; ++i) { + Scan* scan = allScans[i]; + if(scan == this) { + found = i; + addFrame(type); + allScans[0]->addFrame(type); + continue; + } + if (found != 0) { + scan->addFrame(INVALID); + } + } + break; + default: + cerr << "invalid point transformation mode" << endl; + } + +#ifdef WITH_METRICS + ClientMetric::add_frames_time.end(t); +#endif //WITH_METRICS + } +} + +/** + * Transforms the scan by a given transformation and writes a new frame. The idea + * is to write for every transformation in all files, such that the show program + * is able to determine, whcih scans have to be drawn in which color. Hidden scans + * (or later processed scans) are written with INVALID. + * + * @param alignQuat Quaternion for the rotation + * @param alignt Translation vector + * @param colour Specifies which colour should the written to the frames file + * @param islum Is the transformtion part of LUM, i.e., all scans are transformed? + * In this case only LUM transformation is stored, otherwise all scans are processed + * -1 no transformation is stored + * 0 ICP transformation + * 1 LUM transformation, all scans except last scan + * 2 LUM transformation, last scan only + */ +void Scan::transform(const double alignQuat[4], const double alignt[3], + const AlgoType type, int islum) +{ + double alignxf[16]; + QuatToMatrix4(alignQuat, alignt, alignxf); + transform(alignxf, type, islum); +} + +/** + * Transforms the scan, so that the given Matrix + * prepresent the next pose. + * + * @param alignxf Transformation matrix to which this scan will be set to + * @param islum Is the transformation part of LUM? + */ +void Scan::transformToMatrix(double alignxf[16], const AlgoType type, int islum) +{ + double tinv[16]; + M4inv(transMat, tinv); + transform(tinv, INVALID); + transform(alignxf, type, islum); +} + +/** + * Transforms the scan, so that the given Euler angles + * prepresent the next pose. + * + * @param rP Translation to which this scan will be set to + * @param rPT Orientation as Euler angle to which this scan will be set + * @param islum Is the transformation part of LUM? + */ +void Scan::transformToEuler(double rP[3], double rPT[3], const AlgoType type, int islum) +{ +#ifdef WITH_METRICS + // called in openmp context in lum6Deuler.cc:422 + ClientMetric::transform_time.set_threadsafety(true); + ClientMetric::add_frames_time.set_threadsafety(true); +#endif //WITH_METRICS + + double tinv[16]; + double alignxf[16]; + M4inv(transMat, tinv); + transform(tinv, INVALID); + EulerToMatrix4(rP, rPT, alignxf); + transform(alignxf, type, islum); + +#ifdef WITH_METRICS + ClientMetric::transform_time.set_threadsafety(false); + ClientMetric::add_frames_time.set_threadsafety(false); +#endif //WITH_METRICS +} + +/** + * Transforms the scan, so that the given Euler angles + * prepresent the next pose. + * + * @param rP Translation to which this scan will be set to + * @param rPQ Orientation as Quaternion to which this scan will be set + * @param islum Is the transformation part of LUM? + */ +void Scan::transformToQuat(double rP[3], double rPQ[4], const AlgoType type, int islum) +{ + double tinv[16]; + double alignxf[16]; + M4inv(transMat, tinv); + transform(tinv, INVALID); + QuatToMatrix4(rPQ, rP, alignxf); + transform(alignxf, type, islum); +} + +/** + * Calculates Source\Target + * Calculates a set of corresponding point pairs and returns them. It + * computes the k-d trees and deletes them after the pairs have been + * found. This slow function should be used only for testing + * + * @param pairs The resulting point pairs (vector will be filled) + * @param Target The scan to whiche the points are matched + * @param thread_num number of the thread (for parallelization) + * @param rnd randomized point selection + * @param max_dist_match2 maximal allowed distance for matching + */ + +void Scan::getNoPairsSimple(vector &diff, + Scan* Source, Scan* Target, + int thread_num, + double max_dist_match2) +{ + DataXYZ xyz_reduced(Source->get("xyz reduced")); + KDtree* kd = new KDtree(PointerArray(Target->get("xyz reduced")).get(), Target->size("xyz reduced")); + + cout << "Max: " << max_dist_match2 << endl; + for (unsigned int i = 0; i < xyz_reduced.size(); i++) { + + double p[3]; + p[0] = xyz_reduced[i][0]; + p[1] = xyz_reduced[i][1]; + p[2] = xyz_reduced[i][2]; + + + double *closest = kd->FindClosest(p, max_dist_match2, thread_num); + if (!closest) { + diff.push_back(xyz_reduced[i]); + //diff.push_back(closest); + } + } + + delete kd; +} + +/** + * Calculates a set of corresponding point pairs and returns them. It + * computes the k-d trees and deletes them after the pairs have been + * found. This slow function should be used only for testing + * + * @param pairs The resulting point pairs (vector will be filled) + * @param Source The scan whose points are matched to Targets' points + * @param Target The scan to whiche the points are matched + * @param thread_num number of the thread (for parallelization) + * @param rnd randomized point selection + * @param max_dist_match2 maximal allowed distance for matching + */ +void Scan::getPtPairsSimple(vector *pairs, + Scan* Source, Scan* Target, + int thread_num, + int rnd, double max_dist_match2, + double *centroid_m, double *centroid_d) +{ + KDtree* kd = new KDtree(PointerArray(Source->get("xyz reduced")).get(), Source->size("xyz reduced")); + DataXYZ xyz_reduced(Target->get("xyz reduced")); + + for (unsigned int i = 0; i < xyz_reduced.size(); i++) { + if (rnd > 1 && rand(rnd) != 0) continue; // take about 1/rnd-th of the numbers only + + double p[3]; + p[0] = xyz_reduced[i][0]; + p[1] = xyz_reduced[i][1]; + p[2] = xyz_reduced[i][2]; + + double *closest = kd->FindClosest(p, max_dist_match2, thread_num); + if (closest) { + centroid_m[0] += closest[0]; + centroid_m[1] += closest[1]; + centroid_m[2] += closest[2]; + centroid_d[0] += p[0]; + centroid_d[1] += p[1]; + centroid_d[2] += p[2]; + PtPair myPair(closest, p); + pairs->push_back(myPair); + } + } + centroid_m[0] /= pairs[thread_num].size(); + centroid_m[1] /= pairs[thread_num].size(); + centroid_m[2] /= pairs[thread_num].size(); + centroid_d[0] /= pairs[thread_num].size(); + centroid_d[1] /= pairs[thread_num].size(); + centroid_d[2] /= pairs[thread_num].size(); + + delete kd; +} + + +/** + * Calculates a set of corresponding point pairs and returns them. + * The function uses the k-d trees stored the the scan class, thus + * the function createTrees and deletTrees have to be called before + * resp. afterwards. + * Here we implement the so called "fast corresponding points"; k-d + * trees are not recomputed, instead the apply the inverse transformation + * to to the given point set. + * + * @param pairs The resulting point pairs (vector will be filled) + * @param Source The scan whose points are matched to Targets' points + * @param Target The scan to whiche the points are matched + * @param thread_num number of the thread (for parallelization) + * @param rnd randomized point selection + * @param max_dist_match2 maximal allowed distance for matching + * @return a set of corresponding point pairs + */ +void Scan::getPtPairs(vector *pairs, + Scan* Source, Scan* Target, + int thread_num, + int rnd, double max_dist_match2, double &sum, + double *centroid_m, double *centroid_d, PairingMode pairing_mode) +{ + // initialize centroids + for(unsigned int i = 0; i < 3; ++i) { + centroid_m[i] = 0; + centroid_d[i] = 0; + } + + // get point pairs + DataXYZ xyz_reduced(Target->get("xyz reduced")); + DataNormal normal_reduced(Target->get("normal reduced")); + Source->getSearchTree()->getPtPairs(pairs, Source->dalignxf, + xyz_reduced, normal_reduced, 0, xyz_reduced.size(), + thread_num, + rnd, max_dist_match2, sum, centroid_m, centroid_d, + pairing_mode); + + // normalize centroids + unsigned int size = pairs->size(); + if(size != 0) { + for(unsigned int i = 0; i < 3; ++i) { + centroid_m[i] /= size; + centroid_d[i] /= size; + } + } +} + + +/** + * Calculates a set of corresponding point pairs and returns them. + * The function uses the k-d trees stored the the scan class, thus + * the function createTrees and delteTrees have to be called before + * resp. afterwards. + * + * @param pairs The resulting point pairs (vector will be filled) + * @param Source The scan whose points are matched to Targets' points + * @param Target The scan to whiche the points are matched + * @param thread_num The number of the thread that is computing ptPairs in parallel + * @param step The number of steps for parallelization + * @param rnd randomized point selection + * @param max_dist_match2 maximal allowed distance for matching + * @param sum The sum of distances of the points + * + * These intermediate values are for the parallel ICP algorithm + * introduced in the paper + * "The Parallel Iterative Closest Point Algorithm" + * by Langis / Greenspan / Godin, IEEE 3DIM 2001 + * + */ +void Scan::getPtPairsParallel(vector *pairs, + Scan* Source, Scan* Target, + int thread_num, int step, + int rnd, double max_dist_match2, + double *sum, + double centroid_m[OPENMP_NUM_THREADS][3], + double centroid_d[OPENMP_NUM_THREADS][3], + PairingMode pairing_mode) +{ + // initialize centroids + for(unsigned int i = 0; i < 3; ++i) { + centroid_m[thread_num][i] = 0; + centroid_d[thread_num][i] = 0; + } + + // get point pairs + SearchTree* search = Source->getSearchTree(); + // differentiate between a meta scan (which has no reduced points) and a normal scan + // if Source is also a meta scan it already has a special meta-kd-tree + MetaScan* meta = dynamic_cast(Target); + if(meta) { + for(unsigned int i = 0; i < meta->size(); ++i) { + // determine step for each scan individually + DataXYZ xyz_reduced(meta->getScan(i)->get("xyz reduced")); + DataNormal normal_reduced(Target->get("normal reduced")); + unsigned int max = xyz_reduced.size(); + unsigned int step = max / OPENMP_NUM_THREADS; + // call ptpairs for each scan and accumulate ptpairs, centroids and sum + search->getPtPairs(&pairs[thread_num], Source->dalignxf, + xyz_reduced, normal_reduced, + step * thread_num, step * thread_num + step, + thread_num, + rnd, max_dist_match2, sum[thread_num], + centroid_m[thread_num], centroid_d[thread_num], pairing_mode); + } + } else { + DataXYZ xyz_reduced(Target->get("xyz reduced")); + DataNormal normal_reduced(Target->get("normal reduced")); + search->getPtPairs(&pairs[thread_num], Source->dalignxf, + xyz_reduced, normal_reduced, + thread_num * step, thread_num * step + step, + thread_num, + rnd, max_dist_match2, sum[thread_num], + centroid_m[thread_num], centroid_d[thread_num], pairing_mode); + } + + // normalize centroids + unsigned int size = pairs[thread_num].size(); + if(size != 0) { + for(unsigned int i = 0; i < 3; ++i) { + centroid_m[thread_num][i] /= size; + centroid_d[thread_num][i] /= size; + } + } +} + +unsigned int Scan::getMaxCountReduced(ScanVector& scans) +{ + unsigned int max = 0; + for(std::vector::iterator it = scans.begin(); it != scans.end(); ++it) { + unsigned int count = (*it)->size("xyz reduced"); + if(count > max) + max = count; + } + return max; +} diff --git a/.svn/pristine/35/35452722e3484f4b8a583e474ef9d100a4e18b87.svn-base b/.svn/pristine/35/35452722e3484f4b8a583e474ef9d100a4e18b87.svn-base new file mode 100644 index 0000000..6067c9d --- /dev/null +++ b/.svn/pristine/35/35452722e3484f4b8a583e474ef9d100a4e18b87.svn-base @@ -0,0 +1,386 @@ +/** @file + * @brief Representation of the optimized k-d tree. + * @author Remus Dumitru. Jacobs University Bremen, Germany + * @author Corneliu-Claudiu Prodescu. Jacobs University Bremen, Germany + * @author Andreas Nuechter. Jacobs University Bremen, Germany + * @author Kai Lingemann. Inst. of CS, University of Osnabrueck, Germany + * @author Thomas Escher. Inst. of CS, University of Osnabrueck, Germany + */ + +#ifndef __KD_TREE_IMPL_H__ +#define __KD_TREE_IMPL_H__ + +#include "slam6d/kdparams.h" +#include "globals.icc" + +#ifdef _MSC_VER +#if !defined _OPENMP && defined OPENMP +#define _OPENMP +#endif +#endif + +#ifdef _OPENMP +#include +#endif + +class PointCompare { +public: + bool operator() (const std::pair& left, + const std::pair& right) + { + return left.second > right.second; + } +}; + +/** + * @brief The optimized k-d tree. + * + * A kD tree for points, with limited + * capabilities (find nearest point to + * a given point, or to a ray). + **/ +template +class KDTreeImpl { +public: + inline KDTreeImpl() { } + + virtual inline ~KDTreeImpl() { + if (!npts) { +#ifdef WITH_OPENMP_KD + omp_set_num_threads(OPENMP_NUM_THREADS); +#pragma omp parallel for schedule(dynamic) +#endif + for (int i = 0; i < 2; i++) { + if (i == 0 && node.child1) delete node.child1; + if (i == 1 && node.child2) delete node.child2; + } + } else { + if (leaf.p) delete [] leaf.p; + } + } + + virtual void create(PointData pts, AccessorData *indices, size_t n) { + AccessorFunc point; + + // Find bbox + double xmin = point(pts, indices[0])[0], xmax = point(pts, indices[0])[0]; + double ymin = point(pts, indices[0])[1], ymax = point(pts, indices[0])[1]; + double zmin = point(pts, indices[0])[2], zmax = point(pts, indices[0])[2]; + for(unsigned int i = 1; i < n; i++) { + xmin = min(xmin, point(pts, indices[i])[0]); + xmax = max(xmax, point(pts, indices[i])[0]); + ymin = min(ymin, point(pts, indices[i])[1]); + ymax = max(ymax, point(pts, indices[i])[1]); + zmin = min(zmin, point(pts, indices[i])[2]); + zmax = max(zmax, point(pts, indices[i])[2]); + } + + // Leaf nodes + if ((n > 0) && (n <= 10)) { + npts = n; + leaf.p = new AccessorData[n]; + // fill leaf index array with indices + for(unsigned int i = 0; i < n; ++i) { + leaf.p[i] = indices[i]; + } + return; + } + + // Else, interior nodes + npts = 0; + + node.center[0] = 0.5 * (xmin+xmax); + node.center[1] = 0.5 * (ymin+ymax); + node.center[2] = 0.5 * (zmin+zmax); + node.dx = 0.5 * (xmax-xmin); + node.dy = 0.5 * (ymax-ymin); + node.dz = 0.5 * (zmax-zmin); + node.r2 = sqr(node.dx) + sqr(node.dy) + sqr(node.dz); + + // Find longest axis + if (node.dx > node.dy) { + if (node.dx > node.dz) { + node.splitaxis = 0; + } else { + node.splitaxis = 2; + } + } else { + if (node.dy > node.dz) { + node.splitaxis = 1; + } else { + node.splitaxis = 2; + } + } + + // Partition + double splitval = node.center[node.splitaxis]; + + if ( fabs(max(max(node.dx,node.dy),node.dz)) < 0.01 ) { + npts = n; + leaf.p = new AccessorData[n]; + // fill leaf index array with indices + for(unsigned int i = 0; i < n; ++i) { + leaf.p[i] = indices[i]; + } + return; + } + + AccessorData* left = indices, * right = indices + n - 1; + while(true) { + while(point(pts, *left)[node.splitaxis] < splitval) + left++; + while(point(pts, *right)[node.splitaxis] >= splitval) + right--; + if(right < left) + break; + std::swap(*left, *right); + } + + // Build subtrees + int i; +#ifdef WITH_OPENMP_KD // does anybody know the reason why this is slower ?? --Andreas + omp_set_num_threads(OPENMP_NUM_THREADS); +#pragma omp parallel for schedule(dynamic) +#endif + for (i = 0; i < 2; i++) { + if (i == 0) { + node.child1 = new KDTreeImpl(); + node.child1->create(pts, indices, left - indices); + } + if (i == 1) { + node.child2 = new KDTreeImpl(); + node.child2->create(pts, left, n - (left - indices)); + } + } + } + +protected: + /** + * storing the parameters of the k-d tree, i.e., the current closest point, + * the distance to the current closest point and the point itself. + * These global variable are needed in this search. + * + * Padded in the parallel case. + */ +#ifdef _OPENMP +#ifdef __INTEL_COMPILER + __declspec (align(16)) static KDParams params[MAX_OPENMP_NUM_THREADS]; +#else + static KDParams params[MAX_OPENMP_NUM_THREADS]; +#endif //__INTEL_COMPILER +#else + static KDParams params[MAX_OPENMP_NUM_THREADS]; +#endif + + /** + * number of points. If this is 0: intermediate node. If nonzero: leaf. + */ + int npts; + + /** + * Cue the standard rant about anon unions but not structs in C++ + */ + union { + /** + * in case of internal node... + */ + struct { + double center[3]; ///< storing the center of the voxel (R^3) + double dx, ///< defining the voxel itself + dy, ///< defining the voxel itself + dz, ///< defining the voxel itself + r2; ///< defining the voxel itself + int splitaxis; ///< defining the kind of splitaxis + KDTreeImpl *child1; ///< pointers to the childs + KDTreeImpl *child2; ///< pointers to the childs + } node; + /** + * in case of leaf node ... + */ + struct { + /** + * store the value itself + * Here we store just a pointer to the data + */ + AccessorData* p; + } leaf; + }; + + void _FindClosest(const PointData& pts, int threadNum) const { + AccessorFunc point; + + // Leaf nodes + if (npts) { + for (int i = 0; i < npts; i++) { + double myd2 = Dist2(params[threadNum].p, point(pts, leaf.p[i])); + if (myd2 < params[threadNum].closest_d2) { + params[threadNum].closest_d2 = myd2; + params[threadNum].closest = point(pts, leaf.p[i]); + } + } + return; + } + + // Quick check of whether to abort + double approx_dist_bbox = + max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx, + fabs(params[threadNum].p[1]-node.center[1])-node.dy), + fabs(params[threadNum].p[2]-node.center[2])-node.dz); + if (approx_dist_bbox >= 0 && + sqr(approx_dist_bbox) >= params[threadNum].closest_d2) + return; + + // Recursive case + double myd = node.center[node.splitaxis] - params[threadNum].p[node.splitaxis]; + if (myd >= 0.0) { + node.child1->_FindClosest(pts, threadNum); + if (sqr(myd) < params[threadNum].closest_d2) { + node.child2->_FindClosest(pts, threadNum); + } + } else { + node.child2->_FindClosest(pts, threadNum); + if (sqr(myd) < params[threadNum].closest_d2) { + node.child1->_FindClosest(pts, threadNum); + } + } + } + + void _FindClosestAlongDir(const PointData& pts, int threadNum) const { + AccessorFunc point; + + // Leaf nodes + if (npts) { + for (int i=0; i < npts; i++) { + double p2p[] = { params[threadNum].p[0] - point(pts, leaf.p[i])[0], + params[threadNum].p[1] - point(pts, leaf.p[i])[1], + params[threadNum].p[2] - point(pts, leaf.p[i])[2] }; + double myd2 = Len2(p2p) - sqr(Dot(p2p, params[threadNum].dir)); + if ((myd2 < params[threadNum].closest_d2)) { + params[threadNum].closest_d2 = myd2; + params[threadNum].closest = point(pts, leaf.p[i]); + } + } + return; + } + + + // Quick check of whether to abort + double p2c[] = { params[threadNum].p[0] - node.center[0], + params[threadNum].p[1] - node.center[1], + params[threadNum].p[2] - node.center[2] }; + double myd2center = Len2(p2c) - sqr(Dot(p2c, params[threadNum].dir)); + if (myd2center > node.r2 + params[threadNum].closest_d2 + 2.0f * max(node.r2, params[threadNum].closest_d2)) + return; + + + // Recursive case + if (params[threadNum].p[node.splitaxis] < node.center[node.splitaxis] ) { + node.child1->_FindClosestAlongDir(pts, threadNum); + node.child2->_FindClosestAlongDir(pts, threadNum); + } else { + node.child2->_FindClosestAlongDir(pts, threadNum); + node.child1->_FindClosestAlongDir(pts, threadNum); + } + } + + void _FixedRangeSearch(const PointData& pts, int threadNum) const { + AccessorFunc point; + + // Leaf nodes + if (npts) { + for (int i = 0; i < npts; i++) { + double myd2 = Dist2(params[threadNum].p, point(pts, leaf.p[i])); + if (myd2 < params[threadNum].closest_d2) { + params[threadNum].closest = point(pts, leaf.p[i]); + + Point newPt; + double* currPt = point(pts, leaf.p[i]); + newPt.x = currPt[0]; + newPt.y = currPt[1]; + newPt.z = currPt[2]; + params[threadNum].heap.push_back(std::make_pair(newPt, myd2)); + std::push_heap(params[threadNum].heap.begin(), + params[threadNum].heap.end(), + PointCompare()); + } + } + return; + } + + // Quick check of whether to abort + double approx_dist_bbox = + max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx, + fabs(params[threadNum].p[1]-node.center[1])-node.dy), + fabs(params[threadNum].p[2]-node.center[2])-node.dz); + if (approx_dist_bbox >= 0 && + sqr(approx_dist_bbox) >= params[threadNum].closest_d2) + return; + + // Recursive case + double myd = node.center[node.splitaxis] - params[threadNum].p[node.splitaxis]; + if (myd >= 0.0) { + node.child1->_FixedRangeSearch(pts, threadNum); + if (sqr(myd) < params[threadNum].closest_d2) { + node.child2->_FixedRangeSearch(pts, threadNum); + } + } else { + node.child2->_FixedRangeSearch(pts, threadNum); + if (sqr(myd) < params[threadNum].closest_d2) { + node.child1->_FixedRangeSearch(pts, threadNum); + } + } + } + + + void _KNNSearch(const PointData& pts, int threadNum) const { + AccessorFunc point; + + // Leaf nodes + if (npts) { + for (int i = 0; i < npts; i++) { + double myd2 = Dist2(params[threadNum].p, point(pts, leaf.p[i])); + + if (myd2 < params[threadNum].closest_d2) { + Point newPt; + double* currPt = point(pts, leaf.p[i]); + newPt.x = currPt[0]; + newPt.y = currPt[1]; + newPt.z = currPt[2]; + params[threadNum].heap.push_back(std::make_pair(newPt, myd2)); + std::push_heap(params[threadNum].heap.begin(), + params[threadNum].heap.end(), + PointCompare()); + + params[threadNum].closest = point(pts, leaf.p[i]); + } + } + return; + } + + // Quick check of whether to abort + double approx_dist_bbox = + max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx, + fabs(params[threadNum].p[1]-node.center[1])-node.dy), + fabs(params[threadNum].p[2]-node.center[2])-node.dz); + if (approx_dist_bbox >= 0 && + sqr(approx_dist_bbox) >= params[threadNum].closest_d2) + return; + + // Recursive case + double myd = node.center[node.splitaxis] - params[threadNum].p[node.splitaxis]; + if (myd >= 0.0) { + node.child1->_KNNSearch(pts, threadNum); + if (sqr(myd) < params[threadNum].closest_d2) { + node.child2->_KNNSearch(pts, threadNum); + } + } else { + node.child2->_KNNSearch(pts, threadNum); + if (sqr(myd) < params[threadNum].closest_d2) { + node.child1->_KNNSearch(pts, threadNum); + } + } + } +}; + + +#endif diff --git a/.svn/pristine/3b/3bb9b87798e851dc40765533de4663e756f74607.svn-base b/.svn/pristine/3b/3bb9b87798e851dc40765533de4663e756f74607.svn-base new file mode 100644 index 0000000..5acc395 --- /dev/null +++ b/.svn/pristine/3b/3bb9b87798e851dc40765533de4663e756f74607.svn-base @@ -0,0 +1,39 @@ +/** + * @file + * @brief IO of a 3D scan given in ply format + * @author Andreas Nuechter + */ + +#ifndef __SCAN_IO_PLY_H__ +#define __SCAN_IO_PLY_H__ + +#include "scan_io.h" + + +/** + * @brief 3D scan loader for UOS scans + * + * The compiled class is available as shared object file + */ +class ScanIO_ply : public ScanIO { +public: + virtual std::list readDirectory(const char* dir_path, + unsigned int start, + unsigned int end); + virtual void readPose(const char* dir_path, + const char* identifier, + double* pose); + virtual void readScan(const char* dir_path, + const char* identifier, + PointFilter& filter, + std::vector* xyz, + std::vector* rgb, + std::vector* reflectance, + std::vector* temperature, + std::vector* amplitude, + std::vector* type, + std::vector* deviation); + virtual bool supports(IODataType type); +}; + +#endif diff --git a/.svn/pristine/47/479c520a2b9c7b114a8d0cbbeb1af1d6aa426f6e.svn-base b/.svn/pristine/47/479c520a2b9c7b114a8d0cbbeb1af1d6aa426f6e.svn-base new file mode 100644 index 0000000..532b0c2 --- /dev/null +++ b/.svn/pristine/47/479c520a2b9c7b114a8d0cbbeb1af1d6aa426f6e.svn-base @@ -0,0 +1,68 @@ +/** @file + * @brief Representation of the optimized k-d tree. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Thomas Escher + */ + +#ifndef __KD_H__ +#define __KD_H__ + +#include "slam6d/kdparams.h" +#include "slam6d/searchTree.h" +#include "slam6d/kdTreeImpl.h" + +#ifdef _MSC_VER +#if !defined _OPENMP && defined OPENMP +#define _OPENMP +#endif +#endif + +#ifdef _OPENMP +#include +#endif + + +struct Void { }; + +struct PtrAccessor { + inline double *operator() (Void, double* indices) { + return indices; + } +}; + +/** + * @brief The optimized k-d tree. + * + * A kD tree for points, with limited + * capabilities (find nearest point to + * a given point, or to a ray). + **/ +class KDtree : public SearchTree, private KDTreeImpl +{ +public: + KDtree(double **pts, int n); + + virtual ~KDtree(); + + virtual double *FindClosest(double *_p, + double maxdist2, + int threadNum = 0) const; + + virtual double *FindClosestAlongDir(double *_p, + double *_dir, + double maxdist2, + int threadNum = 0) const; + + virtual vector kNearestNeighbors(double *_p, + int k, + double sqRad2, + int threadNum = 0) const; + + virtual vector fixedRangeSearch(double *_p, + double sqRad2, + int threadNum = 0) const; + +}; + +#endif diff --git a/.svn/pristine/4b/4baca755b8ef2733188c331eba089f8ff064bfcc.svn-base b/.svn/pristine/4b/4baca755b8ef2733188c331eba089f8ff064bfcc.svn-base new file mode 100644 index 0000000..e8442ab --- /dev/null +++ b/.svn/pristine/4b/4baca755b8ef2733188c331eba089f8ff064bfcc.svn-base @@ -0,0 +1,186 @@ +/* + * searchTree implementation + * + * Copyright (C) Jan Elseberg, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Representation of a general search trees + * @author Jan Elseberg. Jacobs University Bremen gGmbH, Germany + * @author Andreas Nuechter. Jacobs University Bremen gGmbH, Germany + */ + +#include "slam6d/searchTree.h" +#include "slam6d/scan.h" +#include "slam6d/globals.icc" + +#include + +double *SearchTree::FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const +{ + throw std::runtime_error("Method FindClosestAlongDir is not implemented"); +} + +void SearchTree::getPtPairs(vector *pairs, + double *source_alignxf, // source + double * const *q_points, + unsigned int startindex, unsigned int endindex, // target + int thread_num, + int rnd, double max_dist_match2, double &sum, + double *centroid_m, double *centroid_d) +{ + // prepare this tree for resource access in FindClosest + lock(); + + double local_alignxf_inv[16]; + M4inv(source_alignxf, local_alignxf_inv); + + // t is the original point from target, s is the (inverted) query point from target and then + // the closest point in source + double t[3], s[3]; + for (unsigned int i = startindex; i < endindex; i++) { + if (rnd > 1 && rand(rnd) != 0) continue; // take about 1/rnd-th of the numbers only + + t[0] = q_points[i][0]; + t[1] = q_points[i][1]; + t[2] = q_points[i][2]; + + transform3(local_alignxf_inv, t, s); + + double *closest = this->FindClosest(s, max_dist_match2, thread_num); + if (closest) { + transform3(source_alignxf, closest, s); + + // This should be right, model=Source=First=not moving + centroid_m[0] += s[0]; + centroid_m[1] += s[1]; + centroid_m[2] += s[2]; + centroid_d[0] += t[0]; + centroid_d[1] += t[1]; + centroid_d[2] += t[2]; + + PtPair myPair(s, t); + double p12[3] = { + myPair.p1.x - myPair.p2.x, + myPair.p1.y - myPair.p2.y, + myPair.p1.z - myPair.p2.z }; + sum += Len2(p12); + + pairs->push_back(myPair); + /*cout << "PTPAIR" << i << " " + << p[0] << " " + << p[1] << " " + << p[2] << " - " + << q_points[i][0] << " " + << q_points[i][1] << " " + << q_points[i][2] << " " << Len2(p12) << endl; */ + } + + } + + // release resource access lock + unlock(); + + return; +} + +void SearchTree::getPtPairs(vector *pairs, + double *source_alignxf, // source + const DataXYZ& xyz_r, const DataNormal& normal_r, + unsigned int startindex, unsigned int endindex, // target + int thread_num, + int rnd, double max_dist_match2, double &sum, + double *centroid_m, double *centroid_d, + PairingMode pairing_mode) +{ + // prepare this tree for resource access in FindClosest + lock(); + + double local_alignxf_inv[16]; + M4inv(source_alignxf, local_alignxf_inv); + + // t is the original point from target, s is the (inverted) query point from target and then + // the closest point in source + double t[3], s[3], normal[3]; + for (unsigned int i = startindex; i < endindex; i++) { + if (rnd > 1 && rand(rnd) != 0) continue; // take about 1/rnd-th of the numbers only + + t[0] = xyz_r[i][0]; + t[1] = xyz_r[i][1]; + t[2] = xyz_r[i][2]; + + transform3(local_alignxf_inv, t, s); + + double *closest; + + if (pairing_mode != CLOSEST_POINT) { + normal[0] = normal_r[i][0]; + normal[1] = normal_r[i][1]; + normal[2] = normal_r[i][2]; + Normalize3(normal); + } + + if (pairing_mode == CLOSEST_POINT_ALONG_NORMAL) { + transform3normal(local_alignxf_inv, normal); + closest = this->FindClosestAlongDir(s, normal, max_dist_match2, thread_num); + + // discard points farther than 20 cm + if (closest && sqrt(Dist2(closest, s)) > 20) closest = NULL; + } else { + closest = this->FindClosest(s, max_dist_match2, thread_num); + } + + if (closest) { + transform3(source_alignxf, closest, s); + + if (pairing_mode == CLOSEST_PLANE) { + // need to mutate s if we are looking for closest point-to-plane + // s_ = (n,s-t)*n + t + // to find the projection of s onto plane formed by normal n and point t + double tmp[3], s_[3]; + double dot; + sub3(s, t, tmp); + dot = Dot(normal, tmp); + scal_mul3(normal, dot, tmp); + add3(tmp, t, s_); + s[0] = s_[0]; + s[1] = s_[1]; + s[2] = s_[2]; + } + + // This should be right, model=Source=First=not moving + centroid_m[0] += s[0]; + centroid_m[1] += s[1]; + centroid_m[2] += s[2]; + centroid_d[0] += t[0]; + centroid_d[1] += t[1]; + centroid_d[2] += t[2]; + + PtPair myPair(s, t); + double p12[3] = { + myPair.p1.x - myPair.p2.x, + myPair.p1.y - myPair.p2.y, + myPair.p1.z - myPair.p2.z }; + sum += Len2(p12); + + pairs->push_back(myPair); + /*cout << "PTPAIR" << i << " " + << p[0] << " " + << p[1] << " " + << p[2] << " - " + << q_points[i][0] << " " + << q_points[i][1] << " " + << q_points[i][2] << " " << Len2(p12) << endl; */ + } + + } + + // release resource access lock + unlock(); + + return; +} diff --git a/.svn/pristine/57/57865b7af16ec9f8e2193fd95818a6115330471c.svn-base b/.svn/pristine/57/57865b7af16ec9f8e2193fd95818a6115330471c.svn-base new file mode 100644 index 0000000..6daa88c --- /dev/null +++ b/.svn/pristine/57/57865b7af16ec9f8e2193fd95818a6115330471c.svn-base @@ -0,0 +1,42 @@ +/** + * @file scan_io_uosr.h + * @brief IO of a 3D scan in xyz file format plus an intensity + + * @author Billy Okal + */ + +#ifndef __SCAN_IO_XYZR_H__ +#define __SCAN_IO_XYZR_H__ + +#include "scan_io.h" + + + +/** + * @brief IO of a 3D scan in uos file format plus a + * reflectance/intensity/temperature value + * + * The compiled class is available as shared object file + */ +class ScanIO_xyzr : public ScanIO { +public: + virtual std::list readDirectory(const char* dir_path, + unsigned int start, + unsigned int end); + virtual void readPose(const char* dir_path, + const char* identifier, + double* pose); + virtual void readScan(const char* dir_path, + const char* identifier, + PointFilter& filter, + std::vector* xyz, + std::vector* rgb, + std::vector* reflectance, + std::vector* temperature, + std::vector* amplitude, + std::vector* type, + std::vector* deviation); + virtual bool supports(IODataType type); +}; + +#endif diff --git a/.svn/pristine/58/583c8601c2c72a73e576e447d85ba661b728eff3.svn-base b/.svn/pristine/58/583c8601c2c72a73e576e447d85ba661b728eff3.svn-base new file mode 100644 index 0000000..734be75 --- /dev/null +++ b/.svn/pristine/58/583c8601c2c72a73e576e447d85ba661b728eff3.svn-base @@ -0,0 +1,55 @@ +/** + * @file + * @brief Representation of the parameter of a k-d tree + * @author Andreas Nuechter. jacobs University Bremen, Germany. + */ + +#ifndef __KDPARAMS_H__ +#define __KDPARAMS_H__ + +#include "slam6d/point.h" + +#include + +/** + * @brief Contains the intermediate (static) values of a k-d tree + * + * A parameter class for the latter k-d tree. + * Includes the padding for parallelizetion + * to avoid cache conflicts. + **/ +class KDParams +{ +public: + /** + * pointer to the closest point. size = 4 bytes of 32 bit machines + */ + double *closest; + + /** + * distance to the closest point. size = 8 bytes + */ + double closest_d2; + + /** + * pointer to the point, size = 4 bytes of 32 bit machines + */ + double *p; + + /** + * pointer to direction vector, if we're using FindClosestAlongDir + */ + double *dir; + + /** + * heap for KNN. + */ + std::vector > heap; + + /** + * expand to 128 bytes to avoid false-sharing, 16 bytes from above + 28*4 bytes = 128 bytes + */ + int padding[28]; +}; + +#endif diff --git a/.svn/pristine/58/584b0b2cdfed9982db5b177391c5ecb352cfb0d5.svn-base b/.svn/pristine/58/584b0b2cdfed9982db5b177391c5ecb352cfb0d5.svn-base new file mode 100644 index 0000000..cb952f7 --- /dev/null +++ b/.svn/pristine/58/584b0b2cdfed9982db5b177391c5ecb352cfb0d5.svn-base @@ -0,0 +1,851 @@ +/* + * compacttree implementation + * + * Copyright (C) Jan Elseberg, Kai Lingemann, Jan Elseberg + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Efficient representation of an octree + * @author Jan Elsberg. Automation Group, Jacobs University Bremen gGmbH, Germany. + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + */ + +#include + +#include +using std::vector; +#include +using std::deque; +#include +using std::set; +#include +using std::list; +#include +#include +#include + +#include "slam6d/globals.icc" +#include "slam6d/point_type.h" + +#include "slam6d/Boctree.h" +#include "show/compacttree.h" +#include "show/colormanager.h" +#include "show/scancolormanager.h" +#include "show/viewcull.h" + +using namespace show; +compactTree::~compactTree(){ + delete alloc; + + delete[] mins; + delete[] maxs; +} + + +void compactTree::AllPoints( cbitoct &node, vector &vp, double center[3], double size) { + double ccenter[3]; + cbitunion *children; + cbitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + childcenter(center, ccenter, size, i); // childrens center + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center + tshort *point = children->getPoints(); + lint length = children->getLength(); + + for(unsigned int iterator = 0; iterator < length; iterator++ ) { + double *p = new double[3]; + //cout << point[0] << " " << point[1] << " " << point[2] << endl; + for (unsigned int k = 0; k < 3; k++){ + p[k] = point[k] * precision + ccenter[k]; + } + + vp.push_back(p); + point+=POINTDIM; + } + } else { // recurse + AllPoints( children->node, vp, ccenter, size/2.0); + } + ++children; // next child + } + } +} + + + +void compactTree::GetOctTreeCenter(vector&c, cbitoct &node, double *center, double size) { + double ccenter[3]; + cbitunion *children; + cbitoct::getChildren(node, children); + + for (unsigned char i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + childcenter(center, ccenter, size, i); // childrens center + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center + double * cp = new double[POINTDIM]; + for (unsigned int iterator = 0; iterator < POINTDIM; iterator++) { + cp[iterator] = ccenter[iterator]; + } + c.push_back(cp); + } else { // recurse + GetOctTreeCenter(c, children->node, ccenter, size/2.0); + } + ++children; // next child + } + } +} +long compactTree::countNodes(cbitoct &node) { + long result = 0; + cbitunion *children; + cbitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf + // ++result; + } else { // recurse + result += countNodes(children->node) + 1; + } + ++children; // next child + } + } + return result; +} + +long compactTree::countLeaves(cbitoct &node) { + long result = 0; + cbitunion *children; + cbitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf + lint nrpts = children->getLength(); + result += POINTDIM*nrpts + 1; + } else { // recurse + result += countLeaves(children->node); + } + ++children; // next child + } + } + return result; +} + + +void compactTree::deletetNodes(cbitoct &node) { + cbitunion *children; + cbitoct::getChildren(node, children); + bool haschildren = false; + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf + tshort *points = children->getPoints(); + delete [] points; + // delete [] children->points; + } else { // recurse + deletetNodes(children->node); + } + ++children; // next child + haschildren = true; + } + } + // delete children + if (haschildren) { + cbitoct::getChildren(node, children); + delete[] children; + } +} + + +unsigned long compactTree::maxTargetPoints( cbitoct &node ) { + cbitunion *children; + cbitoct::getChildren(node, children); + + unsigned long max = 0; + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center + lint length = children->getLength(); + if (length > max) max = length; + } else { // recurse + unsigned long tp = maxTargetPoints( children->node); + if (tp > max) max = tp; + } + ++children; // next child + } + } + + return max*POPCOUNT(node.valid); +} + +void compactTree::displayOctTreeAll( cbitoct &node, double *center, double size) { + double ccenter[3]; + cbitunion *children; + cbitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + childcenter(center, ccenter, size, i); // childrens center + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center + tshort *point = children->getPoints(); + lint length = children->getLength(); + glBegin(GL_POINTS); + for(unsigned int iterator = 0; iterator < length; iterator++ ) { + if(cm) cm->setColor(point); + //cout << "C " << point[1] << " " << cm << endl; + //glVertex3f( point[0], point[1], point[2]); + glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]); + point+=POINTDIM; + } + glEnd(); + } else { // recurse + displayOctTreeAll( children->node, ccenter, size/2.0); + } + ++children; // next child + } + } +} + +void compactTree::displayOctTreeAllCulled( cbitoct &node, double *center, double size ) { + int res = CubeInFrustum2(center[0], center[1], center[2], size); + if (res==0) return; // culled do not continue with this branch of the tree + + if (res == 2) { // if entirely within frustrum discontinue culling + displayOctTreeAll(node, center, size); + return; + } + + double ccenter[3]; + cbitunion *children; + cbitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + childcenter(center, ccenter, size, i); // childrens center + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center + // check if leaf is visible + if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) { + tshort *point = children->getPoints(); + lint length = children->getLength(); + glBegin(GL_POINTS); + for(unsigned int iterator = 0; iterator < length; iterator++ ) { + if(cm) cm->setColor(point); + //glVertex3f( point[0], point[1], point[2]); + glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]); + point+=POINTDIM; + } + glEnd(); + } + } else { // recurse + displayOctTreeAllCulled( children->node, ccenter, size/2.0); + } + ++children; // next child + } + } +} + +void compactTree::displayOctTreeCulledLOD(long targetpts, cbitoct &node, double *center, double size ) { + if (targetpts <= 0) return; // no need to display anything + + int res = CubeInFrustum2(center[0], center[1], center[2], size); + if (res==0) return; // culled do not continue with this branch of the tree + + if (res == 2) { // if entirely within frustrum discontinue culling + displayOctTreeLOD(targetpts, node, center, size); + return; + } + + double ccenter[3]; + cbitunion *children; + cbitoct::getChildren(node, children); + + unsigned short nc = POPCOUNT(node.valid); + long newtargetpts = targetpts; + if (nc > 0) { + newtargetpts = newtargetpts/nc; + if (newtargetpts <= 0 ) return; + } + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + childcenter(center, ccenter, size, i); // childrens center + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center + // check if leaf is visible + if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) { + tshort *point = children->getPoints(); + lint length = children->getLength(); + glBegin(GL_POINTS); + if (length > 10 && !LOD(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) { // only a single pixel on screen only paint one point + if(cm) cm->setColor(point); + //glVertex3f( point[0], point[1], point[2]); + glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]); + } else if (length <= newtargetpts) { // more points requested than possible, plot all + for(unsigned int iterator = 0; iterator < length; iterator++ ) { + if(cm) cm->setColor(point); + //glVertex3f( point[0], point[1], point[2]); + glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]); + point+=POINTDIM; + } + } else { // select points to show + // TODO smarter subselection of points here + double each = (double)POINTDIM * (double)((double)length/(double)newtargetpts); + tshort *p; + int index; + for(unsigned int iterator = 0; iterator < newtargetpts; iterator++ ) { + index = (double)iterator * each; + p = point + index - index%POINTDIM; + if(cm) cm->setColor(p); + //glVertex3f( p[0], p[1], p[2]); + glVertex3f( p[0] * precision + ccenter[0], p[1] * precision + ccenter[1], p[2] * precision + ccenter[2]); + //point += each; + } + } + glEnd(); + } + + } else { // recurse + displayOctTreeCulledLOD(newtargetpts, children->node, ccenter, size/2.0); + } + ++children; // next child + } + } +} + +void compactTree::displayOctTreeLOD(long targetpts, cbitoct &node, double *center, double size ) { + if (targetpts <= 0) return; // no need to display anything + + double ccenter[3]; + cbitunion *children; + cbitoct::getChildren(node, children); + + unsigned short nc = POPCOUNT(node.valid); + long newtargetpts = targetpts; + if (nc > 0) { + newtargetpts = newtargetpts/nc; + if (newtargetpts <= 0 ) return; + } + + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + childcenter(center, ccenter, size, i); // childrens center + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center + tshort *point = children->getPoints(); + lint length = children->getLength(); + glBegin(GL_POINTS); + /* if (length > 10 && !LOD(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) { // only a single pixel on screen only paint one point + if(cm) cm->setColor(point); + //glVertex3f( point[0], point[1], point[2]); + glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]); + } else*/ if (length <= newtargetpts) { // more points requested than possible, plot all + for(unsigned int iterator = 0; iterator < length; iterator++ ) { + if(cm) cm->setColor(point); + //glVertex3f( point[0], point[1], point[2]); + glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]); + point+=POINTDIM; + } + } else { // select points to show + // TODO smarter subselection of points here + double each = (double)POINTDIM * (double)((double)length/(double)newtargetpts); + tshort *p; + int index; + for(unsigned int iterator = 0; iterator < newtargetpts; iterator++ ) { + index = (double)iterator * each; + p = point + index - index%POINTDIM; + if(cm) cm->setColor(p); + //glVertex3f( p[0], p[1], p[2]); + glVertex3f( p[0] * precision + ccenter[0], p[1] * precision + ccenter[1], p[2] * precision + ccenter[2]); + //point += each; + } + } + glEnd(); + } else { // recurse + displayOctTreeLOD(newtargetpts, children->node, ccenter, size/2.0); + } + ++children; // next child + } + } +} + +void compactTree::displayOctTreeCulledLOD2(float ratio, cbitoct &node, double *center, double size ) { + int res = CubeInFrustum2(center[0], center[1], center[2], size); + if (res==0) return; // culled do not continue with this branch of the tree + + if (res == 2) { // if entirely within frustrum discontinue culling + displayOctTreeLOD2(ratio, node, center, size); + return; + } + + double ccenter[3]; + cbitunion *children; + cbitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + childcenter(center, ccenter, size, i); // childrens center + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center + // check if leaf is visible + if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) { + tshort *point = children->getPoints(); + lint length = children->getLength(); + + int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0); // only a single pixel on screen only paint one point + l = max((int)(l*l*ratio), 0); + if (l != 0) { + if ((int)length > l ) { + double each = (double)POINTDIM * (double)((double)length/(double)l); + tshort *p; + int index; + for(int iterator = 0; iterator < l; iterator++ ) { + index = (double)iterator * each; + p = point + index - index%POINTDIM; + if(cm) cm->setColor(p); + glVertex3f( p[0] * precision + ccenter[0], p[1] * precision + ccenter[1], p[2] * precision + ccenter[2]); + } + } else if ((int)length <= l) { + for(unsigned int iterator = 0; iterator < length; iterator++ ) { + if(cm) cm->setColor(point); + glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]); + point+=POINTDIM; + } + } else if (l == 1) { + if(cm) cm->setColor(point); + glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]); + } + } + } + } else { // recurse + int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0); // only a single pixel on screen only paint one point + l = max((int)(l*l*ratio), 0); + if (l > 0) { + displayOctTreeCulledLOD2(ratio, children->node, ccenter, size/2.0); + } + } + ++children; // next child + } + } +} + +void compactTree::displayOctTreeLOD2(float ratio, cbitoct &node, double *center, double size ) { + double ccenter[3]; + cbitunion *children; + cbitoct::getChildren(node, children); + + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + childcenter(center, ccenter, size, i); // childrens center + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center + tshort *point = children->getPoints(); + lint length = children->getLength(); + + int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0); // only a single pixel on screen only paint one point + l = max((int)(l*l*ratio), 0); + if (l > 1) { + if ((int)length > l ) { + double each = (double)POINTDIM * (double)((double)length/(double)l); + tshort *p; + int index; + for(int iterator = 0; iterator < l; iterator++ ) { + index = (double)iterator * each; + p = point + index - index%POINTDIM; + if(cm) cm->setColor(p); + glVertex3f( p[0] * precision + ccenter[0], p[1] * precision + ccenter[1], p[2] * precision + ccenter[2]); + } + } else if ((int)length <= l) { + for(unsigned int iterator = 0; iterator < length; iterator++ ) { + if(cm) cm->setColor(point); + glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]); + point+=POINTDIM; + } + } + } else { + if(cm) cm->setColor(point); + glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]); + } + } else { // recurse + int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0); // only a single pixel on screen only paint one point + l = max((int)(l*l*ratio), 0); + if (l > 0) { + displayOctTreeLOD2(ratio, children->node, ccenter, size/2.0); + } + } + ++children; // next child + } + } +} + + +void compactTree::displayOctTreeCAllCulled( cbitoct &node, double *center, double size, double minsize ) { + int res = CubeInFrustum2(center[0], center[1], center[2], size); + if (res==0) return; // culled do not continue with this branch of the tree + + if (res == 2) { // if entirely within frustrum discontinue culling + displayOctTreeCAll(node, center, size, minsize); + return; + } + + double ccenter[3]; + cbitunion *children; + cbitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + childcenter(center, ccenter, size, i); // childrens center + if ( ( 1 << i ) & node.leaf || minsize > size ) { // if ith node is leaf get center + // check if leaf is visible + if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) { + showCube(ccenter, size/2.0); + } + } else { // recurse + displayOctTreeCAllCulled( children->node, ccenter, size/2.0, minsize); + } + ++children; // next child + } + } +} + +void compactTree::displayOctTreeCAll( cbitoct &node, double *center, double size, double minsize ) { + double ccenter[3]; + cbitunion *children; + cbitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + childcenter(center, ccenter, size, i); // childrens center + if ( ( 1 << i ) & node.leaf || minsize > size ) { // if ith node is leaf get center + showCube(ccenter, size/2.0); + } else { // recurse + displayOctTreeCAll( children->node, ccenter, size/2.0, minsize); + } + ++children; // next child + } + } +} + +void compactTree::showCube(double *center, double size) { + glLineWidth(1.0); + glBegin(GL_QUADS); // draw a cube with 6 quads + glColor3f(0.0f,1.0f,0.0f); // Set The Color To Green + glVertex3f(center[0] + size, center[1] + size, center[2] - size); + glVertex3f(center[0] - size, center[1] + size, center[2] - size); + glVertex3f(center[0] - size, center[1] + size, center[2] + size); + glVertex3f(center[0] + size, center[1] + size, center[2] + size); + glColor3f(1.0f,0.5f,0.0f); // Set The Color To Orange + + glVertex3f(center[0] + size, center[1] - size, center[2] + size); + glVertex3f(center[0] - size, center[1] - size, center[2] + size); + glVertex3f(center[0] - size, center[1] - size, center[2] - size); + glVertex3f(center[0] + size, center[1] - size, center[2] - size); + + glColor3f(1.0f,0.0f,0.0f); // Set The Color To Red + glVertex3f(center[0] + size, center[1] + size, center[2] + size); + glVertex3f(center[0] - size, center[1] + size, center[2] + size); + glVertex3f(center[0] - size, center[1] - size, center[2] + size); + glVertex3f(center[0] + size, center[1] - size, center[2] + size); + + glColor3f(1.0f,1.0f,0.0f); // Set The Color To Yellow + glVertex3f(center[0] + size, center[1] - size, center[2] - size); + glVertex3f(center[0] - size, center[1] - size, center[2] - size); + glVertex3f(center[0] - size, center[1] + size, center[2] - size); + glVertex3f(center[0] + size, center[1] + size, center[2] - size); + + glColor3f(0.0f,0.0f,1.0f); // Set The Color To Blue + glVertex3f(center[0] - size, center[1] + size, center[2] + size); + glVertex3f(center[0] - size, center[1] + size, center[2] - size); + glVertex3f(center[0] - size, center[1] - size, center[2] - size); + glVertex3f(center[0] - size, center[1] - size, center[2] + size); + + glColor3f(1.0f,0.0f,1.0f); // Set The Color To Violet + glVertex3f(center[0] + size, center[1] + size, center[2] - size); + glVertex3f(center[0] + size, center[1] + size, center[2] + size); + glVertex3f(center[0] + size, center[1] - size, center[2] + size); + glVertex3f(center[0] + size, center[1] - size, center[2] - size); + + + glEnd(); + +} + + + + +template +void compactTree::selectRay(vector &points) { + //selectRay(points, *root, center, size); +} + + +void compactTree::childcenter(double *pcenter, double *ccenter, double size, unsigned char i) { + switch (i) { + case 0: + ccenter[0] = pcenter[0] - size / 2.0; + ccenter[1] = pcenter[1] - size / 2.0; + ccenter[2] = pcenter[2] - size / 2.0; + break; + case 1: + ccenter[0] = pcenter[0] + size / 2.0; + ccenter[1] = pcenter[1] - size / 2.0; + ccenter[2] = pcenter[2] - size / 2.0; + break; + case 2: + ccenter[0] = pcenter[0] - size / 2.0; + ccenter[1] = pcenter[1] + size / 2.0; + ccenter[2] = pcenter[2] - size / 2.0; + break; + case 3: + ccenter[0] = pcenter[0] + size / 2.0; + ccenter[1] = pcenter[1] + size / 2.0; + ccenter[2] = pcenter[2] - size / 2.0; + break; + case 4: + ccenter[0] = pcenter[0] - size / 2.0; + ccenter[1] = pcenter[1] - size / 2.0; + ccenter[2] = pcenter[2] + size / 2.0; + break; + case 5: + ccenter[0] = pcenter[0] + size / 2.0; + ccenter[1] = pcenter[1] - size / 2.0; + ccenter[2] = pcenter[2] + size / 2.0; + break; + case 6: + ccenter[0] = pcenter[0] - size / 2.0; + ccenter[1] = pcenter[1] + size / 2.0; + ccenter[2] = pcenter[2] + size / 2.0; + break; + case 7: + ccenter[0] = pcenter[0] + size / 2.0; + ccenter[1] = pcenter[1] + size / 2.0; + ccenter[2] = pcenter[2] + size / 2.0; + break; + default: + break; + } +} + + +void compactTree::GetOctTreeCenter(vector&c) { GetOctTreeCenter(c, *root, center, size); } +void compactTree::AllPoints(vector &vp) { AllPoints(*compactTree::root, vp, center, size); } + +long compactTree::countNodes() { return 1 + countNodes(*root); } +long compactTree::countLeaves() { return 1 + countLeaves(*root); } + +void compactTree::setColorManager(ColorManager *_cm) { cm = _cm; } + +void compactTree::drawLOD(float ratio) { + switch (current_lod_mode) { + case 1: + glBegin(GL_POINTS); + displayOctTreeCulledLOD2(ratio , *root, center, size); + glEnd(); + break; + case 2: + /* +#ifdef WITH_GLEE + if (GLEE_ARB_point_parameters) { + glPointParameterfARB(GL_POINT_SIZE_MIN_ARB, 1.0); + glPointParameterfARB(GL_POINT_SIZE_MAX_ARB, 100000.0); + GLfloat p[3] = {0.0, 0.0000, 0.0000005}; + glPointParameterfvARB(GL_POINT_DISTANCE_ATTENUATION_ARB, p); + displayOctTreeCPAllCulled(*BOctTree::root, BOctTree::center, BOctTree::size, BOctTree::size/ pow(2, min( (int)(ratio * BOctTree::max_depth ), BOctTree::max_depth - 3) ) ); + p[0] = 1.0; + p[2] = 0.0; + glPointParameterfvARB(GL_POINT_DISTANCE_ATTENUATION_ARB, p); + } +#endif +*/ + //break; + case 0: + glBegin(GL_POINTS); + displayOctTreeCulledLOD(maxtargetpoints * ratio, *root, center, size); + glEnd(); + break; + default: + break; + } +} + +void compactTree::draw() { + displayOctTreeAllCulled(*root, center, size); +} + +void compactTree::displayOctTree(double minsize ) { + displayOctTreeCAllCulled(*root, center, size, minsize); +} + +shortpointrep* compactTree::createPoints(lint length) { + //shortpointrep *points = new shortpointrep[POINTDIM*length]; + shortpointrep *points = alloc->allocate (POINTDIM*length); + return points; +} + +void compactTree::deserialize(std::string filename) +{ + char buffer[sizeof(float) * 20]; + float *p = reinterpret_cast(buffer); + + std::ifstream file; + file.open (filename.c_str(), std::ios::in | std::ios::binary); + + // read magic bits + file.read(buffer, 2); + if ( buffer[0] != 'X' || buffer[1] != 'T') { + std::cerr << "Not an octree file!!" << endl; + file.close(); + return; + } + + // read header + pointtype = PointType::deserialize(file); + + file.read(buffer, 5 * sizeof(float)); + voxelSize = p[0]; + center[0] = p[1]; + center[1] = p[2]; + center[2] = p[3]; + size = p[4]; + + file.read(buffer, sizeof(int)); + int *ip = reinterpret_cast(buffer); + POINTDIM = *ip; + + float *fmins = new float[POINTDIM]; + float *fmaxs = new float[POINTDIM]; + mins = new double[POINTDIM]; + maxs = new double[POINTDIM]; + + file.read(reinterpret_cast(fmins), POINTDIM * sizeof(float)); + file.read(reinterpret_cast(fmaxs), POINTDIM * sizeof(float)); + + for (unsigned int i = 0; i < POINTDIM; i++) { + mins[i] = fmins[i]; + maxs[i] = fmaxs[i]; + } + + double vs = size; + while (vs > voxelSize) { + vs = vs * 0.5; + } +// precision = vs / 32768; // 2^15 + precision = vs / TSHORT_MAXP1; // 2^15 + + + // read root node + //root = new cbitoct(); + root = alloc->allocate(); + deserialize(file, *root ); + file.close(); +} + + + + +void compactTree::deserialize(std::ifstream &f, cbitoct &node) { + char buffer[2]; + f.read(buffer, 2); + node.valid = buffer[0]; + node.leaf = buffer[1]; + + unsigned short n_children = POPCOUNT(node.valid); + + // create children + //cbitunion *children = new cbitunion[n_children]; + cbitunion *children = alloc->allocate >(n_children); + cbitoct::link(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf read points + lint length; + f.read(reinterpret_cast(&length), sizeof(lint)); + shortpointrep *points = createPoints(length); + + f.read(reinterpret_cast(points), sizeof(shortpointrep) * length * POINTDIM); // read the points + children->linkPoints(points, length); + } else { // write child + deserialize(f, children->node); + } + ++children; // next child + } + } +} + + + +void compactTree::serialize(std::string filename) { + char buffer[sizeof(float) * 20]; + float *p = reinterpret_cast(buffer); + + std::ofstream file; + file.open (filename.c_str(), std::ios::out | std::ios::binary); + + // write magic bits + buffer[0] = 'X'; + buffer[1] = 'T'; + file.write(buffer, 2); + + // write header + pointtype.serialize(file); + + p[0] = voxelSize; + p[1] = center[0]; + p[2] = center[1]; + p[3] = center[2]; + p[4] = size; + + int *ip = reinterpret_cast(&(buffer[5 * sizeof(float)])); + *ip = POINTDIM; + + file.write(buffer, 5 * sizeof(float) + sizeof(int)); + + + for (unsigned int i = 0; i < POINTDIM; i++) { + p[i] = mins[i]; + } + for (unsigned int i = 0; i < POINTDIM; i++) { + p[i+POINTDIM] = maxs[i]; + } + + file.write(buffer, 2*POINTDIM * sizeof(float)); + + // write root node + serialize(file, *root); + + file.close(); +} + + +void compactTree::serialize(std::ofstream &of, cbitoct &node) { + char buffer[2]; + buffer[0] = node.valid; + buffer[1] = node.leaf; + of.write(buffer, 2); + + // write children + cbitunion *children; + cbitoct::getChildren(node, children); + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf write points + tshort *points = children->getPoints(); + lint length = children->getLength(); + + of.write(reinterpret_cast(&length), sizeof(lint) ); + + of.write(reinterpret_cast(points), POINTDIM*length*sizeof(tshort) ); + + } else { // write child + serialize(of, children->node); + } + ++children; // next child + } + } +} diff --git a/.svn/pristine/5d/5d4ec32097e810110fac54c59f2cdf6621e44165.svn-base b/.svn/pristine/5d/5d4ec32097e810110fac54c59f2cdf6621e44165.svn-base new file mode 100644 index 0000000..489e7ce --- /dev/null +++ b/.svn/pristine/5d/5d4ec32097e810110fac54c59f2cdf6621e44165.svn-base @@ -0,0 +1,1077 @@ +/* + * show_common implementation + * + * Copyright (C) Kai Lingemann, Andreas Nuechter, Jan Elseberg, Dorit Borrmann + * + * Released under the GPL version 3. + * + */ + +#ifdef WITH_GLEE +#include +#endif + +#include "show/show.h" +#include "show/show_Boctree.h" +#include "show/compacttree.h" +#include "show/NurbsPath.h" +#include "show/vertexarray.h" +#ifndef DYNAMIC_OBJECT_REMOVAL +#include "slam6d/scan.h" +#include "slam6d/managedScan.h" +#else +#include "veloslam/veloscan.h" +#endif +#include "glui/glui.h" /* Header File For The glui functions */ +#include +using std::ifstream; +#include +using std::exception; +#include + + +#ifdef _MSC_VER +#include "XGetopt.h" +#else +#include +#endif + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + + +#ifdef _MSC_VER +#if !defined _OPENMP && defined OPENMP +#define _OPENMP +#endif +#endif + +#ifdef _OPENMP +#include +#endif + +#include "slam6d/point_type.h" +#include "slam6d/io_utils.h" +#include "show/display.h" + +/** + * This vector contains the pointer to a vertex array for + * all colors (inner vector) and all scans (outer vector) + */ +vector< vector > vvertexArrayList; + +vector< ::SDisplay*> displays; +/** + * the octrees that store the points for each scan + */ +//Show_BOctTree **octpts; +vector octpts; +/** + * Storing the base directory + */ +string scan_dir; + +/** + * Storing the ID of the main windows + */ +int window_id; + +/** + * Size of points + */ +GLfloat pointsize = 1.0; +int anim_delay = 5; + +/** + * Select Color Buffer + */ +GLenum buffermode = GL_BACK; + +/** + * Indicator whether and how the drawing window + * has to be updated. + * + * haveToUpdate == 1 redisplay + * haveToUpdate == 2 reshape + * haveToUpdate == 3 animation scan matching + * haveToUpdate == 4 stop animation scan matching + * haveToUpdate == 6 path animation + * haveToUpdate == 7 force redisplay with all points + */ +int haveToUpdate = 0; + +/** + * Flag for invert the scene + */ +bool invert = true; + +/** + * Flag for indicating brid eyes view + */ +bool showTopView = false; + +/** + * Flag for idicating camera add mode + */ +bool addCameraView = false; //Is the view in add box mode? + +/** + * Storing the apex angle of the camera + */ +GLfloat cangle = 60.0; // Current camera opening mode +GLfloat cangle_old = cangle; + +/** + * Current rotation axis of the scene as quaternion + */ +GLdouble quat[4] ={0.0, 0.0, 0.0, 1.0}; +GLdouble Rquat[4] ={0.0, 0.0, 0.0, 1.0}; + +/** + * Current translation of the scene + */ +GLdouble X = 0.0, Y = 0.0, Z = 0.0; +GLdouble RVX = 0.0, RVY = 0.0, RVZ = 0.0; + +/** + * Center of Mass coordinates + */ +GLdouble CoM[3] = { 0.0, 0.0, 0.0 }; + +/** + * parallel zoom (similar to apex angle) for parallel projection + */ +GLfloat pzoom = 2000.0; +GLfloat pzoom_old = pzoom; + + +/** + * Mode of the fog (exp, exp2, linear) + */ +GLint fogMode = GL_EXP; + +/** + * Indicates if fog should be shown + */ +int show_fog = 1; + +/** + * Indicates if the points should be shown + */ +int show_points = 1; // Show data points in the viewer? + +/** + * Indicates if camera boxes should be shown + */ +int show_cameras = 1; // Show the camera boxes in the viewer? + +/** + * Indicates if camera path or robot path should be shown + */ +int show_path = 1; // Show the camera movement path ? + +/** + * Camera navigation by mouse or by panel + */ +int cameraNavMouseMode = 1; + +int mouseNavX, mouseNavY; +int mouseNavButton = -1; + +double mouseRotX = 0.0; +double mouseRotY = 0.0; +double mouseRotZ = 0.0; + +bool keymap[256]; + +//@@@ +//int animate_both = 0; // Animate both scan matchin and path? + +int frameNr = 0; + +/** + * Storing of all transformation (frames for animation) of all scans + */ +vector < vector > MetaMatrix; + +/** + * Storing of AlgoType for all frames + */ +vector < vector > MetaAlgoType; + +/** + * Window position + */ +int START_X = 0; +int START_Y = 0; +int START_WIDTH = 960; +int START_HEIGHT = 540; +GLdouble aspect = (double)START_WIDTH/(double)START_HEIGHT; // Current aspect ratio +bool advanced_controls = false; + +bool fullscreen = false; +int current_width = START_WIDTH; +int current_height = START_HEIGHT; + + +// the following values are scale dependant, i.e. all values are in m +float neardistance = 0.10; +double oldneardistance = 0.10; +float maxfardistance = 400.0; +double fardistance = 400.0; +double oldfardistance = 40000.0; +double movementSpeed = 0.1; +double defaultZoom = 20.0; +GLfloat fogDensity = 0.1; +double voxelSize = 0.20; + + +float adaption_rate = 1.0; +float LevelOfDetail = 0.0001; + + +// Defines for Point Semantic +#define TYPE_UNKNOWN 0x0000 +#define TYPE_OBJECT 0x0001 +#define TYPE_GROUND 0x0002 +#define TYPE_CEILING 0x0003 + +unsigned int cam_choice = 0; + +static unsigned int path_iterator = 0; +static int oldcamNavMode = 0; + +/** + * Animation sould be saved to file + */ +int save_animation = 0; +/** + * If true, interpolation for camera path is based on distance, else always + * the same number of images between two cameras. + */ +int inter_by_dist = 1; + +/**some variables for the camera path**/ +vector path_vectorX, path_vectorZ, lookat_vectorX, lookat_vectorZ, ups_vectorX, ups_vectorZ; +vector cams; +vector lookats; +vector ups; + +NurbsPath cam_nurbs_path; +char *path_file_name; +char *pose_file_name; + +/** Factor for saved image size */ +int factor = 1; + +/** + * program tries to have this framerate + */ +float idealfps = 20.0; +/** + * value of the listBox fo Color Value and Colormap + */ +int listboxColorVal = 0; +int listboxColorMapVal = 0; +int colorScanVal = 0; +ScanColorManager *cm; +float mincolor_value = 0.0; +float maxcolor_value = 0.0; +//unsigned int types = Point::USE_HEIGHT; +PointType pointtype; + +/** + * Contains the selected points for each scan + */ +set *selected_points; +/** + * Select single points? + */ +int select_voxels = 0; +/** + * Select or unselect points ? + */ +int selectOrunselect = 1; +/** octree depth for selecting groups of points */ +int selection_depth = 1; +int brush_size = 0; +char *selection_file_name; + +int current_frame = 0; +#include "show_menu.cc" +#include "show_animate.cc" +#include "show_gl.cc" + +/** + * Explains the usage of this program's command line parameters + * @param prog name of the program + */ +void usage(char* prog) +{ +#ifndef _MSC_VER + const string bold("\033[1m"); + const string normal("\033[m"); +#else + const string bold(""); + const string normal(""); +#endif + + cout << endl + << bold << "USAGE " << normal << endl + << " " << prog << " [options] directory" << endl << endl; + cout << bold << "OPTIONS" << normal << endl + + << bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl + << " end after scan NR" << endl + << endl + << bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl + << " using shared library F for input" << endl + << " (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply, wrl, xyz, zuf, iais, front, x3d, rxp, ais })" << endl + << endl + << bold << " -F" << normal << " NR, " << bold << "--fps=" << normal << "NR [default: 20]" << endl + << " will attempt to display points with a framerate of NR" << endl + << endl + << bold << " -l" << normal << " FILE, " << bold << "--loadObj=" << normal << + "FILE" << endl + << " load objects specified in " << endl + << endl + << endl + << bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl + << " neglegt all data points with a distance larger than NR 'units'" << endl + << endl + << bold << " -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl + << " neglegt all data points with a distance smaller than NR 'units'" << endl + << endl + << bold << " -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl + << " use randomized octree based point reduction (pts per voxel=)" << endl + << " requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl + << endl + << bold << " -o" << normal << " NR, " << bold << "--origin=" << normal << "NR (optional)" << endl + << " sets the starting and reset position to: " << endl + << " 0 = the origin of the coordinate system (default)" << endl + << " 1 = the position of the first scan (default if --origin is in argument list)" << endl + << " 2 = the center of the first scan" << endl + << endl + << bold << " -S, --scanserver" << normal << endl + << " Use the scanserver as an input method and handling of scan data" << endl + << endl + << bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl + << " turns on octree based point reduction (voxel size=)" << endl + << endl + << bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl + << " start at scan NR (i.e., neglects the first NR scans)" << endl + << " [ATTENTION: counting naturally starts with 0]" << endl + << endl + << bold << " -C" << normal << " NR, " << bold << "--scale=" << normal << "NR" << endl + << " scale factor to use (default: 0.01), modifies movement speed etc. " << endl + << " use 1 when point coordinates are in m, 0.01 when in cm and so forth. " << endl + << " " << endl + << endl + + << bold << " -R, --reflectance, --reflectivity" << normal << endl + << " use reflectivity values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -D, --temperature, --degree" << normal << endl + << " use temperature values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -a, --amplitude" << endl << normal + << " use amplitude values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -d, --deviation" << endl << normal + << " use amplitude values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -h, --height" << endl << normal + << " use y-values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -T, --type" << endl << normal + << " use type values for coloring point clouds" << endl + << " only works when using octree display" << endl + << bold << " -c, --color" << endl << normal + << " use color RGB values for coloring point clouds" << endl + << bold << " -b" << normal << " NR, " << bold << "--sphere=" << normal << "NR" << endl + << " map all measurements on a sphere (of radius NRcm)" << endl + << bold << " --saveOct" << endl << normal + << " stores all used scans as octrees in the given directory" << endl + << " All reflectivity/amplitude/deviation/type settings are stored as well." << endl + << " only works when using octree display" << endl + << bold << " --loadOct" << endl << normal + << " only reads octrees from the given directory" << endl + << " All reflectivity/amplitude/deviation/type settings are read from file." << endl + << " --reflectance/--amplitude and similar parameters are therefore ignored." << endl + << " only works when using octree display" << endl + << endl << endl; + + exit(1); +} + +/** + * A function that parses the command-line arguments and sets the respective flags. + * + * @param argc the number of arguments + * @param argv the arguments + * @param dir parsing result - the directory + * @param start parsing result - starting at scan number 'start' + * @param end parsing result - stopping at scan number 'end' + * @param maxDist parsing result - maximal distance + * @param minDist parsing result - minimal distance + * @param readInitial parsing result - read a file containing a initial transformation matrix + * @param type parsing result - file format to be read + * @return 0, if the parsing was successful, 1 otherwise + */ +int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxDist, int& minDist, + double &red, bool &readInitial, int &octree, PointType &ptype, float &fps, string &loadObj, + bool &loadOct, bool &saveOct, int &origin, double &scale, IOType &type, bool& scanserver, + double& sphereMode) +{ + unsigned int types = PointType::USE_NONE; + start = 0; + end = -1; // -1 indicates no limitation + maxDist = -1; // -1 indicates no limitation + int c; + // from unistd.h + extern char *optarg; + extern int optind; + + WriteOnce w_type(type); + WriteOnce w_start(start), w_end(end); + + cout << endl; + static struct option longopts[] = { + { "origin", optional_argument, 0, 'o' }, + { "format", required_argument, 0, 'f' }, + { "fps", required_argument, 0, 'F' }, + { "scale", required_argument, 0, 'C' }, + { "start", required_argument, 0, 's' }, + { "end", required_argument, 0, 'e' }, + { "reduce", required_argument, 0, 'r' }, + { "max", required_argument, 0, 'm' }, + { "min", required_argument, 0, 'M' }, + { "octree", optional_argument, 0, 'O' }, + { "reflectance", no_argument, 0, 'R' }, + { "reflectivity", no_argument, 0, 'R' }, + { "temperature", no_argument, 0, 'D' }, + { "degree", no_argument, 0, 'D' }, + { "amplitude", no_argument, 0, 'a' }, + { "deviation", no_argument, 0, 'd' }, + { "height", no_argument, 0, 'h' }, + { "type", no_argument, 0, 'T' }, + { "color", no_argument, 0, 'c' }, + { "loadObj", required_argument, 0, 'l' }, + { "saveOct", no_argument, 0, '0' }, + { "loadOct", no_argument, 0, '1' }, + { "advanced", no_argument, 0, '2' }, + { "scanserver", no_argument, 0, 'S' }, + { "sphere", required_argument, 0, 'b' }, + { 0, 0, 0, 0} // needed, cf. getopt.h + }; + + while ((c = getopt_long(argc, argv,"F:f:s:e:r:m:M:O:o:l:C:SwtRDadhTcb", longopts, NULL)) != -1) { + switch (c) { + case 's': + w_start = atoi(optarg); + if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } + break; + case 'e': + w_end = atoi(optarg); + if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } + if (end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } + break; + case 'm': + maxDist = atoi(optarg); + break; + case 'M': + minDist = atoi(optarg); + break; + case 'r': + red = atof(optarg); + break; + case 't': + readInitial = true; + break; + case 'O': + if (optarg) { + octree = atoi(optarg); + } else { + octree = 1; + } + break; + case 'f': + try { + w_type = formatname_to_io_type(optarg); + } catch (...) { // runtime_error + cerr << "Format " << optarg << " unknown." << endl; + abort(); + } + break; + case '?': + usage(argv[0]); + return 1; + case 'R': + types |= PointType::USE_REFLECTANCE; + break; + case 'D': + types |= PointType::USE_TEMPERATURE; + break; + case 'a': + types |= PointType::USE_AMPLITUDE; + break; + case 'd': + types |= PointType::USE_DEVIATION; + break; + case 'h': + types |= PointType::USE_HEIGHT; + break; + case 'T': + types |= PointType::USE_TYPE; + break; + case 'c': + types |= PointType::USE_COLOR; + break; + case 'F': + fps = atof(optarg); + break; + case 'C': + scale = atof(optarg); + break; + case 'S': + scanserver = true; + break; + case 'o': + if (optarg) { + origin = atoi(optarg); + } else { + origin = 1; + } + break; + case '0': + saveOct = true; + break; + case '1': + loadOct = true; + break; + case 'l': + loadObj = optarg; + break; + case '2': + advanced_controls = true; + break; + case 'b': + sphereMode = atof(optarg); + break; + default: + abort (); + } + } + + if (optind != argc-1) { + cerr << "\n*** Directory missing ***" << endl; + usage(argv[0]); + } + dir = argv[optind]; + +#ifndef _MSC_VER + if (dir[dir.length()-1] != '/') dir = dir + "/"; +#else + if (dir[dir.length()-1] != '\\') dir = dir + "\\"; +#endif + + parseFormatFile(dir, w_type, w_start, w_end); + + ptype = PointType(types); + return 0; +} + +void setResetView(int origin) { + if (origin == 1) { + // set origin to the pose of the first scan + double *transmat = MetaMatrix[0].back(); + cout << transmat << endl; + + RVX = -transmat[12]; + RVY = -transmat[13]; + RVZ = -transmat[14]; + Matrix4ToQuat(transmat, Rquat); + X = RVX; + Y = RVY; + Z = RVZ; + quat[0] = Rquat[0]; + quat[1] = Rquat[1]; + quat[2] = Rquat[2]; + quat[3] = Rquat[3]; + } else if (origin == 2) { + // set origin to the center of the first octree + double center[3], center_transformed[3]; +#ifdef USE_COMPACT_TREE + ((compactTree*)octpts[0])->getCenter(center); +#else + ((Show_BOctTree*)octpts[0])->getCenter(center); +#endif + VTrans(MetaMatrix[0].back(), center, center_transformed); + RVX = -center_transformed[0]; + RVY = -center_transformed[1]; + RVZ = -center_transformed[2]; + X = RVX; + Y = RVY; + Z = RVZ; + } else if (origin == 3) { + // set origin to the center of mass of all scans + for (size_t i = 0; i < octpts.size(); ++i) { + vector points; +#ifndef USE_COMPACT_TREE + BOctTree* cur_tree = ((Show_BOctTree*)octpts[i])->getTree(); + cur_tree->AllPoints( points ); +#endif + + cout << "Scan " << i << " size: " << points.size() << endl; + double centroid[3] = {0., 0., 0.}; + double centroid_transformed[3];; + for (size_t j = 0; j < points.size(); ++j) { + for (unsigned int k = 0; k < 3; ++k) + centroid[k] += points[j][k]; + } + for (unsigned int k = 0; k < 3; ++k) { + centroid[k] /= (double)points.size(); + } + VTrans(MetaMatrix[i].back(), centroid, centroid_transformed); + for (unsigned int k = 0; k < 3; ++k) { + CoM[k] += centroid_transformed[k]; + } + } + for (unsigned int k = 0; k < 3; ++k) + CoM[k] /= octpts.size() * 1.; + + cout << "Center of Mass at: " << CoM[0] << ", " << CoM[1] << ", " << CoM[2] << endl; + + RVX = -CoM[0]; + RVY = -CoM[1]; + RVZ = -CoM[2]; + X = RVX; + Y = RVY; + Z = RVZ; + } +} + +/* + * A function that read the .frame files created by slam6D + * + * @param dir the directory + * @param start starting at scan number 'start' + * @param end stopping at scan number 'end' + * @param read a file containing a initial transformation matrix and apply it + */ +int readFrames(string dir, int start, int end, bool readInitial, IOType &type) +{ + + // convert to OpenGL coordinate system + double mirror[16]; + M4identity(mirror); + mirror[10] = -1.0; + + double initialTransform[16]; + if (readInitial) { + cout << "Initial Transform:" << endl; + string initialTransformFileName = dir + "initital.frame"; + ifstream initial_in(initialTransformFileName.c_str()); + if (!initial_in.good()) { + cout << "Error opening " << initialTransformFileName << endl; + exit(-1); + } + initial_in >> initialTransform; + cout << initialTransform << endl; + + // update the mirror to apply the initial frame for all frames + double tempxf[16]; + MMult(mirror, initialTransform, tempxf); + memcpy(mirror, tempxf, sizeof(tempxf)); + } + + for(std::vector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + const double* transformation; + Scan::AlgoType algoType; + vector Matrices; + vector algoTypes; + + // iterate over frames (stop if none were created) and pull/convert the frames into local containers + unsigned int frame_count; + try { + frame_count = (*it)->readFrames(); + } catch(std::ios_base::failure& e) { + break; + } + for(unsigned int i = 0; i < frame_count; ++i) { + (*it)->getFrame(i, transformation, algoType); + double* transMatOpenGL = new double[16]; + + // apply mirror to convert (and initial frame if requested) the frame and save in opengl + MMult(mirror, transformation, transMatOpenGL); + + Matrices.push_back(transMatOpenGL); + algoTypes.push_back(algoType); + } + + MetaAlgoType.push_back(algoTypes); + MetaMatrix.push_back(Matrices); + + if((type == UOS_MAP || type == UOS_MAP_FRAMES || type == RTS_MAP) && it == Scan::allScans.begin()) { + MetaAlgoType.push_back(algoTypes); + MetaMatrix.push_back(Matrices); + } + current_frame = MetaMatrix.back().size() - 1; + } + + if (MetaMatrix.size() == 0) { + cerr << "*****************************************" << endl; + cerr << "** ERROR: No .frames could be found! **" << endl; + cerr << "*****************************************" << endl; + cerr << " ERROR: Missing or empty directory: " << dir << endl << endl; + return -1; + } + return 0; +} + +void generateFrames(int start, int end, bool identity) { + if (identity) { + cout << "using Identity for frames " << endl; + } else { + cout << "using pose information for frames " << endl; + } + int fileCounter = start; + int index = 0; + for (;;) { + if (fileCounter > end) break; // 'nuf read + fileCounter++; + + vector Matrices; + vector algoTypes; + + for (int i = 0; i < 3; i++) { + double *transMat = new double[16]; + + if (identity) { + M4identity(transMat); + transMat[10] = -1.0; + } else { + EulerToMatrix4(Scan::allScans[index]->get_rPos(), Scan::allScans[index]->get_rPosTheta(), transMat ); + } + + Matrices.push_back(transMat); + algoTypes.push_back(Scan::ICP); + + } + index++; + MetaAlgoType.push_back(algoTypes); + MetaMatrix.push_back(Matrices); + current_frame = MetaMatrix.back().size() - 1; + } +} + +void cycleLOD() { + LevelOfDetail = 0.00001; + for (unsigned int i = 0; i < octpts.size(); i++) + octpts[i]->cycleLOD(); +} + + +void initShow(int argc, char **argv){ + + /***************/ + /* init OpenGL */ + /***************/ + glutInit(&argc,argv); + + cout << "(wx)show - A highly efficient 3D point cloud viewer" << endl + << "(c) Jacobs University Bremen gGmbH, Germany, since 2009" << endl + << " University of Osnabrueck, Germany, 2006 - 2009" << endl << endl; + + if(argc <= 1){ + usage(argv[0]); + } + + double red = -1.0; + int start = 0, end = -1, maxDist = -1, minDist = -1; + string dir; + bool readInitial = false; + IOType type = UOS; + int octree = 0; + bool loadOct = false; + bool saveOct = false; + string loadObj; + int origin = 0; + double scale = 0.01; // in m + bool scanserver = false; + double sphereMode = 0.0; + + pose_file_name = new char[1024]; + path_file_name = new char[1024]; + selection_file_name = new char[1024]; + + strncpy(pose_file_name, "pose.dat", 1024); + strncpy(path_file_name, "path.dat", 1024); + strncpy(selection_file_name, "selected.3d", 1024); + + parseArgs(argc, argv, dir, start, end, maxDist, minDist, red, readInitial, + octree, pointtype, idealfps, loadObj, loadOct, saveOct, origin, scale, + type, scanserver, sphereMode); + + // modify all scale dependant variables + scale = 1.0 / scale; + movementSpeed *= scale; + neardistance *= scale; + oldneardistance *= scale; + maxfardistance *= scale; + fardistance *= scale; + fogDensity /= scale; + defaultZoom *= scale; + voxelSize *= scale; +// oldfardistance *= scale; + + //////////////////////// + SDisplay::readDisplays(loadObj, displays); + //////////////////// + + if (type == OCT) { + loadOct = true; + } + + // if we want to load display file get pointtypes from the files first + if(loadOct) { + string scanFileName = dir + "scan" + to_string(start,3) + ".oct"; + cout << "Getting point information from " << scanFileName << endl; + cout << "Attention! All subsequent oct-files must be of the same type!" << endl; + } + scan_dir = dir; + + // init and create display + M4identity(view_rotate_button); + obj_pos_button[0] = obj_pos_button[1] = obj_pos_button[2] = 0.0; + + // Loading scans, reducing, loading frames and generation if neccessary + + // load all available scans + Scan::openDirectory(scanserver, dir, type, start, end); + + if(Scan::allScans.size() == 0) { + cerr << "No scans found. Did you use the correct format?" << endl; + exit(-1); + } + + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + scan->setRangeFilter(maxDist, minDist); + if (sphereMode > 0.0) scan->setRangeMutation(sphereMode); + if (red > 0) { + // scanserver differentiates between reduced for slam and reduced for show, can handle both at the same time + if(scanserver) { + dynamic_cast(scan)->setShowReductionParameter(red, octree); + } else { + scan->setReductionParameter(red, octree); + } + } + } + if (sphereMode > 0.0) { + cm = new ScanColorManager(4096, pointtype, /* animation_color = */ false); + } else { + cm = new ScanColorManager(4096, pointtype, /* animation_color = */ true); + } + +#ifdef USE_COMPACT_TREE + cout << "Creating compact display octrees.." << endl; +#else + cout << "Creating display octrees.." << endl; +#endif + + if (loadOct) cout << "Loading octtrees from file where possible instead of creating them from scans." << endl; + + // for managed scans the input phase needs to know how much it can handle + std::size_t free_mem = 0; + if(scanserver) + free_mem = ManagedScan::getMemorySize(); + + for(unsigned int i = 0; i < Scan::allScans.size(); ++i) { + Scan* scan = Scan::allScans[i]; + + // create data structures +#ifdef USE_COMPACT_TREE // FIXME: change compact tree, then this case can be removed + compactTree* tree; + try { + if (loadOct) { + string sfName = dir + "scan" + to_string(i,3) + ".oct"; + cout << "Load " << sfName; + tree = new compactTree(sfName, cm); + cout << " done." << endl; + } else { + if (red > 0) { // with reduction, only xyz points + DataXYZ xyz_r(scan->get("xyz reduced show")); + tree = new compactTree(PointerArray(xyz_r).get(), xyz_r.size(), voxelSize, pointtype, cm); + } else { // without reduction, xyz + attribute points + sfloat** pts = pointtype.createPointArray(scan); + unsigned int nrpts = scan->size("xyz"); + tree = new compactTree(pts, nrpts, voxelSize, pointtype, cm); + for(unsigned int i = 0; i < nrpts; ++i) delete[] pts[i]; + delete[] pts; + if (saveOct) { + string sfName = dir + "scan" + to_string(i,3) + ".oct"; + tree->serialize(sfName); + } + } + } + } catch(...) { + cout << "Scan " << i << " could not be loaded into memory, stopping here." << endl; + break; + } +#else // FIXME: remove the case above + scan->setOcttreeParameter(red, voxelSize, pointtype, loadOct, saveOct); + + DataOcttree* data_oct; + try { + data_oct = new DataOcttree(scan->get("octtree")); + } catch(runtime_error& e) { + cout << "Scan " << i << " could not be loaded into memory, stopping here." << endl; + break; + } + BOctTree* btree = &(data_oct->get()); + unsigned int tree_size = btree->getMemorySize(); + + if(scanserver) { + // check if the octtree would actually fit with all the others + if(tree_size > free_mem) { + delete data_oct; + cout << "Stopping at scan " << i << ", no more octtrees could fit in memory." << endl; + break; + } else { + // subtract available memory + free_mem -= tree_size; + } + } +#endif //FIXME: COMPACT_TREE + +#if !defined USE_COMPACT_TREE + // show structures + // associate show octtree with the scan and hand over octtree pointer ownership + + Show_BOctTree* tree = new Show_BOctTree(scan, data_oct, cm); + + // unlock cached octtree to enable creation of more octtres without blocking the space for full scan points + tree->unlockCachedTree(); +#endif + + // octtrees have been created successfully + octpts.push_back(tree); + + // print something +#ifdef USE_COMPACT_TREE // TODO: change compact tree for memory footprint output, remove this case + cout << "Scan " << i << " octree finished." << endl; +#else + cout << "Scan " << i << " octree finished ("; + bool space = false; + if(tree_size/1024/1024 > 0) { + cout << tree_size/1024/1024 << "M"; + space = true; + } + if((tree_size/1024)%1024 > 0) { + if(space) cout << " "; + cout << (tree_size/1024)%1024 << "K"; + space = true; + } + if(tree_size%1024 > 0) { + if(space) cout << " "; + cout << tree_size%1024 << "B"; + } + cout << ")." << endl; +#endif + } + +/* +TODO: to maximize space for octtrees, implement a heuristic to remove all +CacheObjects sequentially from the start and pack octtrees one after another +until it reaches the maximum size allowed, resetting the index, but causing the +first to be locked again and stopping by catching the exception +set heuristic, do locking, catch exception, reset heuristic to default or old +*/ +#if !defined USE_COMPACT_TREE + if(scanserver) { + // activate all octtrees until they don't fit anymore + cout << "Locking managed octtrees in memory " << flush; + bool stop = false; + unsigned int loaded = 0; + unsigned int dots = (octpts.size() / 20); + if(dots == 0) dots = 1; + vector::iterator it_remove_first = octpts.end(); + for(vector::iterator it = octpts.begin(); it != octpts.end(); ++it) { + if(!stop) { + // try to lock the octtree in cache + try { + Show_BOctTree* stree = dynamic_cast*>(*it); + stree->lockCachedTree(); + loaded++; + if(loaded % dots == 0) cout << '.' << flush; + } catch(runtime_error& e) { + stop = true; + it_remove_first = it; + } + } + if(stop) { + // delete the octtree, resize after iteration + delete *it; + } + } + // now remove iterators for deleted octtrees + if(stop) octpts.erase(it_remove_first, octpts.end()); + cout << ' ' << loaded << " octtrees loaded." << endl; + } + +#endif // !COMPACT_TREE + + + // load frames now that we know how many scans we actually loaded + unsigned int real_end = min((unsigned int)(end), + (unsigned int)(start + octpts.size() - 1)); + if(readFrames(dir, start, real_end, readInitial, type)) + generateFrames(start, real_end, true); + + cm->setCurrentType(PointType::USE_HEIGHT); + //ColorMap cmap; + //cm->setColorMap(cmap); + resetMinMax(0); + + selected_points = new set[octpts.size()]; + + // sets (and computes if necessary) the pose that is used for the reset button + setResetView(origin); + + for (unsigned int i = 0; i < 256; i++) { + keymap[i] = false; + } +} + +void deinitShow() +{ + static volatile bool done = false; + if(done) return; + done = true; + + cout << "Cleaning up octtrees and scans." << endl; + if(octpts.size()) { + // delete octtrees to release the cache locks within + for(vector::iterator it = octpts.begin(); it!= octpts.end(); ++it) { + delete *it; + } + } + + Scan::closeDirectory(); +} + +/** + * Global program scope destructor workaround to clean up data regardless of + * the way of program exit. + */ +struct Deinit { ~Deinit() { deinitShow(); } } deinit; diff --git a/.svn/pristine/6e/6ee9fe28738858d3b9331e25d7914925c340b330.svn-base b/.svn/pristine/6e/6ee9fe28738858d3b9331e25d7914925c340b330.svn-base new file mode 100644 index 0000000..0ef4583 --- /dev/null +++ b/.svn/pristine/6e/6ee9fe28738858d3b9331e25d7914925c340b330.svn-base @@ -0,0 +1,13 @@ +IF(WITH_SEGMENTATION) +IF(OpenCV_VERSION_MAJOR GREATER 1 AND OpenCV_VERSION_MINOR GREATER 2) + add_executable(scan2segments scan2segments.cc ../slam6d/fbr/fbr_global.cc) + target_link_libraries(scan2segments scan ANN fbr_cv_io fbr_panorama fbr_feature fbr_feature_matcher fbr_registration ${Boost_LIBRARIES} ${OpenCV_LIBS}) +ELSE(OpenCV_VERSION_MAJOR GREATER 1 AND OpenCV_VERSION_MINOR GREATER 2) + MESSAGE("OpenCV Version > 2.2 required for scan2segmentation") +ENDIF(OpenCV_VERSION_MAJOR GREATER 1 AND OpenCV_VERSION_MINOR GREATER 2) + add_executable(fhsegmentation fhsegmentation.cc FHGraph.cc disjoint-set.cc segment-graph.cc) + + target_link_libraries(fhsegmentation scan ANN ${Boost_LIBRARIES} ${OpenCV_LIBS}) + + +ENDIF(WITH_SEGMENTATION) diff --git a/.svn/pristine/78/78a71ec4ee8c1c126617fb7272fc790d76de6aae.svn-base b/.svn/pristine/78/78a71ec4ee8c1c126617fb7272fc790d76de6aae.svn-base new file mode 100644 index 0000000..7577d9c --- /dev/null +++ b/.svn/pristine/78/78a71ec4ee8c1c126617fb7272fc790d76de6aae.svn-base @@ -0,0 +1,129 @@ +/* + * kd implementation + * + * Copyright (C) Andreas Nuechter, Kai Lingemann, Thomas Escher + * + * Released under the GPL version 3. + * + */ + +/** @file + * @brief An optimized k-d tree implementation + * @author Remus Dumitru. Jacobs University Bremen, Germany + * @author Corneliu-Claudiu Prodescu. Jacobs University Bremen, Germany + * @author Andreas Nuechter. Jacobs University Bremen, Germany. + * @author Kai Lingemann. Inst. of CS, University of Osnabrueck, Germany. + * @author Thomas Escher Inst. of CS, University of Osnabrueck, Germany. + */ + +#ifdef _MSC_VER +#define _USE_MATH_DEFINES +#endif + +#include "slam6d/kd.h" +#include "slam6d/globals.icc" + +#include +using std::cout; +using std::cerr; +using std::endl; +#include +using std::swap; +#include +#include +#include +#include + +// KDtree class static variables +template +KDParams KDTreeImpl::params[MAX_OPENMP_NUM_THREADS]; + +/** + * Constructor + * + * Create a KD tree from the points pointed to by the array pts + * + * @param pts 3D array of points + * @param n number of points + */ +KDtree::KDtree(double **pts, int n) +{ + create(Void(), pts, n); +} + +KDtree::~KDtree() +{ +} + +/** + * Finds the closest point within the tree, + * wrt. the point given as first parameter. + * @param _p point + * @param maxdist2 maximal search distance. + * @param threadNum Thread number, for parallelization + * @return Pointer to the closest point + */ +double *KDtree::FindClosest(double *_p, + double maxdist2, + int threadNum) const +{ + params[threadNum].closest = 0; + params[threadNum].closest_d2 = maxdist2; + params[threadNum].p = _p; + _FindClosest(Void(), threadNum); + return params[threadNum].closest; +} + +double *KDtree::FindClosestAlongDir(double *_p, + double *_dir, + double maxdist2, + int threadNum) const +{ + params[threadNum].closest = NULL; + params[threadNum].closest_d2 = maxdist2; + params[threadNum].p = _p; + params[threadNum].dir = _dir; + _FindClosestAlongDir(Void(), threadNum); + return params[threadNum].closest; +} + +vector KDtree::kNearestNeighbors(double *_p, + int k, + double sqRad2, + int threadNum) const +{ + vector result; + params[threadNum].closest = 0; + params[threadNum].closest_d2 = sqRad2; + params[threadNum].p = _p; + params[threadNum].heap.clear(); + _KNNSearch(Void(), threadNum); + + while (k > 0 && params[threadNum].heap.empty() == false) { + Point pt = params[threadNum].heap.front().first; + result.push_back(pt); + std::pop_heap(params[threadNum].heap.begin(), params[threadNum].heap.end(), PointCompare()); + params[threadNum].heap.pop_back(); + k--; + } + + return result; +} + +vector KDtree::fixedRangeSearch(double *_p, + double sqRad2, + int threadNum) const +{ + vector result; + params[threadNum].closest = 0; + params[threadNum].closest_d2 = sqRad2; + params[threadNum].p = _p; + params[threadNum].heap.clear(); + _FixedRangeSearch(Void(), threadNum); + + for (vector >::iterator it = params[threadNum].heap.begin(); it != params[threadNum].heap.end(); ++it) { + result.push_back(it->first); + } + + return result; +} diff --git a/.svn/pristine/88/88689f7360d62e6f8cd63f8d04565e8f5bc0841c.svn-base b/.svn/pristine/88/88689f7360d62e6f8cd63f8d04565e8f5bc0841c.svn-base new file mode 100644 index 0000000..1184149 --- /dev/null +++ b/.svn/pristine/88/88689f7360d62e6f8cd63f8d04565e8f5bc0841c.svn-base @@ -0,0 +1,35 @@ +1. +--metascan segfaults when destroying the allscanlist. + +2. +scanserver segfaults whith reflectances sometimes. +E.g., + +bin/scanserver -c 3500 +and +bin/show -s 0 -e 1 -f riegl_txt --reflectance ~/dat/bremen_city --scanserver + +or +bin/scanserver +and +bin/slam6D -s 0 -e 1 -f uosr dat --scanserver + +3. +scan_red with panorama range image and cylindrical coordinates does +not work correctly. + +4. +fast_normals does not work on SRI yet. Commented out. + +5. +reflectance_reduced not in managedScan/scanserver + +6. +normals not integrated managedScan/scanserver + +7. +kdMeta/kdManaged does not support kNN nor range search yet + +8. +Make our own knn efficient, then get rid of ANN ;-) + diff --git a/.svn/pristine/91/91cd31e1704e7bc495aa3c6879088dcbbf71c484.svn-base b/.svn/pristine/91/91cd31e1704e7bc495aa3c6879088dcbbf71c484.svn-base new file mode 100644 index 0000000..ebb4bae --- /dev/null +++ b/.svn/pristine/91/91cd31e1704e7bc495aa3c6879088dcbbf71c484.svn-base @@ -0,0 +1,7 @@ +add_library(normals normals.cc) +target_link_libraries(normals newmat scan ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS}) + +IF(WITH_TOOLS) + add_executable(calc_normals calc_normals.cc) + target_link_libraries(calc_normals normals ${Boost_LIBRARIES}) +ENDIF(WITH_TOOLS) diff --git a/.svn/pristine/92/92778e36b9fcba7dbe7d7b8a1c555930862af850.svn-base b/.svn/pristine/92/92778e36b9fcba7dbe7d7b8a1c555930862af850.svn-base new file mode 100644 index 0000000..9f41007 --- /dev/null +++ b/.svn/pristine/92/92778e36b9fcba7dbe7d7b8a1c555930862af850.svn-base @@ -0,0 +1,37 @@ +SET(FBR_IO_SRC scan_cv.cc) +add_library(fbr_cv_io STATIC ${FBR_IO_SRC}) + +SET(FBR_PANORAMA_SRC panorama.cc) +add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC} fbr_global.cc) + +IF(WITH_FBR) +IF(OpenCV_VERSION_MAJOR GREATER 1 AND OpenCV_VERSION_MINOR GREATER 2) +SET(FBR_FEATURE_SRC feature.cc) +add_library(fbr_feature STATIC ${FBR_FEATURE_SRC}) + +SET(FBR_FEATURE_MATCHER_SRC feature_matcher.cc) +add_library(fbr_feature_matcher STATIC ${FBR_FEATURE_MATCHER_SRC}) + +SET(FBR_REGISTRATION_SRC registration.cc) +add_library(fbr_registration STATIC ${FBR_REGISTRATION_SRC}) + +SET(FBR_SRC scan_cv.cc panorama.cc feature.cc feature_matcher.cc registration.cc fbr_global.cc) +add_library(fbr STATIC ${FBR_SRC}) + +SET(FBR_LIBS scan ANN ${OpenCV_LIBS}) + +add_executable(featurebasedregistration feature_based_registration.cc fbr_global.cc) +#target_link_libraries(featurebasedregistration fbr_cv_io fbr_panorama fbr_feature fbr_feature_matcher fbr_registration ${FBR_LIBS}) +target_link_libraries(featurebasedregistration fbr ${FBR_LIBS}) + +### EXPORT SHARED LIBS + +IF(EXPORT_SHARED_LIBS) +add_library(fbr_s SHARED ${FBR_SRC}) +target_link_libraries(fbr_s scan_s ANN_s ${OpenCV_LIBS}) +ENDIF(EXPORT_SHARED_LIBS) + +ELSE(OpenCV_VERSION_MAJOR GREATER 1 AND OpenCV_VERSION_MINOR GREATER 2) + MESSAGE("OpenCV Version > 2.2 required for FBR") +ENDIF(OpenCV_VERSION_MAJOR GREATER 1 AND OpenCV_VERSION_MINOR GREATER 2) +ENDIF(WITH_FBR) diff --git a/.svn/pristine/92/92ef8a9f69a7dcb1d3eb01b3259fea032402080f.svn-base b/.svn/pristine/92/92ef8a9f69a7dcb1d3eb01b3259fea032402080f.svn-base new file mode 100644 index 0000000..b8c3ce0 --- /dev/null +++ b/.svn/pristine/92/92ef8a9f69a7dcb1d3eb01b3259fea032402080f.svn-base @@ -0,0 +1,155 @@ +#ifndef NORMALS_H +#define NORMALS_H + +#include +#include +#if (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#include +#else +#include +#endif + +void calculateNormalsApxKNN(std::vector &normals, + vector &points, + int k, + const double _rPos[3], + double eps = 0.0); + +void calculateNormalsAdaptiveApxKNN(std::vector &normals, + vector &points, + int kmin, + int kmax, + const double _rPos[3], + double eps = 0.0); + +void calculateNormalsKNN(std::vector &normals, + vector &points, + int k, + const double _rPos[3] ); + + +void calculateNormalsAdaptiveKNN(vector &normals, + vector &points, + int kmin, int kmax, + const double _rPos[3]); + +void calculateNormalsPANORAMA(vector &normals, + vector &points, + vector< vector< vector< cv::Vec3f > > > extendedMap, + const double _rPos[3]); + +// TODO should be exported to separate library +/* + * retrieve a cv::Mat with x,y,z,r from a scan object + * functionality borrowed from scan_cv::convertScanToMat but this function + * does not allow a scanserver to be used, prints to stdout and can only + * handle a single scan + */ +static inline cv::Mat scan2mat(Scan *source) +{ + DataXYZ xyz = source->get("xyz"); + + DataReflectance xyz_reflectance = source->get("reflectance"); + + unsigned int nPoints = xyz.size(); + cv::Mat scan(nPoints,1,CV_32FC(4)); + scan = cv::Scalar::all(0); + + cv::MatIterator_ it; + + it = scan.begin(); + for(unsigned int i = 0; i < nPoints; i++){ + float x, y, z, reflectance; + x = xyz[i][0]; + y = xyz[i][1]; + z = xyz[i][2]; + if(xyz_reflectance.size() != 0) + { + reflectance = xyz_reflectance[i]; + + //normalize the reflectance + reflectance += 32; + reflectance /= 64; + reflectance -= 0.2; + reflectance /= 0.3; + if (reflectance < 0) reflectance = 0; + if (reflectance > 1) reflectance = 1; + } + + (*it)[0] = x; + (*it)[1] = y; + (*it)[2] = z; + if(xyz_reflectance.size() != 0) + (*it)[3] = reflectance; + else + (*it)[3] = 0; + + ++it; + } + return scan; +} +// TODO should be exported to separate library +/* + * convert a matrix of float values (range image) to a matrix of unsigned + * eight bit characters using different techniques + */ +static inline cv::Mat float2uchar(cv::Mat &source, bool logarithm, float cutoff) +{ + cv::Mat result(source.size(), CV_8U, cv::Scalar::all(0)); + float max = 0; + // find maximum value + if (cutoff == 0.0) { + // without cutoff, just iterate through all values to find the largest + for (cv::MatIterator_ it = source.begin(); + it != source.end(); ++it) { + float val = *it; + if (val > max) { + max = val; + } + } + } else { + // when a cutoff is specified, sort all the points by value and then + // specify the max so that values are larger than it + vector sorted(source.cols*source.rows); + int i = 0; + for (cv::MatIterator_ it = source.begin(); + it != source.end(); ++it, ++i) { + sorted[i] = *it; + } + std::sort(sorted.begin(), sorted.end()); + max = sorted[(int)(source.cols*source.rows*(1.0-cutoff))]; + cout << "A cutoff of " << cutoff << " resulted in a max value of " << max << endl; + } + + cv::MatIterator_ src = source.begin(); + cv::MatIterator_ dst = result.begin(); + cv::MatIterator_ end = source.end(); + if (logarithm) { + // stretch values from 0 to max logarithmically over 0 to 255 + // using the logarithm allows to represent smaller values with more + // precision and larger values with less + max = log(max+1); + for (; src != end; ++src, ++dst) { + float val = (log(*src+1)*255.0)/max; + if (val > 255) + *dst = 255; + else + *dst = (uchar)val; + } + } else { + // stretch values from 0 to max linearly over 0 to 255 + for (; src != end; ++src, ++dst) { + float val = (*src*255.0)/max; + if (val > 255) + *dst = 255; + else + *dst = (uchar)val; + } + } + return result; +} + + + +#endif // NORMALS_H diff --git a/.svn/pristine/97/97bbbc7740c95b88c9588a5244f8156b4c4d63fd.svn-base b/.svn/pristine/97/97bbbc7740c95b88c9588a5244f8156b4c4d63fd.svn-base new file mode 100644 index 0000000..e8d1e58 --- /dev/null +++ b/.svn/pristine/97/97bbbc7740c95b88c9588a5244f8156b4c4d63fd.svn-base @@ -0,0 +1,194 @@ +/* + * scan_io_xyzr implementation + * + * Copyright (C) Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + + +/** + * @file scan_io_xyzr.cc + * @brief IO of a 3D scan in xyz file format plus a reflectance/intensity + * @author Andreas Nuechter. Jacobs University Bremen, Germany. + */ + +#include "scanio/scan_io_xyzr.h" + +#include +using std::cout; +using std::cerr; +using std::endl; +#include + +#ifdef _MSC_VER +#include +#endif + +#include +#include +using namespace boost::filesystem; + +#include "slam6d/globals.icc" + +#define DATA_PATH_PREFIX "scan" +#define DATA_PATH_SUFFIX ".3d" +#define POSE_PATH_PREFIX "scan" +#define POSE_PATH_SUFFIX ".pose" + +std::list ScanIO_xyzr::readDirectory(const char* dir_path, + unsigned int start, + unsigned int end) +{ + std::list identifiers; + for(unsigned int i = start; i <= end; ++i) { + // identifier is /d/d/d (000-999) + std::string identifier(to_string(i,3)); + // scan consists of data (.3d) and pose (.pose) files + path data(dir_path); + data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX); + path pose(dir_path); + pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX); + // stop if part of a scan is missing or end by absence is detected + if(!exists(data) || !exists(pose)) + break; + identifiers.push_back(identifier); + } + return identifiers; +} + +void ScanIO_xyzr::readPose(const char* dir_path, + const char* identifier, + double* pose) +{ + unsigned int i; + + path pose_path(dir_path); + pose_path /= path(std::string(POSE_PATH_PREFIX) + + identifier + + POSE_PATH_SUFFIX); + if(!exists(pose_path)) + throw std::runtime_error(std::string("There is no pose file for [") + + identifier + "] in [" + dir_path + "]"); + + // open pose file + ifstream pose_file(pose_path); + + // if the file is open, read contents + if(pose_file.good()) { + // read 6 plain doubles + for(i = 0; i < 6; ++i) pose_file >> pose[i]; + pose_file.close(); + + // convert angles from deg to rad + for(i = 3; i < 6; ++i) pose[i] = rad(pose[i]); + } else { + throw std::runtime_error(std::string("Pose file could not be opened for [") + + identifier + "] in [" + + dir_path + "]"); + } +} + +bool ScanIO_xyzr::supports(IODataType type) +{ + return !!(type & ( DATA_REFLECTANCE | DATA_XYZ )); +} + +void ScanIO_xyzr::readScan(const char* dir_path, + const char* identifier, + PointFilter& filter, + std::vector* xyz, + std::vector* rgb, + std::vector* reflectance, + std::vector* temperature, + std::vector* amplitude, + std::vector* type, + std::vector* deviation) +{ + unsigned int i; + + // error handling + path data_path(dir_path); + data_path /= path(std::string(DATA_PATH_PREFIX) + + identifier + + DATA_PATH_SUFFIX); + if(!exists(data_path)) + throw std::runtime_error(std::string("There is no scan file for [") + + identifier + "] in [" + + dir_path + "]"); + + if(xyz != 0) { + // open data file + ifstream data_file(data_path); + data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit); + + // overread the first line ignoring the header information + char dummy[255]; + data_file.getline(dummy, 255); + + // read points and reflectance/intensity/temperature value + double point[3]; + float reflection; + while(data_file.good()) { + try { + for(i = 0; i < 3; ++i) data_file >> point[i]; +/* + point[0] -= 485531.0; + point[1] -= 5882078.400; + point[2] -= 52; +*/ + std::swap(point[2], point[1]); + data_file >> reflection; + } catch(std::ios_base::failure& e) { + break; + } + + // apply filter then insert point and reflectance + if(filter.check(point)) { + for(i = 0; i < 3; ++i) xyz->push_back(point[i]); + reflectance->push_back(reflection); + } + } + data_file.close(); + } +} + + + +/** + * class factory for object construction + * + * @return Pointer to new object + */ +#ifdef _MSC_VER +extern "C" __declspec(dllexport) ScanIO* create() +#else +extern "C" ScanIO* create() +#endif +{ + return new ScanIO_xyzr; +} + + +/** + * class factory for object construction + * + * @return Pointer to new object + */ +#ifdef _MSC_VER +extern "C" __declspec(dllexport) void destroy(ScanIO *sio) +#else +extern "C" void destroy(ScanIO *sio) +#endif +{ + delete sio; +} + +#ifdef _MSC_VER +BOOL APIENTRY DllMain(HANDLE hModule, DWORD dwReason, LPVOID lpReserved) +{ + return TRUE; +} +#endif + diff --git a/.svn/pristine/97/97df0014d39a4e57120a8ee05465615fa460a92f.svn-base b/.svn/pristine/97/97df0014d39a4e57120a8ee05465615fa460a92f.svn-base new file mode 100644 index 0000000..bd48c5a --- /dev/null +++ b/.svn/pristine/97/97df0014d39a4e57120a8ee05465615fa460a92f.svn-base @@ -0,0 +1,48 @@ +if(WIN32) + add_library(pointfilter STATIC ../slam6d/pointfilter.cc) +else(WIN32) + add_library(pointfilter SHARED ../slam6d/pointfilter.cc) +endif(WIN32) + +set(SCANIO_LIBNAMES + uos uosr uos_rgb uos_rrgbt xyzr ply ks ks_rgb riegl_txt riegl_rgb rts velodyne +) + +if(WITH_RIVLIB) + set(SCANIO_LIBNAMES ${SCANIO_LIBNAMES} rxp) + if(LIBXML2_FOUND) + include_directories(${LIBXML2_INCLUDE_DIR}) +# set(SCANIO_LIBNAMES ${SCANIO_LIBNAMES} riegl_project) +# target_link_libraries(scan_io_riegl_project ${RIVLIB} scan_io_rxp ${LIBXML2_LIBRARIES}) + endif(LIBXML2_FOUND) +endif(WITH_RIVLIB) + + +foreach(libname ${SCANIO_LIBNAMES}) +if(WIN32) + #add_library(scan_io_${libname} STATIC scan_io_${libname}.cc) + add_library(scan_io_${libname} SHARED scan_io_${libname}.cc) +else(WIN32) + add_library(scan_io_${libname} SHARED scan_io_${libname}.cc) +endif(WIN32) + target_link_libraries(scan_io_${libname} pointfilter ${Boost_LIBRARIES} ${Boost_PROGRAM_OPTIONS_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY}) +endforeach(libname) + +if(WITH_RIVLIB) + target_link_libraries(scan_io_rxp ${RIVLIB}) + if(LIBXML2_FOUND) + target_link_libraries(scan_io_rxp ${LIBXML2_LIBRARIES}) #scan_io_riegl_project ${RIVLIB}) + endif(LIBXML2_FOUND) +endif(WITH_RIVLIB) + +if(WIN32) + add_library(scanio STATIC scan_io.cc ../slam6d/io_types.cc) +else(WIN32) + add_library(scanio SHARED scan_io.cc ../slam6d/io_types.cc) +endif(WIN32) + +if(UNIX) + target_link_libraries(scanio dl) +endif(UNIX) + + diff --git a/.svn/pristine/9e/9e481491afe9990bebcc4fb43e0efaf52fd2deb9.svn-base b/.svn/pristine/9e/9e481491afe9990bebcc4fb43e0efaf52fd2deb9.svn-base new file mode 100644 index 0000000..3f9c228 --- /dev/null +++ b/.svn/pristine/9e/9e481491afe9990bebcc4fb43e0efaf52fd2deb9.svn-base @@ -0,0 +1,227 @@ +/* + * scan_io_ply implementation + * + * Copyright (C) Dorit Borrmann, Thomas Escher, Kai Lingemann, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + + +/** + * @file + * @brief Implementation of reading 3D scans + * @author Dorit Borrmann. Jacobs University Bremen, Germany. + * @author Kai Lingemann. Inst. of CS, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Jacobs University Bremen, Germany. + * @author Thomas Escher. Inst. of CS, University of Osnabrueck, Germany. + */ + +#include "scanio/scan_io_ply.h" +#include "slam6d/point.h" + +#include +using std::cout; +using std::cerr; +using std::endl; +#include +#include + +#ifdef _MSC_VER +#include +#endif + +#include +#include +using namespace boost::filesystem; + +#include "slam6d/globals.icc" + +#define DATA_PATH_PREFIX "scan" +#define DATA_PATH_SUFFIX ".ply" + +std::list ScanIO_ply::readDirectory(const char* dir_path, + unsigned int start, + unsigned int end) +{ + std::list identifiers; + for (unsigned int i = start; i <= end; ++i) { + // identifier is /d/d/d (000-999) + std::string identifier(to_string(i,3)); + // scan consists of data (.3d) and pose (.pose) files + path data(dir_path); + data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX); + // stop if part of a scan is missing or end by absence is detected + if (!exists(data)) + break; + identifiers.push_back(identifier); + } + return identifiers; +} + +void ScanIO_ply::readPose(const char* dir_path, + const char* identifier, + double* pose) +{ + for (unsigned int i = 0; i < 6; ++i) pose[i] = 0.0; +} + +bool ScanIO_ply::supports(IODataType type) +{ + return !!(type & (DATA_XYZ | DATA_REFLECTANCE | DATA_RGB)); +} + +void ScanIO_ply::readScan(const char* dir_path, + const char* identifier, + PointFilter& filter, + std::vector* xyz, + std::vector* rgb, + std::vector* reflectance, + std::vector* temperature, + std::vector* amplitude, + std::vector* type, + std::vector* deviation) +{ + unsigned int i; + + // error handling + path data_path(dir_path); + data_path /= path(std::string(DATA_PATH_PREFIX) + + identifier + DATA_PATH_SUFFIX); + if(!exists(data_path)) + throw std::runtime_error(std::string("There is no scan file for [") + + identifier + "] in [" + dir_path + "]"); + + // open data file + ifstream data_file; + data_file.open(data_path); + data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit); + + if(xyz != 0 && rgb != 0 && reflectance != 0) { + + // read ply file + bool binary = false; + char dummy[256]; + char str[20]; // whatever size + double matrix[16]; + int matrixPos = 0; + int nr; + float d1,d2,d3,d4; + + // header + int counter = -2; + do { + if (counter > -2) counter++; + if (data_file.good()) { + data_file.getline(dummy, 255); + } + if (strncmp(dummy, "format", 6) == 0) { + if (dummy[7] == 'a') binary = false; + else if (dummy[7] == 'b') binary = true; + else { cerr << "Don't recognize the format!" << endl; exit(1); } + } + else if (strncmp(dummy, "element vertex", 14) == 0) { + sscanf(dummy,"%s %*s %d",str,&nr); + counter++; + } + else if (strncmp(dummy, "matrix", 6) == 0) { + sscanf(dummy,"%s %f %f %f %f", str, &d1, &d2, &d3, &d4); + matrix[matrixPos++] = d1; + matrix[matrixPos++] = d2; + matrix[matrixPos++] = d3; + matrix[matrixPos++] = d4; + } + } while (!(strncmp(dummy, "end_header",10) == 0 || !data_file.good())); + + if (matrixPos > 0) { + double rPosTheta[3]; + double rPos[3]; + Matrix4ToEuler(matrix, rPosTheta, rPos); + } + + for (int i=0; i < nr; i++) { + Point p; + float data, confidence, intensity; + float dummy; + int r, g, b; + if (!binary) { + switch(counter) { + case 6: + case 12: + data_file >> p.z >> p.y >> p.x >> r >> g >> b; + break; + case 9: + data_file >> p.z >> p.y >> p.x + >> dummy >> dummy >> dummy + >> r >> g >> b; + break; + default: + data_file >> p.z >> p.x >> p.y >> confidence >> intensity; + break; + } + if(counter == 6 || counter == 9 || counter == 12) { + p.rgb[0] = (char)r; + p.rgb[1] = (char)g; + p.rgb[2] = (char)b; + } else { + p.reflectance = intensity; + } + } else { + data_file.read((char*)&data, sizeof(float)); + p.z = (double)data; + data_file.read((char*)&data, sizeof(float)); + p.x = (double)data; + data_file.read((char*)&data, sizeof(float)); + p.y = (double)data; + data_file.read((char*)&confidence, sizeof(float)); + data_file.read((char*)&intensity, sizeof(float)); + } + + reflectance->push_back(p.reflectance); + xyz->push_back(p.x * 100); + xyz->push_back(p.y * 100); + xyz->push_back(p.z * 100); + rgb->push_back(static_cast(p.rgb[0])); + rgb->push_back(static_cast(p.rgb[1])); + rgb->push_back(static_cast(p.rgb[2])); + } + } +} + + + +/** + * class factory for object construction + * + * @return Pointer to new object + */ +#ifdef _MSC_VER +extern "C" __declspec(dllexport) ScanIO* create() +#else +extern "C" ScanIO* create() +#endif +{ + return new ScanIO_ply; +} + + +/** + * class factory for object construction + * + * @return Pointer to new object + */ +#ifdef _MSC_VER +extern "C" __declspec(dllexport) void destroy(ScanIO *sio) +#else +extern "C" void destroy(ScanIO *sio) +#endif +{ + delete sio; +} + +#ifdef _MSC_VER +BOOL APIENTRY DllMain(HANDLE hModule, DWORD dwReason, LPVOID lpReserved) +{ + return TRUE; +} +#endif diff --git a/.svn/pristine/aa/aa64aec2aa9b57ff566abb39d885906a01ed2ce8.svn-base b/.svn/pristine/aa/aa64aec2aa9b57ff566abb39d885906a01ed2ce8.svn-base new file mode 100644 index 0000000..73080b2 --- /dev/null +++ b/.svn/pristine/aa/aa64aec2aa9b57ff566abb39d885906a01ed2ce8.svn-base @@ -0,0 +1,26 @@ +IF (WITH_THERMO) + + include_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob) + include_directories(${CMAKE_SOURCE_DIR}/include/shapes/) + include_directories(${CMAKE_SOURCE_DIR}/include/thermo/) + include_directories(/usr/include/) + include_directories(/usr/include/opencv) + + add_executable(caliboard caliboard.cc) + add_executable(thermo thermo.cc) +# add_executable(thermo thermo.cc src/cvaux.cpp src/cvblob.cpp src/cvcolor.cpp src/cvcontour.cpp src/cvlabel.cpp src/cvtrack.cpp) + + IF(UNIX) + target_link_libraries(caliboard scan shape newmat dl ANN) + target_link_libraries(thermo scan shape newmat dl ANN) + target_link_libraries(thermo GL GLU cvblob ${OpenCV_LIBS} scan ANN) + ENDIF(UNIX) + + IF (WIN32) + target_link_libraries(caliboard scan shape newmat XGetopt ANN) + target_link_libraries(thermo scan shape newmat XGetopt ANN) + target_link_libraries(thermo GL GLU cvblob ${OpenCV_LIBS} scan ANN) + ENDIF(WIN32) + + +ENDIF(WITH_THERMO) diff --git a/.svn/pristine/b0/b03ec22d7cba6c3b4bdd91895a682bc62d2efba7.svn-base b/.svn/pristine/b0/b03ec22d7cba6c3b4bdd91895a682bc62d2efba7.svn-base new file mode 100644 index 0000000..db01dc1 --- /dev/null +++ b/.svn/pristine/b0/b03ec22d7cba6c3b4bdd91895a682bc62d2efba7.svn-base @@ -0,0 +1,275 @@ +/** + * + * Copyright (C) Jacobs University Bremen + * + * @author Vaibhav Kumar Mehta + * @file calc_normals.cc + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include "slam6d/fbr/panorama.h" +#include + +#include + + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + +namespace po = boost::program_options; +using namespace std; + +enum normal_method {AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST}; + +/* + * validates normal calculation method specification + */ +void validate(boost::any& v, const std::vector& values, + normal_method*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + if(strcasecmp(arg.c_str(), "AKNN") == 0) v = AKNN; + else if(strcasecmp(arg.c_str(), "ADAPTIVE_AKNN") == 0) v = ADAPTIVE_AKNN; + else if(strcasecmp(arg.c_str(), "PANORAMA") == 0) v = PANORAMA; + else if(strcasecmp(arg.c_str(), "PANORAMA_FAST") == 0) v = PANORAMA_FAST; + else throw std::runtime_error(std::string("normal calculation method ") + arg + std::string(" is unknown")); +} + +/// validate IO types +void validate(boost::any& v, const std::vector& values, + IOType*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + try { + v = formatname_to_io_type(arg.c_str()); + } catch (...) { // runtime_error + throw std::runtime_error("Format " + arg + " unknown."); + } +} + +/// Parse commandline options +void parse_options(int argc, char **argv, int &start, int &end, bool &scanserver, int &max_dist, int &min_dist, string &dir, + IOType &iotype, int &k1, int &k2, normal_method &ntype,int &width,int &height) +{ + /// ---------------------------------- + /// set up program commandline options + /// ---------------------------------- + po::options_description cmd_options("Usage: calculateNormals where options are (default values in brackets)"); + cmd_options.add_options() + ("help,?", "Display this help message") + ("start,s", po::value(&start)->default_value(0), "Start at scan number ") + ("end,e", po::value(&end)->default_value(-1), "Stop at scan number ") + ("scanserver,S", po::value(&scanserver)->default_value(false), "Use the scanserver as an input method") + ("format,f", po::value(&iotype)->default_value(UOS), + "using shared library for input. (chose format from [uos|uosr|uos_map|" + "uos_rgb|uos_frames|uos_map_frames|old|rts|rts_map|ifp|" + "riegl_txt|riegl_rgb|riegl_bin|zahn|ply])") + ("max,M", po::value(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than 'units") + ("min,m", po::value(&min_dist)->default_value(-1),"neglegt all data points with a distance smaller than 'units") + ("normal,g", po::value(&ntype)->default_value(AKNN), "normal calculation method " + "(AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST)") + ("K1,k", po::value(&k1)->default_value(20), " value of K value used in the nearest neighbor search of ANN or" "kmin for k-adaptation") + ("K2,K", po::value(&k2)->default_value(20), " value of Kmax for k-adaptation") + ("width,w", po::value(&width)->default_value(1280),"width of panorama image") + ("height,h", po::value(&height)->default_value(960),"height of panorama image") + ; + + po::options_description hidden("Hidden options"); + hidden.add_options() + ("input-dir", po::value(&dir), "input dir"); + + po::positional_options_description pd; + pd.add("input-dir", 1); + + po::options_description all; + all.add(cmd_options).add(hidden); + + po::variables_map vmap; + po::store(po::command_line_parser(argc, argv).options(all).positional(pd).run(), vmap); + po::notify(vmap); + + if (vmap.count("help")) { + cout << cmd_options << endl << endl; + cout << "SAMPLE COMMAND FOR CALCULATING NORMALS" << endl; + cout << " bin/normals -s 0 -e 0 -f UOS -g AKNN -k 20 dat/" < &points, vector &normals, int scanNumber) +{ + string ofilename = dir + "/scan" + to_string(scanNumber, 3) + ".3d"; + ofstream normptsout(ofilename.c_str()); + + for (size_t i=0; i::iterator it = Scan::allScans.begin(); + int scanNumber = 0; + + for( ; it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + + // apply optional filtering + scan->setRangeFilter(max_dist, min_dist); + + const double* rPos = scan->get_rPos(); + const double* rPosTheta = scan->get_rPosTheta(); + + /// read scan into points + DataXYZ xyz(scan->get("xyz")); + vector points; + points.reserve(xyz.size()); + vector normals; + normals.reserve(xyz.size()); + + for(unsigned int j = 0; j < xyz.size(); j++) { + points.push_back(Point(xyz[j][0], xyz[j][1], xyz[j][2])); + } + + if(ntype == AKNN) + calculateNormalsApxKNN(normals,points, k1, rPos); + else if(ntype == ADAPTIVE_AKNN) + calculateNormalsAdaptiveApxKNN(normals,points, k1, k2, rPos); + else + { + // create panorama + fbr::panorama fPanorama(width, height, fbr::EQUIRECTANGULAR, 1, 0, fbr::EXTENDED); + fPanorama.createPanorama(scan2mat(scan)); + + // the range image has to be converted from float to uchar + img = fPanorama.getRangeImage(); + img = float2uchar(img, 0, 0.0); + + if(ntype == PANORAMA) + calculateNormalsPANORAMA(normals,points,fPanorama.getExtendedMap(), rPos); + else if(ntype == PANORAMA_FAST) + cout << "PANORAMA_FAST is not working yet" << endl; + // calculateNormalsFAST(normals,points,img,fPanorama.getExtendedMap()); + } + + // pose file (repeated for the number of segments + writePoseFiles(normdir, rPos, rPosTheta, scanNumber); + // scan files for all segments + writeScanFiles(normdir, points,normals,scanNumber); + + scanNumber++; + } + + // shutdown everything + if (scanserver) + ClientInterface::destroy(); + + Scan::closeDirectory(); + + cout << "Normal program end" << endl; + + return 0; +} diff --git a/.svn/pristine/be/becc3bca612dff3c260324085b9e15e86162ebdb.svn-base b/.svn/pristine/be/becc3bca612dff3c260324085b9e15e86162ebdb.svn-base new file mode 100644 index 0000000..0174007 --- /dev/null +++ b/.svn/pristine/be/becc3bca612dff3c260324085b9e15e86162ebdb.svn-base @@ -0,0 +1,996 @@ +/* + * slam6D implementation + * + * Copyright (C) Andreas Nuechter, Kai Lingemann, Jochen Sprickerhof + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Main programm for matching 3D scans (6D SLAM) + * + * Main programm to match 3D scans with ICP and the globally + * consistent matching approach. + * Use -i from the command line to match with ICP, + * and -I to match 3D Scans using the global algorithm. + * + * @author Andreas Nuechter. Jacobs University Bremen gGmbH, Germany + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Jochen Sprickerhof. Institute of Computer Science, University of Osnabrueck, Germany. + */ + +#include "slam6d/scan.h" +#include "slam6d/metaScan.h" +#include "slam6d/io_utils.h" + +#include "slam6d/icp6Dapx.h" +#include "slam6d/icp6Dsvd.h" +#include "slam6d/icp6Dquat.h" +#include "slam6d/icp6Dortho.h" +#include "slam6d/icp6Dhelix.h" +#include "slam6d/icp6Ddual.h" +#include "slam6d/icp6Dlumeuler.h" +#include "slam6d/icp6Dlumquat.h" +#include "slam6d/icp6Dquatscale.h" +#include "slam6d/icp6D.h" +#ifdef WITH_CUDA +#include "slam6d/cuda/icp6Dcuda.h" +#endif +#include "slam6d/lum6Deuler.h" +#include "slam6d/lum6Dquat.h" +#include "slam6d/ghelix6DQ2.h" +#include "slam6d/elch6Deuler.h" +#include "slam6d/elch6Dquat.h" +#include "slam6d/elch6DunitQuat.h" +#include "slam6d/elch6Dslerp.h" +#include "slam6d/graphSlam6D.h" +#include "slam6d/gapx6D.h" +#include "slam6d/graph.h" +#include "slam6d/globals.icc" + +#ifndef _MSC_VER +#include +#else +#include "XGetopt.h" +#endif + +#include + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + +#ifdef WITH_METRICS +#include "slam6d/metrics.h" +#endif //WITH_METRICS + + +#ifdef _MSC_VER +#if !defined _OPENMP && defined OPENMP +#define _OPENMP +#endif +#endif + +#ifdef _OPENMP +#include +#endif + +#define WANT_STREAM ///< define the WANT stream :) + +#include +using std::string; +#include +using std::cout; +using std::cerr; +using std::endl; +#include +using std::ifstream; + + +// Handling Segmentation faults and CTRL-C +void sigSEGVhandler (int v) +{ + static bool segfault = false; + if(!segfault) { + segfault = true; + cout << endl + << "# **************************** #" << endl + << " Segmentation fault or Ctrl-C" << endl + << "# **************************** #" << endl + << endl; + + // save frames and close scans + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + (*it)->saveFrames(); + } + cout << "Frames saved." << endl; + Scan::closeDirectory(); + } + exit(-1); +} + + +/** + * Explains the usage of this program's command line parameters + */ +void usage(char* prog) +{ +#ifndef _MSC_VER + const string bold("\033[1m"); + const string normal("\033[m"); +#else + const string bold(""); + const string normal(""); +#endif + cout << endl + << bold << "USAGE " << normal << endl + << " " << prog << " [options] directory" << endl << endl; + cout << bold << "OPTIONS" << normal << endl + + << bold << " -a" << normal << " NR, " << bold << "--algo=" << normal << "NR [default: 1]" << endl + << " selects the minimizazion method for the ICP matching algorithm" << endl + << " 1 = unit quaternion based method by Horn" << endl + << " 2 = singular value decomposition by Arun et al. " << endl + << " 3 = orthonormal matrices by Horn et al." << endl + << " 4 = dual quaternion method by Walker et al." << endl + << " 5 = helix approximation by Hofer & Potmann" << endl + << " 6 = small angle approximation" << endl + << " 7 = Lu & Milios style, i.e., uncertainty based, with Euler angles" << endl + << " 8 = Lu & Milios style, i.e., uncertainty based, with Quaternion" << endl + << " 9 = unit quaternion with scale method by Horn" << endl + << endl + << bold << " -A" << normal << " NR, " << bold << "--anim=" << normal << "NR [default: first and last frame only]" << endl + << " if specified, use only every NR-th frame for animation" << endl + << endl + << bold << " -c" << normal << " NR, " << bold << "--cldist=" << normal << "NR [default: 500]" << endl + << " specifies the maximal distance for closed loops" << endl + << endl + << bold << " -C" << normal << " NR, " << bold << "--clpairs=" << normal << "NR [default: 6]" << endl + << " specifies the minimal number of points for an overlap. If not specified" << endl + << " cldist is used instead" << endl + << endl + << bold << " --cache" << normal << endl + << " turns on cached k-d tree search" << endl + << endl + << bold << " -d" << normal << " NR, " << bold << "--dist=" << normal << "NR [default: 25]" << endl + << " sets the maximal point-to-point distance for matching with ICP to 'units'" << endl + << " (unit of scan data, e.g. cm)" << endl + << endl + << bold << " -D" << normal << " NR, " << bold << "--distSLAM=" + << normal << "NR [default: same value as -d option]" << endl + << " sets the maximal point-to-point distance for matching with SLAM to 'units'" << endl + << " (unit of scan data, e.g. cm)" << endl + << endl + << bold << " --DlastSLAM" << normal << " NR [default not set]" << endl + << " sets the maximal point-to-point distance for the final SLAM correction," << endl + << " if final SLAM is not required don't set it." << endl + << endl + << bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl + << " end after scan NR" << endl + << endl + << bold << " --exportAllPoints" << normal << endl + << " writes all registered reduced points to the file points.pts before" << endl + << " slam6D terminated" << endl + << endl + << bold << " --epsICP=" << normal << "NR [default: 0.00001]" << endl + << " stop ICP iteration if difference is smaller than NR" << endl + << endl + << bold << " --epsSLAM=" << normal << " NR [default: 0.5]" << endl + << " stop SLAM iteration if average difference is smaller than NR" << endl + << endl + << bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl + << " using shared library F for input" << endl + << " (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply, wrl, xyz, zuf, iais, front, x3d, rxp, ais })" << endl + << endl + << bold << " -G" << normal << " NR, " << bold << "--graphSlam6DAlgo=" << normal << "NR [default: 0]" << endl + << " selects the minimizazion method for the SLAM matching algorithm" << endl + << " 0 = no global relaxation technique" << endl + << " 1 = Lu & Milios extension using euler angles due to Borrmann et al." << endl + << " 2 = Lu & Milios extension using using unit quaternions" << endl + << " 3 = HELIX approximation by Hofer and Pottmann" << endl + << " 4 = small angle approximation" << endl + << bold << " -i" << normal << " NR, " << bold << "--iter=" << normal << "NR [default: 50]" << endl + << " sets the maximal number of ICP iterations to " << endl + << endl + << bold << " -I" << normal << " NR, " << bold << "--iterSLAM=" << normal << "NR [default: 0]" << endl + << " sets the maximal number of iterations for SLAM to " << endl + << " (if not set, graphSLAM is not executed)" << endl + << endl + << bold << " -l" << normal << " NR, " << bold << "--loopsize=" << normal << "NR [default: 20]" << endl + << " sets the size of a loop, i.e., a loop must exceed of scans" << endl + << endl + << bold << " -L" << normal << " NR, " << bold << "--loop6DAlgo=" << normal << "NR [default: 0]" << endl + << " selects the method for closing the loop explicitly" << endl + << " 0 = no loop closing technique" << endl + << " 1 = euler angles" << endl + << " 2 = quaternions " << endl + << " 3 = unit quaternions" << endl + << " 4 = SLERP (recommended)" << endl + << endl + << bold << " --metascan" << normal << endl + << " Match current scan against a meta scan of all previous scans (default match against the last scan only)" << endl + << endl + << bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl + << " neglegt all data points with a distance larger than NR 'units'" << endl + << endl + << bold << " -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl + << " neglegt all data points with a distance smaller than NR 'units'" << endl + << endl + << bold << " -n" << normal << " FILE, " << bold << "--net=" << normal << "FILE" << endl + << " specifies the file that includes the net structure for SLAM" << endl + << endl + << bold << " -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl + << " use randomized octree based point reduction (pts per voxel=)" << endl + << " requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl + << endl + << bold << " -p, --trustpose" << normal << endl + << " Trust the pose file, do not extrapolate the last transformation." << endl + << " (just for testing purposes, or gps input.)" << endl + << endl + << bold << " -q, --quiet" << normal << endl + << " Quiet mode. Suppress (most) messages" << endl + << endl + << bold << " -Q, --veryquiet" << normal << endl + << " Very quiet mode. Suppress all messages, except in case of error." << endl + << endl + << bold << " -S, --scanserver" << normal << endl + << " Use the scanserver as an input method and handling of scan data" << endl + << endl + << bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl + << " turns on octree based point reduction (voxel size=)" << endl + << endl + << bold << " -R" << normal << " NR, " << bold << "--random=" << normal << "NR" << endl + << " turns on randomized reduction, using about every -th point only" << endl + << endl + << bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl + << " start at scan NR (i.e., neglects the first NR scans)" << endl + << " [ATTENTION: counting naturally starts with 0]" << endl + << endl + << bold << " -t" << normal << " NR, " << bold << "--nns_method=" << normal << "NR [default: 1]" << endl + << " selects the Nearest Neighbor Search Algorithm" << endl + << " 0 = simple k-d tree " << endl + << " 1 = cached k-d tree " << endl + << " 2 = ANNTree " << endl + << " 3 = BOCTree " << endl + << endl + << bold << " -u" << normal <<", "<< bold<<"--cuda" << normal << endl + << " this option activates icp running on GPU instead of CPU"<exact) + * @param meta match against all scans (= meta scan), or against the last scan only??? + * @param anim selects the rotation representation for the matching algorithm + * @param mni_lum sets the maximal number of iterations for SLAM + * @param net specifies the file that includes the net structure for SLAM + * @param cldist specifies the maximal distance for closed loops + * @param epsilonICP stop ICP iteration if difference is smaller than this value + * @param epsilonSLAM stop SLAM iteration if average difference is smaller than this value + * @param algo specfies the used algorithm for rotation computation + * @param lum6DAlgo specifies the used algorithm for global SLAM correction + * @param loopsize defines the minimal loop size + * @return 0, if the parsing was successful. 1 otherwise + */ +int parseArgs(int argc, char **argv, string &dir, double &red, int &rand, + double &mdm, double &mdml, double &mdmll, + int &mni, int &start, int &end, int &maxDist, int &minDist, bool &quiet, bool &veryQuiet, + bool &extrapolate_pose, bool &meta, int &algo, int &loopSlam6DAlgo, int &lum6DAlgo, int &anim, + int &mni_lum, string &net, double &cldist, int &clpairs, int &loopsize, + double &epsilonICP, double &epsilonSLAM, int &nns_method, bool &exportPts, double &distLoop, + int &iterLoop, double &graphDist, int &octree, bool &cuda_enabled, IOType &type, + bool& scanserver, PairingMode& pairing_mode) +{ + int c; + // from unistd.h: + extern char *optarg; + extern int optind; + + WriteOnce w_type(type); + WriteOnce w_start(start), w_end(end); + + /* options descriptor */ + // 0: no arguments, 1: required argument, 2: optional argument + static struct option longopts[] = { + { "format", required_argument, 0, 'f' }, + { "algo", required_argument, 0, 'a' }, + { "nns_method", required_argument, 0, 't' }, + { "loop6DAlgo", required_argument, 0, 'L' }, + { "graphSlam6DAlgo", required_argument, 0, 'G' }, + { "net", required_argument, 0, 'n' }, + { "iter", required_argument, 0, 'i' }, + { "iterSLAM", required_argument, 0, 'I' }, + { "max", required_argument, 0, 'm' }, + { "loopsize", required_argument, 0, 'l' }, + { "cldist", required_argument, 0, 'c' }, + { "clpairs", required_argument, 0, 'C' }, + { "min", required_argument, 0, 'M' }, + { "dist", required_argument, 0, 'd' }, + { "distSLAM", required_argument, 0, 'D' }, + { "start", required_argument, 0, 's' }, + { "end", required_argument, 0, 'e' }, + { "reduce", required_argument, 0, 'r' }, + { "octree", optional_argument, 0, 'O' }, + { "random", required_argument, 0, 'R' }, + { "quiet", no_argument, 0, 'q' }, + { "veryquiet", no_argument, 0, 'Q' }, + { "trustpose", no_argument, 0, 'p' }, + { "anim", required_argument, 0, 'A' }, + { "metascan", no_argument, 0, '2' }, // use the long format only + { "DlastSLAM", required_argument, 0, '4' }, // use the long format only + { "epsICP", required_argument, 0, '5' }, // use the long format only + { "epsSLAM", required_argument, 0, '6' }, // use the long format only + { "normalshoot", no_argument, 0, '7' }, // use the long format only + { "point-to-plane", no_argument, 0, 'z' }, // use the long format only + { "exportAllPoints", no_argument, 0, '8' }, + { "distLoop", required_argument, 0, '9' }, // use the long format only + { "iterLoop", required_argument, 0, '1' }, // use the long format only + { "graphDist", required_argument, 0, '3' }, // use the long format only + { "cuda", no_argument, 0, 'u' }, // cuda will be enabled + { "scanserver", no_argument, 0, 'S' }, + { 0, 0, 0, 0} // needed, cf. getopt.h + }; + + cout << endl; + while ((c = getopt_long(argc, argv, "O:f:A:G:L:a:t:r:R:d:D:i:l:I:c:C:n:s:e:m:M:uqQpS", longopts, NULL)) != -1) { + switch (c) { + case 'a': + algo = atoi(optarg); + if ((algo < 0) || (algo > 9)) { + cerr << "Error: ICP Algorithm not available." << endl; + exit(1); + } + break; + case 't': + nns_method = atoi(optarg); + if ((nns_method < 0) || (nns_method > 3)) { + cerr << "Error: NNS Method not available." << endl; + exit(1); + } + break; + case 'L': + loopSlam6DAlgo = atoi(optarg); + if (loopSlam6DAlgo < 0 || loopSlam6DAlgo > 6) { + cerr << "Error: global loop closing algorithm not available." << endl; + exit(1); + } + break; + case 'G': + lum6DAlgo = atoi(optarg); + if ((lum6DAlgo < 0) || (lum6DAlgo > 6)) { + cerr << "Error: global relaxation algorithm not available." << endl; + exit(1); + } + break; + case 'c': + cldist = atof(optarg); + break; + case 'C': + clpairs = atoi(optarg); + break; + case 'l': + loopsize = atoi(optarg); + break; + case 'r': + red = atof(optarg); + break; + case 'O': + if (optarg) { + octree = atoi(optarg); + } else { + octree = 1; + } + break; + case 'R': + rand = atoi(optarg); + break; + case 'd': + mdm = atof(optarg); + break; + case 'D': + mdml = atof(optarg); + break; + case 'i': + mni = atoi(optarg); + break; + case 'I': + mni_lum = atoi(optarg); + break; + case 'n': + net = optarg; + break; + case 's': + w_start = atoi(optarg); + if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } + break; + case 'e': + w_end = atoi(optarg); + if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } + if (end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } + break; + case 'm': + maxDist = atoi(optarg); + break; + case 'M': + minDist = atoi(optarg); + break; + case 'q': + quiet = true; + break; + case 'Q': + quiet = veryQuiet = true; + break; + case 'p': + extrapolate_pose = false; + break; + case 'A': + anim = atoi(optarg); + break; + case '2': // = --metascan + meta = true; + break; + case '4': // = --DlastSLAM + mdmll = atof(optarg); + break; + case '5': // = --epsICP + epsilonICP = atof(optarg); + break; + case '6': // = --epsSLAM + epsilonSLAM = atof(optarg); + break; + case '8': // not used + exportPts = true; + break; + case '9': // = --distLoop + distLoop = atof(optarg); + break; + case '1': // = --iterLoop + iterLoop = atoi(optarg); + break; + case '3': // = --graphDist + graphDist = atof(optarg); + break; + case '7': // = --normalshoot + pairing_mode = CLOSEST_POINT_ALONG_NORMAL; + break; + case 'z': // = --point-to-plane + pairing_mode = CLOSEST_PLANE; + break; + case 'f': + try { + w_type = formatname_to_io_type(optarg); + } catch (...) { // runtime_error + cerr << "Format " << optarg << " unknown." << endl; + abort(); + } + break; + case 'u': + cuda_enabled = true; + break; + case 'S': + scanserver = true; + break; + case '?': + usage(argv[0]); + return 1; + default: + abort (); + } + } + + if (optind != argc-1) { + cerr << "\n*** Directory missing ***" << endl; + usage(argv[0]); + } + dir = argv[optind]; + +#ifndef _MSC_VER + if (dir[dir.length()-1] != '/') dir = dir + "/"; +#else + if (dir[dir.length()-1] != '\\') dir = dir + "\\"; +#endif + + parseFormatFile(dir, w_type, w_start, w_end); + + return 0; +} + +/** + * This function is does all the matching stuff + * it iterates over all scans using the algorithm objects to calculate new poses + * objects could be NULL if algorithm should not be used + * + * @param cldist maximal distance for closing loops + * @param loopsize minimal loop size + * @param allScans Contains all laser scans + * @param my_icp6D the ICP implementation + * @param meta_icp math ICP against a metascan + * @param nns_method Indicates the nearest neigbor search method to be used + * @param my_loopSlam6D used loopoptimizer + * @param my_graphSlam6D used global optimization + * @param nrIt The number of iterations the global SLAM-algorithm will run + * @param epsilonSLAM epsilon for global SLAM iteration + * @param mdml maximal distance match for global SLAM + * @param mdmll maximal distance match for global SLAM after all scans ar matched + */ +void matchGraph6Dautomatic(double cldist, int loopsize, vector allScans, icp6D *my_icp6D, + bool meta_icp, int nns_method, bool cuda_enabled, + loopSlam6D *my_loopSlam6D, graphSlam6D *my_graphSlam6D, int nrIt, + double epsilonSLAM, double mdml, double mdmll, double graphDist, + bool &eP, IOType type) +{ + double cldist2 = sqr(cldist); + + // list of scan for metascan + vector < Scan* > metas; + + // graph for loop optimization + graph_t g; + + int n = allScans.size(); + + int loop_detection = 0; + double dist, min_dist = -1; + int first = 0, last = 0; + + for(int i = 1; i < n; i++) { + cout << i << "/" << n << endl; + + add_edge(i-1, i, g); + + if(eP) { + allScans[i]->mergeCoordinatesWithRoboterPosition(allScans[i-1]); + } + + //Hack to get all icp transformations into the .frames Files + if(i == n-1 && my_icp6D != NULL && my_icp6D->get_anim() == -2) { + my_icp6D->set_anim(-1); + } + + /*if(i == 85 || i == 321 || i == 533) { + my_icp6D->set_anim(1); + }*/ + + if(my_icp6D != NULL){ + cout << "ICP" << endl; + // Matching strongly linked scans with ICPs + if(meta_icp) { + metas.push_back(allScans[i - 1]); + MetaScan* meta_scan = new MetaScan(metas); + my_icp6D->match(meta_scan, allScans[i]); + delete meta_scan; + } else { + switch(type) { + case UOS_MAP: + case UOS_MAP_FRAMES: + my_icp6D->match(allScans[0], allScans[i]); + break; + case RTS_MAP: + //untested (and could not work) + //if(i < 220-22 && i > 250-22) match(allScans[0], CurrentScan); + my_icp6D->match(allScans[0], allScans[i]); + break; + default: + my_icp6D->match(allScans[i - 1], allScans[i]); + break; + } + } + } else { + double id[16]; + M4identity(id); + allScans[i]->transform(id, Scan::ICP, 0); + } + + /*if(i == 85 || i == 321 || i == 533) { + my_icp6D->set_anim(-2); + }*/ + + if(loop_detection == 1) { + loop_detection = 2; + } + + for(int j = 0; j < i - loopsize; j++) { + dist = Dist2(allScans[j]->get_rPos(), allScans[i]->get_rPos()); + if(dist < cldist2) { + loop_detection = 1; + if(min_dist < 0 || dist < min_dist) { + min_dist = dist; + first = j; + last = i; + } + } + } + + if(loop_detection == 2) { + loop_detection = 0; + min_dist = -1; + + if(my_loopSlam6D != NULL) { + cout << "Loop close: " << first << " " << last << endl; + my_loopSlam6D->close_loop(allScans, first, last, g); + add_edge(first, last, g); + } + + if(my_graphSlam6D != NULL && mdml > 0) { + int j = 0; + double ret; + do { + // recalculate graph + Graph *gr = new Graph(i + 1, cldist2, loopsize); + cout << "Global: " << j << endl; + ret = my_graphSlam6D->doGraphSlam6D(*gr, allScans, 1); + delete gr; + j++; + } while (j < nrIt && ret > epsilonSLAM); + } + } + } + + if(loop_detection == 1 && my_loopSlam6D != NULL) { + cout << "Loop close: " << first << " " << last << endl; + my_loopSlam6D->close_loop(allScans, first, last, g); + add_edge(first, last, g); + } + + if(my_graphSlam6D != NULL && mdml > 0.0) { + int j = 0; + double ret; + do { + // recalculate graph + Graph *gr = new Graph(n, cldist2, loopsize); + cout << "Global: " << j << endl; + ret = my_graphSlam6D->doGraphSlam6D(*gr, allScans, 1); + delete gr; + j++; + } while (j < nrIt && ret > epsilonSLAM); + } + + if(my_graphSlam6D != NULL && mdmll > 0.0) { + my_graphSlam6D->set_mdmll(mdmll); + int j = 0; + double ret; + do { + // recalculate graph + Graph *gr = new Graph(n, sqr(graphDist), loopsize); + cout << "Global: " << j << endl; + ret = my_graphSlam6D->doGraphSlam6D(*gr, allScans, 1); + delete gr; + j++; + } while (j < nrIt && ret > epsilonSLAM); + } +} + +/** + * Main program for 6D SLAM. + * Usage: bin/slam6D 'dir', + * with 'dir' the directory of a set of scans + * ... + */ +int main(int argc, char **argv) +{ + signal (SIGSEGV, sigSEGVhandler); + signal (SIGINT, sigSEGVhandler); + + cout << "slam6D - A highly efficient SLAM implementation based on scan matching" << endl + << " with 6 degrees of freedom" << endl + << "(c) Jacobs University Bremen gGmbH, Germany, since 2009" << endl + << " University of Osnabrueck, Germany, since 2006" << endl << endl; + + if (argc <= 1) { + usage(argv[0]); + } + + // parsing the command line parameters + // init, default values if not specified + string dir; + double red = -1.0, mdmll = -1.0, mdml = 25.0, mdm = 25.0; + int rand = -1, mni = 50; + int start = 0, end = -1; + bool quiet = false; + bool veryQuiet = false; + int maxDist = -1; + int minDist = -1; + bool eP = true; // should we extrapolate the pose?? + bool meta = false; // match against meta scan, or against LAST scan only? + int algo = 1; + int mni_lum = -1; + double cldist = 500; + int clpairs = -1; + int loopsize = 20; + string net = "none"; + int anim = -1; + double epsilonICP = 0.00001; + double epsilonSLAM = 0.5; + int nns_method = simpleKD; + bool exportPts = false; + int loopSlam6DAlgo = 0; + int lum6DAlgo = 0; + double distLoop = 700.0; + int iterLoop = 100; + double graphDist = cldist; + int octree = 0; // employ randomized octree reduction? + bool cuda_enabled = false; + IOType type = UOS; + bool scanserver = false; + PairingMode pairing_mode = CLOSEST_POINT; + + parseArgs(argc, argv, dir, red, rand, mdm, mdml, mdmll, mni, start, end, + maxDist, minDist, quiet, veryQuiet, eP, meta, algo, loopSlam6DAlgo, lum6DAlgo, anim, + mni_lum, net, cldist, clpairs, loopsize, epsilonICP, epsilonSLAM, + nns_method, exportPts, distLoop, iterLoop, graphDist, octree, cuda_enabled, type, + scanserver, pairing_mode); + + cout << "slam6D will proceed with the following parameters:" << endl; + //@@@ to do :-) + // TODO: writer a proper TODO ^ + + Scan::openDirectory(scanserver, dir, type, start, end); + + if(Scan::allScans.size() == 0) { + cerr << "No scans found. Did you use the correct format?" << endl; + exit(-1); + } + + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + scan->setRangeFilter(maxDist, minDist); + unsigned int types = 0; + if ((pairing_mode == CLOSEST_POINT_ALONG_NORMAL) || + (pairing_mode == CLOSEST_PLANE)) { + types = PointType::USE_NORMAL; + } + scan->setReductionParameter(red, octree, PointType(types)); + scan->setSearchTreeParameter(nns_method, cuda_enabled); + } + + icp6Dminimizer *my_icp6Dminimizer = 0; + switch (algo) { + case 1 : + my_icp6Dminimizer = new icp6D_QUAT(quiet); + break; + case 2 : + my_icp6Dminimizer = new icp6D_SVD(quiet); + break; + case 3 : + my_icp6Dminimizer = new icp6D_ORTHO(quiet); + break; + case 4 : + my_icp6Dminimizer = new icp6D_DUAL(quiet); + break; + case 5 : + my_icp6Dminimizer = new icp6D_HELIX(quiet); + break; + case 6 : + my_icp6Dminimizer = new icp6D_APX(quiet); + break; + case 7 : + my_icp6Dminimizer = new icp6D_LUMEULER(quiet); + break; + case 8 : + my_icp6Dminimizer = new icp6D_LUMQUAT(quiet); + break; + case 9 : + my_icp6Dminimizer = new icp6D_QUAT_SCALE(quiet); + break; + } + + // match the scans and print the time used + long starttime = GetCurrentTimeInMilliSec(); + +#ifdef WITH_METRICS + Timer t = ClientMetric::matching_time.start(); +#endif //WITH_METRICS + + if (mni_lum == -1 && loopSlam6DAlgo == 0) { + icp6D *my_icp = 0; + if (cuda_enabled) { +#ifdef WITH_CUDA + my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, cuda_enabled); +#else + cout << "slam6d was not compiled for excuting CUDA code" << endl; +#endif + } else { + my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, cuda_enabled); + } + + // check if CAD matching was selected as type + if (type == UOS_CAD) + { + my_icp->set_cad_matching (true); + } + + if (my_icp) my_icp->doICP(Scan::allScans, pairing_mode); + delete my_icp; + } else if (clpairs > -1) { + //!!!!!!!!!!!!!!!!!!!!!!!! + icp6D *my_icp = 0; + if (cuda_enabled) { +#ifdef WITH_CUDA + my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, cuda_enabled); +#else + cout << "slam6d was not compiled for excuting CUDA code" << endl; +#endif + } else { + my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, cuda_enabled); + } + my_icp->doICP(Scan::allScans, pairing_mode); + graphSlam6D *my_graphSlam6D = new lum6DEuler(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, + rand, eP, anim, epsilonICP, nns_method, epsilonSLAM); + my_graphSlam6D->matchGraph6Dautomatic(Scan::allScans, mni_lum, clpairs, loopsize); + //!!!!!!!!!!!!!!!!!!!!!!!! + } else { + graphSlam6D *my_graphSlam6D = 0; + switch (lum6DAlgo) { + case 1 : + my_graphSlam6D = new lum6DEuler(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, epsilonSLAM); + break; + case 2 : + my_graphSlam6D = new lum6DQuat(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, epsilonSLAM); + break; + case 3 : + my_graphSlam6D = new ghelix6DQ2(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, epsilonSLAM); + break; + case 4 : + my_graphSlam6D = new gapx6D(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, epsilonSLAM); + break; + } + // Construct Network + if (net != "none") { + icp6D *my_icp = 0; + if (cuda_enabled) { +#ifdef WITH_CUDA + my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method); +#else + cout << "slam6d was not compiled for excuting CUDA code" << endl; +#endif + } else { + my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method); + } + my_icp->doICP(Scan::allScans, pairing_mode); + + Graph* structure; + structure = new Graph(net); + my_graphSlam6D->doGraphSlam6D(*structure, Scan::allScans, mni_lum); + if(mdmll > 0.0) { + my_graphSlam6D->set_mdmll(mdmll); + my_graphSlam6D->doGraphSlam6D(*structure, Scan::allScans, mni_lum); + } + + } else { + icp6D *my_icp = 0; + if(algo > 0) { + if (cuda_enabled) { +#ifdef WITH_CUDA + my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method); +#else + cout << "slam6d was not compiled for excuting CUDA code" << endl; +#endif + } else { + my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method); + } + + loopSlam6D *my_loopSlam6D = 0; + switch(loopSlam6DAlgo) { + case 1: + my_loopSlam6D = new elch6Deuler(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, + rand, eP, 10, epsilonICP, nns_method); + break; + case 2: + my_loopSlam6D = new elch6Dquat(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, + rand, eP, 10, epsilonICP, nns_method); + break; + case 3: + my_loopSlam6D = new elch6DunitQuat(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, + rand, eP, 10, epsilonICP, nns_method); + break; + case 4: + my_loopSlam6D = new elch6Dslerp(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, + rand, eP, 10, epsilonICP, nns_method); + break; + } + + matchGraph6Dautomatic(cldist, loopsize, Scan::allScans, my_icp, meta, + nns_method, cuda_enabled, my_loopSlam6D, my_graphSlam6D, + mni_lum, epsilonSLAM, mdml, mdmll, graphDist, eP, type); + delete my_icp; + if(loopSlam6DAlgo > 0) { + delete my_loopSlam6D; + } + } + if(my_graphSlam6D > 0) { + delete my_graphSlam6D; + } + } + } + +#ifdef WITH_METRICS + ClientMetric::matching_time.end(t); +#endif //WITH_METRICS + + long endtime = GetCurrentTimeInMilliSec() - starttime; + cout << "Matching done in " << endtime << " milliseconds!!!" << endl; + + if (exportPts) { + cout << "Export all 3D Points to file \"points.pts\"" << endl; + ofstream redptsout("points.pts"); + for(unsigned int i = 0; i < Scan::allScans.size(); i++) { + DataXYZ xyz_r(Scan::allScans[i]->get("xyz reduced")); + DataNormal normal_r(Scan::allScans[i]->get("normal reduced")); + for(unsigned int i = 0; i < xyz_r.size(); ++i) { + int r,g,b; + r = (int)(normal_r[i][0] * (127.5) + 127.5); + g = (int)(normal_r[i][1] * (127.5) + 127.5); + b = (int)(fabs(normal_r[i][2]) * (255.0)); + redptsout << xyz_r[i][0] << ' ' << xyz_r[i][1] << ' ' << xyz_r[i][2] + << ' ' << r << ' ' << g << ' ' << b << '\n'; + } + redptsout << std::flush; + } + redptsout.close(); + redptsout.clear(); + } + + const double* p; + ofstream redptsout("loopclose.pts"); + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) + { + Scan* scan = *it; + p = scan->get_rPos(); + Point x(p[0], p[1], p[2]); + redptsout << x << endl; + scan->saveFrames(); + } + redptsout.close(); + + Scan::closeDirectory(); + delete my_icp6Dminimizer; + + cout << endl << endl; + cout << "Normal program end." << endl + << (red < 0 && rand < 0 ? "(-> HINT: For a significant speedup, please use the '-r' or '-R' parameter <-)\n" + : "") + << endl; + + // print metric information +#ifdef WITH_METRICS + ClientMetric::print(scanserver); +#endif //WITH_METRICS +} diff --git a/.svn/pristine/be/bed5706f445696a09d44974b7fbc00582316ea46.svn-base b/.svn/pristine/be/bed5706f445696a09d44974b7fbc00582316ea46.svn-base new file mode 100644 index 0000000..ce5de17 --- /dev/null +++ b/.svn/pristine/be/bed5706f445696a09d44974b7fbc00582316ea46.svn-base @@ -0,0 +1,424 @@ +cmake_minimum_required (VERSION 2.8.2) +SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/3rdparty/CMakeModules" ${CMAKE_MODULE_PATH}) +project (3DTK) + + +#include_directories(OPENGL_INCLUDE_DIR) +IF(WIN32) + set(Boost_USE_STATIC_LIBS TRUE) +ELSE(WIN32) + set(Boost_USE_STATIC_LIBS FALSE) +ENDIF(WIN32) + +SET(Boost_ADDITIONAL_VERSIONS "1.42" "1.42.0" "1.44" "1.44.0" "1.45.0" "1.45" "1.46" "1.46.1" "1.47.0" "1.47" "1.48" "1.49") +IF(WIN32) + # for some unknown reason no one variant works on all windows platforms + find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time program_options REQUIRED) +ELSE(WIN32) + find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time program_options REQUIRED) +ENDIF(WIN32) + +if(Boost_FOUND) + link_directories(${BOOST_LIBRARY_DIRS}) + include_directories(${Boost_INCLUDE_DIRS}) + add_definitions(${Boost_LIB_DIAGNOSTIC_DEFINITIONS}) +endif() + +FIND_PACKAGE(OpenCV REQUIRED) +include("3rdparty/CMakeModules/OpenCV.cmake") +SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_OPENCV_FLAGS}") + +################################################# +# Declare Options and modify build accordingly ## +################################################# + + +FUNCTION(ENFORCE_OPTION_DEP_3DTK option VALUE) + SET (${option} "${VALUE}" CACHE BOOL "${${option}_DESCRIPTION}" FORCE) # this option set to VALUE as advised + + #now make sure other dependencies are also true + FOREACH(d ${${option}_DEPENDENCIES}) # look through all my dependencies + STRING(REGEX REPLACE " +" ";" CMAKE_DEPENDENT_OPTION_DEP "${d}") + + # check for a not in front + STRING(STRIP "${CMAKE_DEPENDENT_OPTION_DEP}" CMAKE_DEPENDENT_OPTION_DEP) + STRING(SUBSTRING "${CMAKE_DEPENDENT_OPTION_DEP}" 0 3 CMAKE_DEPENDENT_OPTION_DEP_3) + STRING(TOUPPER "${CMAKE_DEPENDENT_OPTION_DEP_3}" CMAKE_DEPENDENT_OPTION_DEP_3) + STRING(COMPARE EQUAL "${CMAKE_DEPENDENT_OPTION_DEP_3}" "NOT" CMAKE_DEPENDENT_OPTION_DEP_NOT) + #STRING(REPLACE "NOT " "" CMAKE_DEPENDENT_OPTION_DEP "${d}") + IF(CMAKE_DEPENDENT_OPTION_DEP_NOT) # we found a NOT + STRING(REPLACE "NOT;" "" CMAKE_DEPENDENT_OPTION_DEP "${CMAKE_DEPENDENT_OPTION_DEP}") + IF(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is + ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} OFF) + ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met + ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) + ELSE(CMAKE_DEPENDENT_OPTION_DEP_NOT) + IF(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met + ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is + ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} ON) + ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) + ENDIF(CMAKE_DEPENDENT_OPTION_DEP_NOT) + ENDFOREACH(d) + +ENDFUNCTION(ENFORCE_OPTION_DEP_3DTK) + +MACRO(OPT_DEP option doc default depends) + OPTION(${option} "${doc}" "${default}") + SET(${option} "${${option}}" CACHE BOOL "${doc}" FORCE) + SET(${option}_DEPENDENCIES "${depends}" CACHE INTERNAL "" FORCE) + SET(${option}_DESCRIPTION "${doc}" CACHE INTERNAL "" FORCE) + + IF (${option}) + #MESSAGE(STATUS "Yes ${option} is true") + # MESSAGE("FOREACH d in ${depends}") + + FOREACH(d ${depends}) + STRING(REGEX REPLACE " +" ";" CMAKE_DEPENDENT_OPTION_DEP "${d}") + + # check for a not in front + STRING(STRIP "${CMAKE_DEPENDENT_OPTION_DEP}" CMAKE_DEPENDENT_OPTION_DEP) + STRING(SUBSTRING "${CMAKE_DEPENDENT_OPTION_DEP}" 0 3 CMAKE_DEPENDENT_OPTION_DEP_3) + STRING(TOUPPER "${CMAKE_DEPENDENT_OPTION_DEP_3}" CMAKE_DEPENDENT_OPTION_DEP_3) + STRING(COMPARE EQUAL "${CMAKE_DEPENDENT_OPTION_DEP_3}" "NOT" CMAKE_DEPENDENT_OPTION_DEP_NOT) + IF(CMAKE_DEPENDENT_OPTION_DEP_NOT) # we found a NOT + STRING(REPLACE "NOT;" "" CMAKE_DEPENDENT_OPTION_DEP "${CMAKE_DEPENDENT_OPTION_DEP}") + IF(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is + ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} OFF) + ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met + ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) + ELSE(CMAKE_DEPENDENT_OPTION_DEP_NOT) + IF(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met + ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is + ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} ON) + ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) + ENDIF(CMAKE_DEPENDENT_OPTION_DEP_NOT) + ENDFOREACH(d) + + ENDIF(${option}) +ENDMACRO(OPT_DEP) + + + +## FreeGLUT +OPT_DEP(WITH_FREEGLUT "Whether freeglut is available. This enables iterative drawing in show. ON/OFF" ON "") + +IF(WITH_FREEGLUT) + MESSAGE(STATUS "With freeglut") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_FREEGLUT") +ELSE(WITH_FREEGLUT) + MESSAGE(STATUS "Without freeglut") +ENDIF(WITH_FREEGLUT) + +## Show +OPT_DEP(WITH_SHOW "Whether to build Show. This is the Visualization program of slam6d. ON/OFF" ON "" ) + +IF(WITH_SHOW) + FIND_PACKAGE(OpenGL REQUIRED) + FIND_PACKAGE(GLUT REQUIRED) + MESSAGE(STATUS "With show") +ELSE(WITH_SHOW) + # SET (WITH_OCTREE_DISPLAY "ON" CACHE INTERNAL "" FORCE) + MESSAGE(STATUS "Without show") +ENDIF(WITH_SHOW) + +## WXShow +OPT_DEP(WITH_WXSHOW "Whether to build WXShow. This is the wxwidgets variant of Show. ON/OFF" OFF "") + +IF(WITH_WXSHOW) + FIND_PACKAGE(OpenGL REQUIRED) + FIND_PACKAGE(GLUT REQUIRED) + find_package(wxWidgets COMPONENTS core base gl REQUIRED) + # set wxWidgets_wxrc_EXECUTABLE to be ignored in the configuration + SET (wxWidgets_wxrc_EXECUTABLE " " CACHE INTERNAL "" FORCE) + # wxWidgets include (this will do all the magic to configure everything) + include( ${wxWidgets_USE_FILE}) + MESSAGE(STATUS "With wxshow") +ELSE(WITH_WXSHOW) + MESSAGE(STATUS "Without wxshow") +ENDIF(WITH_WXSHOW) + +## Shapes +OPT_DEP(WITH_SHAPE_DETECTION "Whether to build shapes and planes executable for detecting planes. ON/OFF" OFF "") +IF(WITH_SHAPE_DETECTION) + MESSAGE(STATUS "With shape detection") +ELSE(WITH_SHAPE_DETECTION) + MESSAGE(STATUS "Without shape detection") +ENDIF(WITH_SHAPE_DETECTION) + +## Interior reconstruction +OPT_DEP(WITH_MODEL "Whether to build model executable for modelling interior environments. ON/OFF" OFF "") + +if(WITH_MODEL) + message(STATUS "With interior reconstruction") +else(WITH_MODEL) + message(STATUS "Without interior reconstruction") +endif(WITH_MODEL) + +## Thermo +OPT_DEP(WITH_THERMO "Whether to build executables for mutual calibration of laser scanner and camera. ON/OFF" OFF "WITH_SHAPE_DETECTION") +IF(WITH_THERMO) + add_subdirectory(3rdparty/cvblob) + include_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob) + link_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob) + MESSAGE(STATUS "With thermo") +ELSE(WITH_THERMO) + MESSAGE(STATUS "Without thermo") +ENDIF(WITH_THERMO) + + +## Octree +OPT_DEP(WITH_COMPACT_OCTREE "Whether to use the compact octree display ON/OFF" OFF "") + +IF(WITH_COMPACT_OCTREE) + MESSAGE(STATUS "Using compact octrees") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DUSE_COMPACT_TREE") +ELSE(WITH_COMPACT_OCTREE) + MESSAGE(STATUS "Not using compact octreees: Warning uses more memory") +ENDIF(WITH_COMPACT_OCTREE) + +## Glee? +OPT_DEP(WITH_GLEE "Whether to use OpenGL extensions, requires glee. ON/OFF" OFF "") + +IF(WITH_GLEE) + MESSAGE(STATUS "Using opengl extensions") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_GLEE") +ELSE(WITH_GLEE) + MESSAGE(STATUS "Not using opengl extensions") +ENDIF(WITH_GLEE) + +## Gridder +OPT_DEP(WITH_GRIDDER "Whether to build the 2DGridder binary ON/OFF" OFF "") + +IF(WITH_GRIDDER) + MESSAGE(STATUS "With 2DGridder") +ELSE(WITH_GRIDDER) + MESSAGE(STATUS "Without 2DGridder") +ENDIF(WITH_GRIDDER) + +## Dynamic VELOSLAM +OPT_DEP(WITH_VELOSLAM "Whether to build the Velodyne data processing (veloslam/veloshow) ON/OFF" OFF "WITH_SHOW") + +IF(WITH_VELOSLAM) + MESSAGE(STATUS "With VELOSLAM") +ELSE(WITH_VELOSLAM) + MESSAGE(STATUS "Without VELOSLAM") +ENDIF(WITH_VELOSLAM) + +## Home-made Laserscanner +OPT_DEP(WITH_DAVID_3D_SCANNER "Whether to build the David scanner app for homemade laser scanners binary ON/OFF" OFF "") + +IF(WITH_DAVID_3D_SCANNER) + MESSAGE(STATUS "With David scanner") +ELSE(WITH_DAVID_3D_SCANNER) + MESSAGE(STATUS "Without David scanner") +ENDIF(WITH_DAVID_3D_SCANNER) + +## Tools + +OPT_DEP(WITH_TOOLS "Whether to build additional tools like convergence frame_to_graph etc. ON/OFF" OFF "WITH_FBR") + +IF(WITH_TOOLS) + MESSAGE(STATUS "With Tools") + find_package (Boost COMPONENTS program_options REQUIRED) +ELSE(WITH_TOOLS) + MESSAGE(STATUS "Without Tools") +ENDIF(WITH_TOOLS) + +## Segmentation + +OPT_DEP(WITH_SEGMENTATION "Whether to build scan segmantion program ON/OFF" OFF "WITH_FBR") + +IF(WITH_SEGMENTATION) + MESSAGE(STATUS "With segmentation") + find_package (Boost COMPONENTS program_options REQUIRED) +ELSE(WITH_SEGMENTATION) + MESSAGE(STATUS "Without segmentation") +ENDIF(WITH_SEGMENTATION) + +## RivLib +OPT_DEP(WITH_RIVLIB "Whether the RIEGL rivlib is present ON/OFF" OFF "") + +IF(WITH_RIVLIB) + MESSAGE(STATUS "Compiling a scan IO for RXP files") + include_directories(${CMAKE_SOURCE_DIR}/3rdparty) + link_directories(${CMAKE_SOURCE_DIR}/3rdparty) + SET(RIEGL_DIR ${CMAKE_SOURCE_DIR}/3rdparty/riegl/) + IF(WIN32) + SET(RIVLIB ${RIEGL_DIR}libscanlib-mt.lib ${RIEGL_DIR}libctrllib-mt.lib ${RIEGL_DIR}libboost_system-mt-1_43_0-vns.lib) + ELSE(WIN32) + SET(RIVLIB ${RIEGL_DIR}libscanlib-mt-s.a ${RIEGL_DIR}libctrllib-mt-s.a ${RIEGL_DIR}libboost_system-mt-s-1_43_0-vns.a pthread) + ENDIF(WIN32) + FIND_PACKAGE(LibXml2 ) + +ELSE(WITH_RIVLIB) + MESSAGE(STATUS "Do NOT compile a scan IO for RXP") +ENDIF(WITH_RIVLIB) + +## CUDA support, TODO depend on CUDA_FIND +OPT_DEP(WITH_CUDA "Compile with CUDA support" OFF "") +IF(WITH_CUDA) + MESSAGE(STATUS "Compiling WITH CUDA support") + FIND_PACKAGE(CUDA) + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_CUDA") +ELSE(WITH_CUDA) + MESSAGE(STATUS "Compiling WITHOUT CUDA support") +ENDIF(WITH_CUDA) + +## PMD +OPT_DEP(WITH_PMD "Whether to build the PMD tools like grabVideoAnd3D calibrate etc. ON/OFF" OFF "") + +IF(WITH_PMD) + FIND_PACKAGE(OpenGL REQUIRED) + MESSAGE(STATUS "With Tools") +ELSE(WITH_PMD) + MESSAGE(STATUS "Without Tools") +ENDIF(WITH_PMD) + +## FBR +OPT_DEP(WITH_FBR "Whether to compile feature based registration. ON/OFF" OFF "") + +IF(WITH_FBR) + MESSAGE(STATUS "With FBR ") +ELSE(WITH_FBR) + MESSAGE(STATUS "Without FBR") +ENDIF(WITH_FBR) + +# OPEN +FIND_PACKAGE(OpenMP) +IF(OPENMP_FOUND) + OPT_DEP(WITH_OPENMP "Whether to use parallel processing capabilities of OPENMP. ON/OFF" ON "") +ENDIF(OPENMP_FOUND) + +IF(OPENMP_FOUND AND WITH_OPENMP) + MESSAGE(STATUS "With OpenMP ") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=${NUMBER_OF_CPUS} -DOPENMP_NUM_THREADS=${NUMBER_OF_CPUS} ${OpenMP_CXX_FLAGS} -DOPENMP") +ELSE(OPENMP_FOUND AND WITH_OPENMP) + MESSAGE(STATUS "Without OpenMP") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=1 -DOPENMP_NUM_THREADS=1") +ENDIF(OPENMP_FOUND AND WITH_OPENMP) + + +OPT_DEP(EXPORT_SHARED_LIBS "Whether to build additional shared libraries for use in other projects. ON/OFF" OFF "WITH_SHOW;WITH_FBR") + +IF(EXPORT_SHARED_LIBS) + ## Compile a single shared library containing all of 3DTK + add_library(slam SHARED src/slam6d/icp6D.cc) + target_link_libraries(slam scan_s ANN_s sparse_s newmat_s show_s fbr_s) + MESSAGE(STATUS "exporting additional libraries") +ELSE(EXPORT_SHARED_LIBS) + MESSAGE(STATUS "not exporting libraries") +ENDIF(EXPORT_SHARED_LIBS) + + +OPT_DEP(WITH_METRICS "Whether to use metrics in slam6d. ON/OFF" OFF "") + +IF(WITH_METRICS) + MESSAGE(STATUS "With metrics in slam6d.") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_METRICS") +ELSE(WITH_METRICS) + MESSAGE(STATUS "Without metrics in slam6d.") +ENDIF(WITH_METRICS) + +################################################# +# OPERATING SYSTEM SPECIFIC BEHAVIOUR ## +################################################# + +## Special treatment for system specifics +IF(APPLE) +add_definitions(-Dfopen64=fopen) +ENDIF(APPLE) + +## Multiple Cores +IF(APPLE) + SET(PROCESSOR_COUNT 2) +ELSE(APPLE) + INCLUDE(CountProcessors) + SET(NUMBER_OF_CPUS "${PROCESSOR_COUNT}" CACHE STRING "The number of processors to use (default: ${PROCESSOR_COUNT})" ) +ENDIF(APPLE) + +IF(WIN32) + SET(ADDITIONAL_CFLAGS "-O2" CACHE STRING "Additional flags given to the compiler ( -O2)" ) + include_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/) + link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows) + link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/x64) + add_library(XGetopt STATIC ${CMAKE_SOURCE_DIR}/3rdparty/windows/XGetopt.cpp) + SET(CMAKE_STATIC_LIBRARY_SUFFIX "32.lib") +ELSE(WIN32) + SET(ADDITIONAL_CFLAGS "-O3 -std=c++0x -msse3 -Wall -finline-functions -Wno-unused-but-set-variable -Wno-write-strings -Wno-char-subscripts -Wno-unused-result" CACHE STRING"Additional flags given to the compiler (-O3 -Wall -finline-functions -Wno-write-strings)" ) + # Add include path for OpenGL without GL/-prefix + # to avoid the include incompatibility between MACOS + # and linux + FIND_PATH(OPENGL_INC gl.h /usr/include/GL) + include_directories(${OPENGL_INC}) +ENDIF(WIN32) + +# Add OpenGL includes for MACOS if needed +# The OSX OpenGL frameworks natively supports freeglut extensions +IF(APPLE) + include_directories(/System/Library/Frameworks/GLUT.framework/Headers) + include_directories(/System/Library/Frameworks/OpenGL.framework/Headers) +ENDIF(APPLE) + +# hack to "circumvent" Debug and Release folders that are created under visual studio +# this is why the INSTALL target has to be used in visual studio +IF(MSVC) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Release/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + IF( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 ) + + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Debug/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + IF( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 ) +ENDIF(MSVC) + + + +################################################# +# GENERAL PROJECT SETTINGS ## +################################################# + +SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CFLAGS}") + +# Hide CMake variables +SET (CMAKE_INSTALL_PREFIX "/usr/local" CACHE INTERNAL "" FORCE) +SET (CMAKE_BUILD_TYPE "" CACHE INTERNAL "" FORCE) + + +# Set output directories for libraries and executables +SET( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/lib ) +SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/obj ) +SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin ) + +# Set include and link dirs ... +include_directories(${CMAKE_SOURCE_DIR}/include) +include_directories(${CMAKE_SOURCE_DIR}/3rdparty/) +include_directories(${CMAKE_SOURCE_DIR}/3rdparty/glui) +include_directories(${CMAKE_SOURCE_DIR}/3rdparty/wxthings/include/) +include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/include) +include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/src) +link_directories(${CMAKE_SOURCE_DIR}/obj) +link_directories(${CMAKE_SOURCE_DIR}/lib) + +add_subdirectory(3rdparty) +add_subdirectory(src/slam6d) +add_subdirectory(src/scanio) +add_subdirectory(src/scanserver) +add_subdirectory(src/segmentation) +add_subdirectory(src/normals) +add_subdirectory(src/veloslam) +add_subdirectory(src/show) +add_subdirectory(src/grid) +add_subdirectory(src/pmd) +add_subdirectory(src/shapes) +add_subdirectory(src/thermo) +add_subdirectory(src/slam6d/fbr) +add_subdirectory(src/scanner) +add_subdirectory(src/model) + +MESSAGE (STATUS "Build environment is set up!") diff --git a/.svn/pristine/df/dfe7a87a356300cae6565726ccc1c7d05b04365a.svn-base b/.svn/pristine/df/dfe7a87a356300cae6565726ccc1c7d05b04365a.svn-base new file mode 100644 index 0000000..b1f9264 --- /dev/null +++ b/.svn/pristine/df/dfe7a87a356300cae6565726ccc1c7d05b04365a.svn-base @@ -0,0 +1,610 @@ +/** + * + * Copyright (C) Jacobs University Bremen + * + * @author Vaibhav Kumar Mehta + * @author Corneliu Claudiu Prodescu + * @file normals.cc + */ + +#include +#include +#include "slam6d/io_types.h" +#include "slam6d/globals.icc" +#include "slam6d/scan.h" +#include "slam6d/fbr/panorama.h" +#include "slam6d/kd.h" +#include +#include "newmat/newmat.h" +#include "newmat/newmatap.h" + +#include "normals/normals.h" + +using namespace NEWMAT; +using namespace std; + +////////////////////////////////////////////////////// +/////////////NORMALS USING AKNN METHOD //////////////// +/////////////////////////////////////////////////////// +void calculateNormalsApxKNN(vector &normals, + vector &points, + int k, + const double _rPos[3], + double eps) +{ + cout<<"Total number of points: "< &normals, + vector &points, + int kmin, + int kmax, + const double _rPos[3], + double eps) +{ + ColumnVector rPos(3); + for (int i = 0; i < 3; ++i) + rPos(i+1) = _rPos[i]; + + cout<<"Total number of points: "< 0.25 * e2) && (fabs(1.0 - (double)e2/(double)e3) < 0.25)) + break; + } + + //normal = eigenvector corresponding to lowest + //eigen value that is the 1rd column of matrix U + ColumnVector n(3); + n(1) = U(1,1); + n(2) = U(2,1); + n(3) = U(3,1); + ColumnVector point_vector(3); + point_vector(1) = p[0] - rPos(1); + point_vector(2) = p[1] - rPos(2); + point_vector(3) = p[2] - rPos(3); + point_vector = point_vector / point_vector.NormFrobenius(); + Real angle = (n.t() * point_vector).AsScalar(); + if (angle < 0) { + n *= -1.0; + } + n = n / n.NormFrobenius(); + normals.push_back(Point(n(1), n(2), n(3))); + } + annDeallocPts(pa); +} + + + +/////////////////////////////////////////////////////// +/////////////NORMALS USING AKNN METHOD //////////////// +/////////////////////////////////////////////////////// +void calculateNormalsKNN(vector &normals, + vector &points, + int k, + const double _rPos[3] ) +{ + cout<<"Total number of points: "< temp = t.kNearestNeighbors(p, + nr_neighbors, + numeric_limits::max()); + nr_neighbors = temp.size(); + + Point mean(0.0,0.0,0.0); + Matrix X(nr_neighbors,3); + SymmetricMatrix A(3); + Matrix U(3,3); + DiagonalMatrix D(3); + + //calculate mean for all the points + for (int j = 0; j < nr_neighbors; ++j) + { + mean.x += temp[j].x; + mean.y += temp[j].y; + mean.z += temp[j].z; + } + mean.x /= nr_neighbors; + mean.y /= nr_neighbors; + mean.z /= nr_neighbors; + + //calculate covariance = A for all the points + for (int i = 0; i < nr_neighbors; ++i) { + X(i+1, 1) = temp[i].x - mean.x; + X(i+1, 2) = temp[i].y - mean.y; + X(i+1, 3) = temp[i].z - mean.z; + } + + A << 1.0/nr_neighbors * X.t() * X; + + EigenValues(A, D, U); + + //normal = eigenvector corresponding to lowest + //eigen value that is the 1st column of matrix U + ColumnVector n(3); + n(1) = U(1,1); + n(2) = U(2,1); + n(3) = U(3,1); + ColumnVector point_vector(3); + point_vector(1) = p[0] - rPos(1); + point_vector(2) = p[1] - rPos(2); + point_vector(3) = p[2] - rPos(3); + point_vector = point_vector / point_vector.NormFrobenius(); + Real angle = (n.t() * point_vector).AsScalar(); + if (angle < 0) { + n *= -1.0; + } + n = n / n.NormFrobenius(); + normals.push_back(Point(n(1), n(2), n(3))); + } + + + for (size_t i = 0; i < points.size(); ++i) + { + delete[] pa[i]; + } + delete[] pa; + +} +//////////////////////////////////////////////////////////////// +/////////////NORMALS USING ADAPTIVE AKNN METHOD //////////////// +//////////////////////////////////////////////////////////////// +void calculateNormalsAdaptiveKNN(vector &normals, + vector &points, + int kmin, + int kmax, + const double _rPos[3]) +{ + ColumnVector rPos(3); + for (int i = 0; i < 3; ++i) + rPos(i+1) = _rPos[i]; + + cout<<"Total number of points: "< temp = t.kNearestNeighbors(p, + nr_neighbors, + numeric_limits::max()); + nr_neighbors = temp.size(); + + mean.x=0,mean.y=0,mean.z=0; + //calculate mean for all the points + for (int j=0; j 0.25 * e2) && (fabs(1.0 - (double)e2/(double)e3) < 0.25)) + break; + } + + //normal = eigenvector corresponding to lowest + //eigen value that is the 1rd column of matrix U + ColumnVector n(3); + n(1) = U(1,1); + n(2) = U(2,1); + n(3) = U(3,1); + ColumnVector point_vector(3); + point_vector(1) = p[0] - rPos(1); + point_vector(2) = p[1] - rPos(2); + point_vector(3) = p[2] - rPos(3); + point_vector = point_vector / point_vector.NormFrobenius(); + Real angle = (n.t() * point_vector).AsScalar(); + if (angle < 0) { + n *= -1.0; + } + n = n / n.NormFrobenius(); + normals.push_back(Point(n(1), n(2), n(3))); + } + + for (size_t i = 0; i < points.size(); ++i) + { + delete[] pa[i]; + } + delete[] pa; + +} + + +/////////////////////////////////////////////////////// +/////////////NORMALS USING IMAGE NEIGHBORS //////////// +/////////////////////////////////////////////////////// +void calculateNormalsPANORAMA(vector &normals, + vector &points, + vector< vector< vector< cv::Vec3f > > > extendedMap, + const double _rPos[3]) +{ + ColumnVector rPos(3); + for (int i = 0; i < 3; ++i) + rPos(i+1) = _rPos[i]; + + cout<<"Total number of points: "< neighbors; + for (size_t i=0; i< extendedMap.size(); i++) + { + for (size_t j=0; j= 0 && x < (int)extendedMap.size() && + y >= 0 && y < (int)extendedMap[x].size()) { + for (unsigned int k = 0; k < extendedMap[x][y].size(); k++) { + neighbors.push_back(extendedMap[x][y][k]); + } + } + } + + nr_neighbors = neighbors.size(); + cv::Vec3f p = extendedMap[i][j][0]; + + //if no neighbor point is found in the 4-neighboring pixels then normal is set to zero + if (nr_neighbors < 3) + { + points.push_back(Point(p[0], p[1], p[2])); + normals.push_back(Point(0.0,0.0,0.0)); + continue; + } + + //calculate mean for all the points + Matrix X(nr_neighbors,3); + SymmetricMatrix A(3); + Matrix U(3,3); + DiagonalMatrix D(3); + + //calculate mean for all the points + for(int k = 0; k < nr_neighbors; k++) + { + cv::Vec3f pp = neighbors[k]; + mean.x += pp[0]; + mean.y += pp[1]; + mean.z += pp[2]; + } + + mean.x /= nr_neighbors; + mean.y /= nr_neighbors; + mean.z /= nr_neighbors; + //calculate covariance = A for all the points + for (int n = 0; n < nr_neighbors; ++n) { + cv::Vec3f pp = neighbors[n]; + X(n+1, 1) = pp[0] - mean.x; + X(n+1, 2) = pp[1] - mean.y; + X(n+1, 3) = pp[2] - mean.z; + } + + A << 1.0/nr_neighbors * X.t() * X; + + EigenValues(A, D, U); + //normal = eigenvector corresponding to lowest + //eigen value that is the 1st column of matrix U + ColumnVector n(3); + n(1) = U(1,1); + n(2) = U(2,1); + n(3) = U(3,1); + ColumnVector point_vector(3); + point_vector(1) = p[0] - rPos(1); + point_vector(2) = p[1] - rPos(2); + point_vector(3) = p[2] - rPos(3); + point_vector = point_vector / point_vector.NormFrobenius(); + Real angle = (n.t() * point_vector).AsScalar(); + if (angle < 0) { + n *= -1.0; + } + n = n / n.NormFrobenius(); + + for (unsigned int k = 0; k < extendedMap[i][j].size(); k++) { + cv::Vec3f p = extendedMap[i][j][k]; + points.push_back(Point(p[0], p[1], p[2])); + normals.push_back(Point(n(1), n(2), n(3))); + } + } + } +} +////////////////////////////////////////////////////////////////////////////////////////////// +///////////FAST NORMALS USING PANORAMA EQUIRECTANGULAR RANGE IMAGE ////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////////// +/* + void calculateNormalsFAST(vector &normals,vector &points,cv::Mat &img,vector>> extendedMap) + { + cout<<"Total number of points: "<(i-1,j); + dRdPhi += 3 *img.at(i-1,j-1); + dRdPhi += 3 *img.at(i-1,j+1); + dRdPhi -= 10*img.at(i+1,j); + dRdPhi -= 3 *img.at(i+1,j-1); + dRdPhi -= 3 *img.at(i+1,j+1); + + dRdTheta += 10*img.at(i,j-1); + dRdTheta += 3 *img.at(i-1,j-1); + dRdTheta += 3 *img.at(i+1,j-1); + dRdTheta -= 10*img.at(i,j+1); + dRdTheta -= 3 *img.at(i-1,j+1); + dRdTheta -= 3 *img.at(i+1,j+1); + + n[0] = cos(theta) * sin(phi) - sin(theta) * dRdTheta / rho / sin(phi) + + cos(theta) * cos(phi) * dRdPhi / rho; + + n[1] = sin(theta) * sin(phi) + cos(theta) * dRdTheta / rho / sin(phi) + + sin(theta) * cos(phi) * dRdPhi / rho; + + n[2] = cos(phi) - sin(phi) * dRdPhi / rho; + + //n[2] = -n[2]; + + m = sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]); + n[0] /= m; n[1] /= m; n[2] /= m; + + points.push_back(Point(x, y, z)); + normals.push_back(Point(n[0],n[1],n[2])); + } + } + } + } +*/ diff --git a/.svn/pristine/f1/f16e6c52546d685f14bc88c1b412a66c2e069b85.svn-base b/.svn/pristine/f1/f16e6c52546d685f14bc88c1b412a66c2e069b85.svn-base new file mode 100644 index 0000000..cc9a0a6 --- /dev/null +++ b/.svn/pristine/f1/f16e6c52546d685f14bc88c1b412a66c2e069b85.svn-base @@ -0,0 +1,64 @@ +This file describes the scanserver functionality, the code changes and its behaviour. + +To run with the scanserver functionality, pass -S or --scanserver to the slam6D/show call. Start the scanserver with bin/scanserver & first. If you want to directly jump to usage examples, see the "USAGE" section below. + +The scanserver is a new method to load and manage scans for 'slam6D', 'show' and some few other tools (so far). It removes all the IO code from the clients and handles it in the server process. This separation offers persistence of scan data and avoids unneccessary reloads of full scans or even reduced versions thereof. By using a caching framework it also transparently handles the available memory given and enables (nearly) endless amounts of data. The client is only required to open the interface, load a directory and start working on those scans without having to alter its workflow (e.g., pre-reduce them) to accomodate huge data volumes. + +If you have questions or problems (or both), contact Thomas Escher . + + + +USAGE: + +1. General + +Start the scanserver once (in another terminal, or in the same one as a background process): + bin/scanserver & + +Do all the normal work as you would normally do, adding the parameter -S: + bin/slam6D dat -S + bin/show dat -S + +2. Changing the available memory size + +Changing the cache memory size used by scan data (about half the system memory usually works): + + bin/scanserver -c 3500 (for 8GB RAM) + +If you intend to not reload the full scans for different reduction parameters or don't have too much memory/disk space, disable binary scan caching. Binary scan caching saves the full scans as long as the range or height parameters aren't touched, which would cause a full reload: + + bin/scanserver -b 0 + +If your dataset contains many scans and loops (e.g., 'hannover' with 468 scans), the default data memory (150M) won't be enough to hold all the animation frames and you need to increase it: + + bin/scanserver -d 250 + +3. Altering the shared memory on your linux system (bus_error) + +If you receive a bus_error, the size of your shared memory is too small and the requested allocation was too big. This is resolved via remounting your shm device. Default is half of the available RAM. This limit can be increased to nearly 90% of the RAM if required. + + sudo mount -o remount,size=7000M /dev/shm (for 8GB RAM) + +4. Locking the memory to avoid swapping (Linux only) + +If a great portion of the RAM is used for the cache data, swapping will usually occur after 50% of usage. To avoid this, the scanserver tries to lock the whole memory in place. This will fail without superuser rights, as well as on a full shared memory (see 3.) even with rights. To solve this problem, add these two additional lines to '/etc/security/limits.conf': + + * soft memlock unlimited + * hard memlock unlimited +After adding these lines and rebooting the system the scanserver can be started without superuser rights. + +5. Using the octtree serialization feature in show with scanserver + +The octtree serialization behaves slightly different than before. Since the scanserver caches octtrees between calls of 'show', the loading of octtrees only becomes relevant if no octtrees are in the cache and have to be created from the scan itself. If this has been done once before and the octtrees have been saved via --saveOct before this can be used to speed up the octtree loading with --loadOct. + + bin/show dat --loadOct --saveOct + +If octtrees are not cached, they are deserialized if available, created otherwise and then saved for future calls. + +IMPLEMENTATION STATUS: + +Currently only 'slam6D' and 'show' are working with the scanserver. + +Since the scanserver handles the disk IO now and the filtering has been optimized, not all ScanIOs are updated yet. Just copy and paste and change the minor parts about reading the input lines. + +Working: 'ls src/scanio' in a shell diff --git a/.svn/wc.db b/.svn/wc.db index 2972030feea091436323e51d68a608d3ff98a13d..d14c5f74425787a6c67b3f34e458bd928302ba2c 100644 GIT binary patch delta 28535 zcma)l2YeO9_VCWk?%v&yLP$sgBoJy6Aj$0oM5IWOqGAUGZlwwc2%_&9K@_+Y!AM51 zgU^CW5zVS36!Bpf6$JzZ5er4-DJlpm-r>V@-lB+Xm+YB z?7hA*p>wCetE=XYfflFdj@YoJegBTF?qu*URU7`RJX!y5WoGX?DH{nFJ7GMt#%Kkw zatY-J|70#gD|-xa!cFEW)dM$~#XkO7J6n4`;Zs|e4VMimB>dJfq$Rl*KJUn<7Sl4) z8GaVObnOW*Wd`5+0?Dd07Ce#sy`gr&EGljeHrKN&uU))c{C2D%S=^XPiz-X*ufPW> zBx^(M{WC<%%7dq~D*He17(V|{%c}hIffte=ZK$n0Meybo=;qyJPbGhI;~r8}xw>k& zcyMryp3RRx!!j#N`5o|}^5->K8)~>O1z&!s0)i_)62E`$2~d7q`MUTmaH_0X)i0GP zdlh9b`IergXDgro?cK_~PYq2rSk?bOvU|X{a>UcW!t=`3e`jqN{mh?okf-7K?W@CrpU3e6LRs9t*~oh7h2uEHpVw3)NEqIRnzftL@J2VXDwotA5x>OJ_*A3I(oFT?pdHloIl5aND?wE?7JKz(($7d8( zPWa?s_*M-1uTP#4zYmVkvNycGbD%E1hCx-f*!w&_pRQ+B?%Vs0`2F*2HEY9b`!*mE z#8mUodYDC(&why{M67S$zeBYA_&Ys&!_5a?#D2wRm94%O-5}(BUn8`GVl=NE8q{x& zqRdeiDi11;C{HObC@(8D%KOTv$`{Hv$`8t)L?x-po64^wo3tZt(wkgMB4iqwLmnhg zk(bHGPuYxmFeG>RSy4lNM+f#g_YgE%ddR*+XvviaBmH8y$qaCth?Q1Hd6-;s|c4Np!+%Jg_i1ui)) z#0B_%8@8PSlzaV5RJ;dj2_E1MAqT(xy%v81pKN&TYzY(ZL00%lO5_V@WNl<*nwUWj zbNye@0(hS{YD6AQernY=BAspG?pPC2Q}?tciv$Ej?s$b{^G3Oe4}_rB=90c@Cd>~{ zE!0|*g&^)Bz9o+|Dv1SL4!a|4_eWixSjgpz#zL{E%N_Fg++jz^?Q%MN0lPcOj#|)s ziYdUm+mM&V^&oH+|EmoVtSorKs%cARN+yNz@hdtIQ~)2h)Sy`hY=$Cg)fACCB~yX$ z@_`*?JBRq_Y)A5(fc>^ye65y;F7nzMc5gqi&=XaVN_#Iy8JVDeHpQO=21mh!jp?X2pc4be3 z3J_pg*b@xas_jKU3bhoTSxK7m$uAP!8q|mUAc-#MJopN-5<3Uo#-Yw=5+&R_CQ;E1??YOO}wPY2zbg6!4A93BFIy&g(pN<`iXjOF%?(a^ zRX;>>h?wB_pVZ5EpXKBzu$p59*-wed8$U{F;qu|5WCUDFAH&N+c{wI8S3FKOHyfth zuWVCJl1^kKsU&ZaGqj9`=rZ~aFPuXILR+L*%by^lnd)L=DPKRw1jKe$|Eb`Afz5Lw;TIw^}Q3MWK525mLnGpH=U))Mv?uh%s3O;!IbQGJ)}GGK0YNx#z`j z;3F_kI8bDk-&VuNuf9lTB)|zBgyM604Ot_9XY);KNuSgf3iwiElo!DQ&F87>$c=J# zK3cEB?3lcDU(z&Va&YXFu@grRniPq`d@}yxIxttUCW-Fni2YJ^|i2j;Tz z0bS#dttB>o%X$z-tug<3J-Li(8gE5uQ>))gg2cqG5Igyr{i%8v3yD`+deq=snL-8JMXN)?5ZMl_ojte_cu$r(cv6r6R& z$QDv0uPkv@6IU&9)e~1+;%eZv1K50WLSafPMQKG(5+8jSB&Fd$v$NwFc|7<62=JaK z%qINzmq`GLEA;V$kCCGM^NFhqa_{Lk$Jm=G=2(4BQ{r zVSt2Bd4nV{$Q5$0TW7a{Pb8qAHaAAnB3|+qxeE~zrl{sE(gC$4=(wi*j}N8)AOyv# z-9c`_Pdk4?w&Dk%dHIZYKzvGeCP0WMH9JYa?3T)hit;UKO1xlLr;$g(c$eK1aTj76466 z&G+Prl%a|;R9U2KRepoWN$bZK&7rCMtD8x&_3Iq=0^zYa%*BT=TM^%WAG?h&pTkPH zeJ=Z``UkQcX@Pcd%}?Y@(HV;5#BpY+_2F^yn|v>1anCOVm6fnH*2WX$X?%s2XYTI+ zNPY^FdJ_BqxDn2{ReO?zMLX0*p3~CL0gLvQcm~d$ReJ`UuffGaDcuDQ@@i#^vI}m0 zAU4v5bR##Csbn#Emb^i}AivYbv?INY-asePd+8%|1AUhsq^DR0>%h9PYuR`*7){?#=UbRlwa`>5+ zvK{(YwXJC1=9$V81^#kul(xylo78|I>Samx(8o{Dz`Bz6oO!7smPISJ+ zQh1z*3iOU5X?qWtt+brpDV{vJ*aX{CPW$Bel_wNslk$#oRQZi4q!GkG z8TSB zaFYP0YMi+nojlH85l`D#6qb?Md$!P>%JaC^%x0tWCXW7a6|@nM%8Y z3RefE2qF zF7Gr@bwJi*Jm0cKYh(deI$70?v_xCNZe(wRN^WHhx{;2#Mp5Z}W84+-VBQ8hO;e!y!7Ye%ro?y@w_WB_v z^7|uTVq-o>*zI?-C|h61s0DCmoXqP~bPaHF|o8YtvX)X#O>2Qi390HQsEG(x;C_t+=Os|tMj<8L6SHt_k z6BF-6Kw&^)fj65ZpIn_Gn0gZJV>AKf3;!)%mZA=pLiEtvwh4sqKEgWig>T!^_>468 zu;aV7)x~bV!yN#>BN_+gqd*D)JmIA|3CwY2)9^V z&)nQU1*RbFe)=$uTZF8&_a}ymO0-oKiUrEzZWs8D%{Y!!>QFA5qHMK#jJ_&>pf>#B zF)H%{IO&hm0Z0!tWMwNUrW#P{@|Fq?StW`0vu=V^uA)OE@kHp^{%HvmvrfM}EfE)A zTD8vzsbu7=LeA26Suav#HGYBS5=K8)mSvi^P~rg@jPYeJQfVqh99pmj;t4SqNt2+? z3d~k`ix){Df8tV_&Tm^wG5nO~WG#I{pn}8O^&DWj{3SXL$0(xHybW}TxRI)9188v3 zR>;7}YPvM}TeB#-Ba;0sdcV~Ay@8FEy$&GfZ=$ydC`gV*o9UzCM$piz-3+7`cM@C3 zMe>zz(4hjb2--d3YyUPnSu_!IWz}w@xft6bolkF<9e;F`jSyy;KbG(i(aE3CIilU~pGbR%-p!Vsv_~&+ zja!1KvVCkLdx+h|LaYxfVrleOx|_a9S1Lo{!`r~W=mF_A3LeKxWFDDJu7I>0Q@&Pe zm4}pnFYS?YX&cS#b&eq^4+kfNM<%RFSvah9ZmyXw*<|u9k@88dNZCM_vutvBGU>#( z?56F-pFDmgWsN8a@H=+X*8C>%Cv7)v#;^JW5`GZl!cB*~T+J-4hZLR;@Sq3vR{rEJ z+J^tQi-LjOMLY64chL^~I`QZ6#w;&)a#PJLssC=sfWi- z`M%F+zM*w$t(iR_-Y=YLHgBPoo^LM8@-@>34JK`)O(|)@2Yp7H6*kjMhlF(tgo_i( zr-q{wqkthXAzbVVtur=(N+6B-sZVJ>{{sFr|K}(8q4*?~KmI9g%PT&mg?#9zw0+ec z`aRCLaCZLOLt9BU5q`XVFUV6JHw@oLFU3353v0iiN5w1jve$k|zY;g#JBxI7oqJox z-#I|L=4C1mDDdBQ>Ma`I`mo zw&7urfN;?3a7JP=SJ>%Af6VU*1)>g5)bDk8ozYMv#EvwOpJ>G6vwLH1x5pa*z6eCz z?tsr7^2I`7yWQh=x*XnU(CuX>t&l)?Qlz#kyA0o8tdi#d&n?iMO%i00=B_(?2BkK3 zIYY6;dAPg}TPd2LFZfp=3_s5Sz-^A0S1P-Om>fWZn6R6_8jy zIf(V(Gadz9zHlH*v;2eEgQWT@b^>Sgw;^nU7!*d4*Rak)DO$&_l|Rs?TAinTHTV2|R+Q3a(AtLH4Mb{Km=`jsJuF{k4(2!OJ~3HcOOy(Y|him%b5&Kk{x1A{D9 zJDS~(2%%SZ<8Z)x-Z)86B!236*^#8~c;-bZIw(93b1K8=S?0Z`GOt8Wcs0fh_5}1G z7fNbpF!XAqzdG^`R+W6kQ|@Gsi5sal;0A|*p7Z3{3_S`Q?u=UiuP*nniTI^(!fNjU zp1(;^Zj$*FQbKMfOUZx9AGDO`A13|ynBUkZ-1{4Ia{uq_kA%qX7is7RxZ@=2-r4W< zgnW=$^hU#hm_O=t1YEI5z~guLK`b0F({?w6MeG|C@XTAaGG29(jpL>BAq-n|n7AR* zQIt%jKl~;AmDiM?Nf5ADPCf>mpz^>ijZ!dkPN*xPK0GHYp%0-e+%uyudyWhrzS ze0Le%@7T)9C@|q`^H^8yPJFcRJ6gtz=7YDkYpGVG({$+NYrYk_g&aGH@b~YBaQox= ztS`5n&`U(#eLe(;D!%Q?7Zp;s0n70bgf)3NE7$tMXKVP6arQKMi2nugmiVTMKLoK` zAtgJNbmblbO%^C8U`?|#d6=ds8);8^2R)+P!wVO(7*5 zWi|>uDC7jUFJeBjl8;%$#;f!t#RvYgS9=ZHJ-=8lH>Uw!>GHFvQO0jy%r2p_6?vPF zC}4v^YR&Hu9dx%y#ue?SqC0qw;=m|)s$p0SF_ zBpPOmREeD>BKf}Oxl*-T?n34yOzC&nhZm}M_-Ww1t}wN`CkIHH?x}EA-V#6$qg$ZZfIgHhD%(C#C5OB zHz-9Z8xzuoDXER@l1A9I2u3sB!ZAs8^cL%al+64NJodErfDnM8MCv`Z6On=uWb>y# zW~;?3bbY3O!q$l!v5>_F?UeKqHqJV}lU*%IDONV~wgOrHwF{L!8h)O-yWRj>wY%B- zFrRJtg3lQ&O?BX_KWELU)}C+woVCo?TGhLxQzry(@kYRn4NerEe9Nl6>^rKpu@3KL zu(oh46xmj~CK``OS?D`VO8UGrU9+ETtP(8o!+C!`o zT)C^iVI$zOPF^y;Wh3Ek(zoKF_2IX$-ayTp_{eNqnbrJzb}JLhSmn)tzc(Ib<>F@4 z4Q3f{eoWrnoC!BG6E}__q|DlLj9rVYQT7X}aOBSa{lYL9hHwi~7$;btye6RU{!f(` z^t-}-FD!#Q!#-!w=ZboK-l*H{i-1`VdcBU2&*_hXwPy!~bjdW2$L|i=L(Xs{7L5b~ zerF`)2|0W|7o?0l(QwG+afV|dd&I-OMTz`yzFBS+A0d|$zUy9ChF)-(q*)V=f;V1u ziVe_uLvq(`U3H4JfeCO~)Ba$vJf=0{z0a_+Hd^xx%`8axZDS_Q&|3WCKHqxq3}n54 ziE#8bpIOXH&aw)@RVW0tXIbCWh9uzWb7+6`Y+m8&*u;`vVMrvQwpAV0uBezhMIWoc z%p`JJzDUe&ceo?2K+p};almSmKN_`r++L5@;faCsVh;wKF>Q0ZTOyvqb{>b{14bg| zi8$ObXVeL9PBiR^xSaM#z~}clhtIwBT=1b#DD3mO?csnY1ZllBt6-|h%_909K@5RUj!kxWLWgdcT)M-MsAkUwUJSlJu& zdA(u3%L6LH<8Xz8H_sgl;8{|*LecX>if)kOJujwEmVPNa05O6YaYQ`saKI6=2kao6 zE{D_OhJ|1+{8MeHxL||Y;HDH6JxZ*M$Y`XZ1s0|r%81myhMoaoEi9|0Po{Uvbh?P! z+;6aA-pOn90<|w1`;jeG3|K|=4o_>P z%B&`u)>f*tjFVMFH3d3l+RCK4&WvO+k{Y+(tqU<1F(Zk?FZROw3GDp;@!lARx-|+@&<#^tY zkW5;5k2y`7ojqSkEF%*i-~pNweT`Rtphea^pL!kIbyTLiN>xajc2&{I#wEa-u4-p| zj};kzU#iL)Ch+I2n(nF(Utt7g_DWzX;*#24s;udgj`{~+-6!->Mdkq^Ts_7ZZ?~3v}k&)_udEO|kH%!?RGD6Lbi^TQ+^wost2@A9Ee|j4C{i2U4jYeVJ zzzNI_i3xwi6$-duO~dJl_yUlscY4Auw><<-J3E1Mdsz+`-|fHX-t;u^OPTVs0uK6* z^bC;lkUH!3cuRg|syR>)NX&UJyW8yqV`q2zW6q#I6mU4a-cVFjAUH#Qw{4*EP`Eut}}4+aEQLkuevwEH0_iN)+*cPJJPGZlJSvHnzx#mSn7rq4I*H=m~; zC;8Op0VeV`aFU&JNMr*SNZ3bn5jTNwhdTY*fwHtN~m3^0=FRmAoR%U~KK8q|W z_3FUC;UwF3w)mPX;W^SIKCX#0?U)mnefqigL`J3y+$=56VrmH zT(3S#wT}F`>(xsPt#u;zxb!`i%hemyu2d_rdfcGa`PA9G`%uY27<2wTR7IClvePiN zD+-gSO5|xFb-NfGX21@I)Gattv3OD&R{OThgue}A;!47`@y+QR+8L5nc8kZNdYSRLHj_mI*dZQX{glh=j+?f<>zl zQ&jYm@N;i}A=*>b`{4_jPnoKYMM7ov0bl=;JJjS-IoIw~S0SCS$c_Qt6P-o4pXDZZm!~t=H)#fa|f0lZ!fQ2~povmWQ8b}vv zKL=D*ST*;+D3ZhAJKI2(a?E||z2b=|{^QM_hC+*yxpF9|xN7IB{f)*76cTo(Uu?+i zO$m1oQKRCEhli;D6IVpw$!pY+ykNdM4xXUid33(|RP{pj63HJTMfu=jpvvtJs3Ob6 z53U5kG%KYZlj}5<>J?PiT*aDsCBJnUBryuB)M5%@w&R$QV~wg(=gRUBk<0sYg<79| zdsyw8*AZ6B$zaG!Pa&6pX3HbG@++)FY~sxyQCISh9#I=tXvGb(lo-2cCFecqaNd*l z=RIkd|CDgrp4X<$c~5+;&wJGJyeF;Bds4{fJgR;{Ayf@LrnUvCT2{Tl=tGbAnrqvp(8J^lg}08_s@2&|;|4#lULA#Bh_$}a zM{GqGsFhOgTZAgBpfgmW6*MT7%H@Y&Qqyg5TOO29LS61XCSh1Twezo8`d*Oyf%dK}nE*VmCnyHi1>JC)t15J}~S1RiU3a*{3#5!xd~u3fK!ZvpYUY70cN0q`bXm z_UElLSTjE9gle;9ey%Q)axHX3+x>Es=m-8!-iSPkReM0a25AQ4f(7O8P=iWS^V4_g z>vil``#pF#nW9AfoE3z!i*@eVF`Umut0Dc@@u>PGf9SC4;jh_k9eCL>HI2W2Saoyv z5%|;fnEEIuM^qP&h-bTxsU^~@o!m@?Y~sx7AJh&~L5oG0u0Kf8)TV|gL=bK|Pk%zo(?|(>U#VPr>LcXl5rK=0&Dk%4K*@Oip8@xU<8AgccZ{Sv z`1=bWJ8@&7_5^uZ>xn8TxpZdZ|00^miPtTr**JLd($x@BlEO4h%?Sf))iQ0QAO$k- z&go!&Z#1?2;%i)=ACRI^so(_VVRnkF`Hi$HBr@dhM1~$s)Hmr8W)ca*s>#rBw?zXi zhBeVdh8(*}&(c<4FIY^Hr8Q5%J24+W6lfwFD5#yq?`)}|A%@Au#ie8iHeS<8>j!>V zXT@s#Dj7(??cO7u9&a0M9g4x>4%!{h9 z>-lo+RvaF(*AO3AiyNJio~RT?2Y^N;E^QVxhLL1jTU=UyL>_~NmX~T9#T${z{7L?n zTPHSm*JKSSuJ(*Q0!!^W8n=$0p*@t3~E&J0Ctub5}>S7D!SI2kkL!DK0sFuq=K5aC!Hq6j`a{PkP&d$_uxv-v-f4f5yW=o91s<~4e zi*1pcy46hDhrL$w9k>M zqW$hwdJ#|ADAlo$ZmVV^XavMbaD}-^Lu-fg;du#AGB<0uAS9MPtc{zs5u$TcU}f8- z+1R^X`#=Byo&@%LOPi8JuU0%o%-9a?sl;bi%?|B)Y=Nq-co&)pIp;@rX&B4mH=B2B zJ#8RJO=(lYjXhdBSY)G3t-@#ZZ;33WF5joYhJ_s7X|Fby7w*$w*FsZqZEAhFSF;Nt zK+(_p7>3b*KN1otdul&Ql(hDn4`^~H63WY~hoSA*uchY2+*j@qcv5l*)rwqsuQ{Z3 zk@F@rbEEGxbpNpN_upyT65U#xzt=`cvn2>$c@)6h^o#a@po!3QRVTD(;J5YM3GG8k zf8lQ}{9PIy4E2gnY9iE?%Iu_uE8jSxJ@*2z_}@}=5bx5n8agII_gg=nO)wU&p_Nlz z2)a-Ku+dKck3c~9c2*73dtuMu9et8~$`9*0x;ALwYi#rC--nF_bMF>UhUe=4!fp`9T)kKlQ5f}(&2^bMY_4yICSs6Q zO$)s*@(FT7UK@$l=WX;45v<5k*0j}Sr6h`5eg_>n4x_c>9dwih$>T-3T#iJxw(K$d6^t?614uW& zQeT9F2ad98uGFuU6cqf`euzX4y^!NWBxLEsU87%>#1vLkuLsEehw7+x(Te;qR2M#p z^yq4a>DM4oC{&YdEjs^kgq(OxA8Z_{-zedUSH&?TozQ68lgNUV)^9*4=sy&X(?tP@ z-2V1ib1><-;}hg+u3u;UJzj5$@Nf~fWQvZq?BEm$PujGp`cOG1pkjFGbRFY3z@|Z$ zGj!NZspVNm?$Bds|3C{$NWxM4F&F-nSLkvx8mxs{H5Iyv?J&;SJ6qp@L;!BIHr=D| z7Z{<)tYT0|^m!a#BZJf~(7%#&7e3TWi}l~dhk}$)3oKvZhlUGQ_4Ym>*;Qndtxc8s z4>(e^xtmw$N5q$66S%eMVZEPf=}%~4|yv=T+CfSfVu!Hbs zm++nc(O=;!B5DcLKM;b3Zs7<~USa1)p4PjPoA?s&u0#>VbNuFKbg!&E@aSYLw*6tc z2%a9TepWw#IDzKC377fCwfZR3pTbdnunJi3rVaYti0vA^ebvjlDogC@clZEGKIsm= zS4cgH1fHv`xPfLlvRtyiv%2^?ye{RM+0t6;$S!cjXgI!D-Wt z4%XcF^sbHSdf2^7|47^j(YI=L!<3jz`ADJ1$N`0KZD(v#XE%zc^T1tZA-7eNE~0Ni zp%WM0G-lf^Gcj+E$IMCPS6?O)C(SdW(}E&xV#L$V&BkQ#Q4hc_(l!?9ED%BEGUV|K zZi-9wO6bcMoS&BwKy0Fe%y=q)cO4J|wz~QRL}7G$Ufk;W`AYz5eqEQi5ATunY76R< z`w#ir{}8kKD}4vfG%T!qCb~o2w()x%w=c=^iOom#-QpEG6g9_y$W7A72<4n=zGb{{ zly=F6!rrL_nTmHgp>LO=!|LOL$H!WyUe+HY)xYX;yBHQ`Z2r9=$$ogwK^>&{ zv@{U1=QH}J_3x|y)(<2bTN}^n`(?}~_AqZwu{FTAk*yR5h#h7$u}OcWi4DCb1iUfJ zwyVC0)!bGGm;-{ntMhGh>-&ANr45w;Zi=pLW&5c96`*uEKh?%2g5m_R+S**GvY?;C z;wc1f?W>DzbJLSe_={c%PY%&%lfe=OcwM^Ku!=Q#lgWSWVq2N~5)75CQ+?-Ym)gEc zzT#QkZQJX*yUeztzS)+Zwq40)R!uM4-ui*pTw&WQUdfRSuzgwoYRiy@UtQCXT(1tb z?XL$o%=UGCUvG}Ep_d|uHq!P*{i{tO+n)Nb!ZwdB*)z`@Z^L*JNj1T?B8f$NzIBQX zSrp*4hkfq0&+FmUOqaOJu4dRis^^B6X4}50173TNt*K6DTgo4wW6O6f zhJBe!NF2_~xC`oKCa{4})c+EDnhjy|NQ_N}>d4#46n2=MW-qW`NfmpQjDob@^vmci z@c@7t7jFs>$ITbNd&k>?25%ZKgx|6G`S5$g{5<&EYd*B0!TGSfLT`vW;Yr*1rnT~3 z+cTtMOk9KSC&r;{kGvX(c$8i-FBAR_oCjYj_IUtB@y`b%N*b z&g%+)Z;b<7l0F}P((B>{aM?AlJN&+BUQhTtHr@#S-W=}?ZA#;!{BCsI3%@(h8v!l< z#owN5Yuohlcn7$toi_tsPl%Vp-|OQg)$?pJJ0vK}YvK*byZ~;JlgMipH+*%!t$Y2e z+?lpw-v0rc3>8HUR(_>Th8jW~9<=SLpRKbgGl zwC!NsH_tXes_J>*ktDDeY~{#Gz}|l)AM)!q*ybc}s$aH^s|UKI8u{9SIIajqSoF&` zy>649qPQ8kvHqsI##WF7-=6n=L&_;8b})^Tu?>_6pduM`n@vZU$so}NX=@vX1JAu> z8)m6<=%1|meVbiCM0<7lCpKKq$0*~ceYPgt+-XDCs1Ahn`c9kl1TkIy=?9R%>A%ai zO!SIg-KjWK9JJVN!{87Pw5Z)}lU`aK5PseJFjq78+Au#_|M-%9_5EAWzf_3m#K-qb zi1nyldcZbT^a}1CgqH_w82zI6aOrn8TzNz9;LLZDBe1@(_Iq33lnhvy0x^VRYK{M*DiAm z=_`xH!rEM;ZxgY9Qywe<8NV~MRN2O}^Nbg9^Jb~v3v2d1msb=&MZGb9SQL9f1p(B{ zLrHzi17-4lH&hh=hMs0)@B*!pW`6^410>^m_4L}u+_c6%eb_f{h)*}I9X)2CYIKQ8jLVkE`AEQD%Ioc;NFFLFk*Y-7F zNjgcBJh4CX=oRw43?v5_GDD0(+ogk%W;p6IgALS$a`vw@gkpu5IUP2eF zY}Jf6P9cSGLf@Hapr;^-GbxEBkdpVGVywb}3X0Er8bXZXsgmN7f2JD!R8`dG@RZK% zRhVWkU(!!)#BcixYKMwT*%#DO|78?mU!`{$Qci^^|8HUI5)SPj1Oo-l!`GN^E83E#1Z?lFy!*&k~%Jd9zx^i;tuUrLh7wOrLw)Qg9aR&$*=vBqYg7e^p7Ok#B0a(GL{kZ;IGN z099nE4;kNM-(|1~;GznH*!L*Z>n4Cu4_j`0s$xfvNfMwo%ez;Q;BiR;!Fg8Aw4X#uO3UM!o9g}&?q^IdI`UU-fZem_u_JZLs<8ZFrTX447li*zy=3Y1(pJhGy zf>A(Q$_i1h}XR`R)#YJI)VAVk#2Jhik(9#476Xe9ppyIG!QmK#*=g)9>l z-czGfrbnk}0qcpEj74bH%d3rK+6`E+Iw8cs+g6Qbpf!=k;Pv`9{Os#er%D;#Xly|m zqu72r8}jb2ZZg7R;*u@}8h>#z8f>FY=pm8p5eu-Ohu%ocu2{9M-DdoZB8v)o+q(w3 z!y7=~Y9vhC(i43Vki z1v?WmhfQ`Gc@n75P0e>1uZt$w`H@|+y;K3ajX@HWC^2lg7u7uK$76e?{S*VTYW5kp zGaX6R@@okIdtbE*=#qEH5Vb_O-fh(!GOkdK&0@Dm3fXL>A2wblB#P$|@wDz{8r4+( z{Sl$^ZsnIAHC&Wk$0uK}wWmro-_e*gwbmXrp1>&trvy`ANNyN(7VmuAK-W)5gthUw zv0f)vTDSaeWN4(E|ND%Qr)y#rc@ggJ;~UNxUQ%HlJ!5PaV+7>0`HU|illH^kMoi#a z5BQF=#)#w-$OIc#V>adL+n^jOSA1~?F;QWoD5g>q{m3L6Wb+!RAs3$`Njfr9WNb0* zwUL=}&3paZ`aPL)_cMY|i3s>WdJ2A!>pTqI1}VPHSqB*8mK^ z`yCzNIQdsuW}!AaE#5khKbZoKkYQ6VN0dSL_qxcMVnzY-ZG|(BPl<#kO@VXpCT3*H zzMyWhLDIy>_uJGW16))%W$^@s@__HvS$D?Uagu2cu0#K`doNf&Q~H$zdFOkH)DIj3 zB?fRX)68dg#oKdwgLxGIK_;JmVHjyIN*J#v%6dd)Bl9}Tx&3-zl}(>BT`QSwi+8Bp z|5QF-_BqSq9j-Tv5HnFNEJ{`{P<@04FYW1a$tj6hBIULJdb6-P+muB%$auLXYye%J zV+t8SAuY}|(PftD$6PbFUgnQ9GckTZ?KQW#d5=H>Z(6lTyb<+Tn_8HaV*kn3_F$Q2 z<(s3V?=H%QjYCj2T+qfuzJSOJSGzp|3^M+fHs%0?DH6JcZOwUg9n`ip`=(MlLSeVi z5%905Pws2o(9X<*?e}naR*z4~Q^4W<_=pa&WEmZ;A)R1wr#hJHl0dE6A`|2gkP9w{ zV%r|Ho(B%fM_i26ziPB=Z4ZA&}=fJE=y&*np{wO)vzT4g%3Y#Od_9La>o3=!cV5 zV1tGy5*a=>+z@@&1-9o_TyTHx#}a=yy9Exs@wq(?*iYfI!_JR@KM?XpT|p15*?VEP zN~j_Hz(t;3lbI&LLmj%SB4YPBf=<}P1DmQMVc0DZ2u7W-dnFif!FG&bL-?MH+^nVM zOZc$S!|V0IekC}wz~K)?;0G|L+aC^kys(!kIDGCXy`sThCzr?VfvqD>I0e=3j=ACF z9_TR&a~F*TA`ZJ9Hb4OwzprzNgb({>VDneV5BqrF`SJngXnuBxJZuoXq}{pT!Y#VSoXU@1olsIDP;n0iB{hGDIlX$I*$;Q| zExrO2(Ct^63sY6aE^1E?D0UJMuCq8NFQ1=DH(T?==}_Wdm~OVm9z{@;2m8J+U86;d1u6NYzmfKDn!)A{Wu?`1XQ&^n7l9FaywoYS!kwcngt!6pXRb`{RX% z*mPDM47BX&*Qog>m{7SnfM2$S)a=6q>H!MA65N&oryeN-v=>{j~weqMq zcfy73b+HFgG3fTTA)6MCOS%}EJbY4ET|^{fD3s{>_{Ux~hv#|K%$%COU@K?szf69f$87Nu^Z;DfLAmh6YZD#-tdH?m=UmV?amm`= zZugF_CoIf+@pWda1oxIg)|mESLx6i*5p#CqxIGUJd1=jm_NsaOyCRmZilWx4`_v9* zqPi8B8wupQ$SOz;2(2JvXX^5w8x$NfpR1gw-KRE7*3g!K>*tWJADh)v z;diG)=`66i#_x=)?F6ezBra}iB|8rZZS%QEh}AO{cA%E?yW?uBypmZR8JAIE^GZTYzAgfuipevgpg&;q z@2DfTGX7}Pw8OIx;Tc>?V|ZC8FUMkLt!UUepuvXU(dOL`9rxNvf_(9-g zro}sP`(*Pv**kdm=k2Jx;GUQB1Ckz}+{GvR*a~>Sf z)YiPR!7il&s?X}9h&@D!W19YXW)shtY8GVv;~*zsL*XO$y4W**2;g_?>}Pv^laz(F zGI;3rR6-V(m|A{2Z=iv${XfvL&Q3M25;JK*G-*mM5hiEybW<*n3av1FhWTuEBl2Gb zs_Gz-D$eK=XH_r~Xo>Y#wGQR9 z|Bl1q8#fAU+n$6g@6(|KWL|EaMrGSlHSir>%(Tlb_`1LM3|=(fH8C zCPw3;OsMBYp5#;qI%z4{Tjye!(7}M>-HXjufL(Fi%LiNsM=MXg-)wea9NY1a7O1cV z(awLm-^?+X@|;4yCU5ekH?c?6*|~8|oEjZMS_+AH5ca@a;K1lc(gDd|+u*?HCR3xq zDd92KPKm-H*Cn%?$H5@Zx)j`mGnazqNW}9eF1)A4l+H;0*8Bq zAvw((K!$P1ZKJpbQ3mAED0V zk=Pw*JSA~}_cNrcm=6?#S0fknTk`r`5RN>@`xye~ctbIbDT-;(Efk4Wo_paPQ<_hL z8rDdnlq{1$Sa7O&ql6=rrof?aok$u8;aLr4r)mA{r1E{GklTl?Jp5K4EtN-83h4xn z>UDvD@PhH+d0T1O$B0^wM0x`pX$;Dvz0IIB$g zMu#NnJ>kMDA7@QgvMZe4T4;)&)sW-Kge*0?Uu*@yNDB`4~<} zog<2&6&$ly&=j1`b0}E?r>N)RDe8Z&p@jlbIhW^t3dg8}ZU%o9*d-hENkKd(5yqUJ zW=f9b4?PCzs@6*j5@)LdC=AptGDmX0*Q|;&5I%94@{dF8WjFtL z2we&t-0Na%5e%Tgu6?6toJ~p-U4c~Tn{o`L2{jd*e3uZ6fAm%|3re%{V5u$*qLT*G z#_wwn(Hh_)O09ce^@WGuYz#UA*9`64p$)R3_2i-8-N3}AvX!fD8_;z-|)H9 z8-f?d%AK=ArsDtDB9k#l!jHgdL?Ot@!Fh~u6eFD111G)sf=`^a&3dOtdXP3(uN7PyLyvXA#>$FtTJMKPHa$)lY zeEl25pAW9Pl)YYqg5#qD$4;Gw|ECfqvvT5H`QjC3f#CcXSD5WmSe|)*%vdvvWyUtvP{`N|NtWzmZ-_!FnxXg*#>ZR~l?tf{ zHP14iwNzA+LdcTHQe>C)Kj+-2F_la;e z58A7+q42k^QyZs+|5b&%q%}`ouFLs9YU{@=%4bi1C-{4atn;ne2JyTEyrpuiN)uZH|~dC6LtK9$?rc}RsV zbmG;|T}3ID;yd_$_aM;sT7N(&*Mv3tdM&*I?Y;_kqt*^uc3TtXCbW-Rv0ArB`+g4D2#35|^!osly+t)K=?U*mdB5iyZzF!}Fvev(QGz~|zl^rXSe8YcynVuq{ z?)jha0P95?a|~a|=}cew z7L|P?PFGy}+3B=!A~G`0jcX(vNUgQ&uaDF_j>6I8=DSF`<2Scs-;SLnY{IQfd=BJ9 zCQ&s}xUSw^MZbZO)~>&oBJeo~pt{bPegK`^je?xw1& zq6p{zWQ`d0Y~SGnhYZh5PEo8gOI7h{Y&xL;q;(6@SX06t)r5+u%@jVW3Ee*Wa9hu*nmugpZX} zBR^r5+rB0HlP0H89on!Z)EjnInQiqAp;gcp*xGS>v}0FDBXsP}Py&%|N_Ak#szDEy z6~N0a=)ocgZImLFd733DyBF(0kWpv8*o(b_Hzf0+z4amhB9N>%t4in))$jJx)I=?; z3i`9#fWi6!Y`9qn>FtS5b=7znOHlruR)m^2l-0&($foMop^P*cioW~dFgA%$Lb|H= zU$T?b;4EzVM`d?$q7@s#mJ(DV!sZwxI3q@UD&PW8c$`nU^-F{?mG4p(dvUqpo zA$hDSpiZl8lc)Ld&pfsfmxDT3`4tR;<$Mm7ef6CB(v|aBB!f+kUBF7=wdO*0gaf3A zMa&PE?-#LN-0r6OG_X@u&&4baZ*sSQKy8E@=x}Z^C}CT*d7EvnG|Sj-TxV4o=s5Nj zyT#LZKc2_G;dew0F+j{0-x@t&+B7x0JP%RsWGC4Yduxj!T&+GVQr$P+0ig^J1LTCU z#Na2{I%5fRpKInsdNbqH?1MmXdDLnh+?u@4tt9naq8$|?Mi}z@es8}xcD;JEi-=bx z-*n1|TJ>tH<1aWd#fu1?l9S>cpq9Pk^g|fP1#15Zo)pjbgSmhCa^_PpSubSvAN+jg zsM1?B#0CvC#MzD+ui7O!aq9bboML85POP`T`s6JqNgY|@6jPohP8yOO)$8hy7(m=* z$XyLT7<#GyfPN!JfiB95EBhcsUNfCgL?B`L%h`{NJKfZGOMtq31z0rLjTLMx?#hQB zv6!HhrOhN^xVi$weBCDuoGVz)O46~U9&WB=D-oXxccay&Rjj35%J{^9f8VNzPubI8 zZd$9-NxZl!_>{G?*fKH< z-goTR1g2qD9yXwT=JS112dCtOcn8LT45v7c@lu9SGQt>QaH|5hvL7~7$2ow1@gr6> z#GiE9PfJ)*S>Di)8=TsQuCE+6d`Q-yOxVz@(CvDzr3+{-wd#mfMx@uPp_kBS4l~_5 zE|U_)t%@=WSc|BKL__g}7${yAbHsA7N&FwKwl3B zuD=GZM*`QQf$Oor^?2a=Ti|+vu95QhFu_2FJY=wk`D3Dw@lfug(Gs?5xcak`(+hb7 z4gS890`};-A-_9aJ*(8w;2y=CpcRgn-Lw?Evl!gqh^5R?CptR`GRO1|QMb0SrwVB` zxIMywQQx(F<$Sq%K{&Mn(gb^ryP`Iv0-U2SIB}t)`}fU)Ey=F$9qOxaq=M?Xm6a#X z0R!UPfZ0Q;zB_L>I^mVnmZzKqUYE#Q(o6xO_6EM|A+zJX!_<@y9ojK8JpUed^+P8i zqHc|}=SRJeH7spNR{!BI49IlvZD;36{gc__U$GBpUbNW-U$I0oE5yIozh~&cE0j3b zzneWvqpJPLwo)tDAvN|Jki(R{Y(im6!CuxXGQl`$7$;dtRu?>w=h<6qlZyL^<%9T@ zQm?LIvA*_);+94K%)nsWbz}Ci1c~EoPP3y`>x1k)yeZKObasO(R-#Hi#9oCK@|ygI z*g1i!qHlZ9n+J}v(PW<6pJdx;&?$Y*J;I~anN!*{k`XTWgSCn2W*Bc6TaBwMg+0S^ z*_Z4eUft-X;wOt%s{S=mN9~^?QY7i+^83tG&MUZEuZVHXefFC0vHU+71Tjbt^bl5_ zXTQ=7&|PH>b*j7HUtm}1IZb%r67%D8G{x@5%V_WecWTNt1O#v}8cM&8ZW1gWLK4@1 zo%P3lq{`B7YBvxZuCK5SRIKa2#WG~aB*SaKzqgIgK@lA{ZnIcci?wHi*(A1zxoi(R z2cbYYp3d9x{(L;o<;(eIzK{PUqyXz6T8lnnte7c25CtHIN5xs;%e_%YB`gHJIQy1u za(OBsOMe8ervukBsxjlGU5oLXJT$~~qHI2TRDYS4FZmI|8$@c}e3_(5d-xkcoC`d> zvf%QpIuXLxaCwwetov~&Z$TuX=*bM{JCF+Bh4VvrJx(5EaSp*5(gxkA4v}0tO-S$y zdqoLVCQ46&?zBIOgD_%Wbi|Wmc%fIMZpCnjz5pgUPO-d)g?BJ*v`Q_@KgXL2;bx4w zRaW;S7t~*lXB3Mxia16^#EUY%63MA*%N8?K?RTvd^~y9+$9>~7tEtF;hz}yLWSw5F zLWoiHs>)U6UYG+d5;*C6IAKZpwP`BQ17;^Vw9@zzywS6#@r>xY5Cy`&Z;YReqsCR^ zEHhZBDqVxuS5Ma9?Ld|)s$;*~minRwFV#R!FWQcoML#bT{oGLW^JdY{J8n!({w*sh zZ$+9;CECt39YTzjx83WtI7Ls|c}}m(F?>P(M~2i6sH2s@8x5h8u`cgvo?<>NW{a;u?Ur-fKh491^7Y{Lxbc&k`I)Sv zzV5-B#Pu|co`FfUBN&)O#UJBM)U2;PA#VCJJd3OSy?H>(+}golf=TL`J92k^A09~* zLbH_Ak5)m{s#^i5RjR+H7U~|?-=BBU(H8`=Dy}0u?~tXRQgS zn0_W>Rl;Q67+9w+d7|$8$$YTLf1U%$1DjT64Bv)?)wCQFpe5<1vAlgya-vkZ37Y1_ z@B7brV7`g>V>ppFrVT=&Q^j-mP6SFf=W}RNDNMV; zmd!q5yV+$P%bW5ce5R^6l`jFr+e5a%a-+QbGMNmTzRb*`Em!ql2Wo`P;&0L_Fs}8_ z3Ls8ARW6SZC$j9X4Q^~HrRhXkMCq=?ape~CbqItc^3TPbQUIF%Z*%bS@DavYsY^Iz z6^Iw(a&fcX)1tes9`7TX^IzPE zk9jP!MV_%6lBDUtd;`_JPdJ$i#6wM4$>$MWgDNp(P!5ZAQtUELx^H)0oXS|?R0wt` z`G8}d-5|$MWjl5q15*3< zXT=|QHeyKD^ZHpBL^+Kha4)_Iq=`kSFo0Dq&UDi5hxNkqLxv-uka*Ln2c1W3=Ovr4 zND#_O@c)1P@Bf+}G#qy-n9idBqt_=+Xbj4Y zRgdoDG3xCI5ywWV!$0#l)ozGWjLYG8lPFV6r=vRdGx&7x{LDj&HShZ@U@EizuQ^hU z#Bpt)zg5|ETEp~>f8=F3(NiReSnwKOoPsEje{VFg>2FD5N|?vq)=8J=4bH{IjpOG<;U;_98k=OYxV{2 zFjakXls76LiHxxga=2iHjP7KIzy$@%I1h3Kw~d*qe0`=>v#OJBrz=lAyN-%W04
fw6!7x}+~UBX#Lx}LUb%o)B6Z%AGFPJw`QJIi0d7LWo`za$!4 z&hgn0R(Jf9zXaj6`tC38hui7rgO14rGg^gRLFOqPAS;Eg*vm zze?TU&lef0T{rkFhsJ#m_>JU313_OeP6$#(I!Y0u38?{8Vk*KT=Hno(e>|cEJ)#s+ zY^Xpr6;K|b0#XZdx*8uY%5gc(eKTAPVWO(>y7EMb24x|Y^DY04-w@&AKjK)>%gMyM z?&1hx1Dt8@@<{QHhrr`9pD8UC7Ty#qBi;&5to&s}Yh|soiiZ`k!5z&8!`19oGE}V_ z0God+RTNWWUW3GEUO6#T{WUY_pd9ZfQ`OaK;srG?3nHZ()x}|Dr-?7~%ZuxzK8Zt% zRSHar$oMOXuW|k<4~gILN(wf+vY<$hRNK?-V9KmSfh>S#(tRdLz!3`cr7fyjMUW+= zzPGE0j|1J5zp7|0w*h}Edz`&aqd#XxFrdo1!&BYl7wyB`l{G{>SpxEc)9c;`uUDwdIm z417$G3=lycYblllvA*4sRuD*_W{7q&_L`0lK~QI6jA(S%BC5(B;|+BNuU-d{uHI@7 z5hKL?DXM!rF-olt1+pcz7mupT3jvMm@XH1bjrH)CwT8<@*z>+@0o%hJ{wRNuujXe&1+dB5>CaUgr-}yV zZK4h41)YM{H!WJLkt9+IfApxs{3SK$AkgK5>?|30cSUA@Ec+nPH)u$ z<>p(lKh-P;2k_ylMyy1%jP*2YiA+d$kDDnT=8&ALQ4TZ>Jo;LFmnLdsr*Em#?gT5< z?K(@0fdo#D+C5uT3YFQFOy@B&>?&=n6YECjif`cYHTUQo@d9xO$=;AWt*&XW!MpSec=5_Yk*MmuEuKRz&bmhY!7*aXfDvTY)w&?;|a&lP|^g7g;UWzh8U{J9|}4|3$#* z&JDHV7Xg0W^>T0+1TI$1PBJcDUMyugHMO5NdU)Sw>kdexj3@YQf9D+(r?|Z0o<1lb zwJ$H_9Tuy=2l@1{ID{Gh8Are$4pM04@qj`>yT9*OpzO5YNK27I3qMiBKgxdsl5~#h z_`B!@mrwPj_({s$2RG@f;-+(@Fu)Z#*l~py#=>q8BC@ z)4G6^zj#ih;Z4m>46=>q#FNAysh3FG5}*EkNl=go{stU4Tox_ki$ks^m|kQlh2GsA zU9yONRQpiXF+DgBD3AQ}=m)IV5;Ab;K}^us`_)aCm-@ zaF+0#y4X=Z9o(t8M=%5axgo!pzVmR?X~7J5@oy-T{_605Fauss)NN2(9qJ^@%j}8X zQL5&vkPk1@S=RaYDM!Rfk($5@^WZ^f#`H<|gNs+gFY@?Gg(+@GMy3V14VkV5#0e=m zVcyYk>fI0-t?c5?WB;CM_3r}CQy!S9xKlV$@MJ9>eDfp4og~+^WmA%2BxSb{N!E}K zQ;Ua6Qa-5apngDR*u@;4X25x1e{dox*Yxb#@-5adoqu`j7^L6x<76A+D=aEeeacFO z(%mu2DXGHCN%9=E16xjN!6)CgSS3k*q|O~wlC6Ll-cX*(vMrZ0-6t!{MWhKXSCN$A z)V8{+e1&Er-|g{Q@&lZSl#f$ezKu7emi@J5D;sK0_-STwRieIC8u;O8Y))A(i*qvI z_^9)J$4BKKK9Z zH&zeU4`WDcuKSZGxl`-OE~GAr%lDzcm1-o(!@wMjThK^Wqvv!;e7A|j{H68=3z`DI z!X2q(@HIrckHm&0313}c{gw7S<)L!k-$88SmgM?!88{>?VNC^{!r z(NFRgE*uGu6guBn9XyK&;~pLab6&^bb>PUXy^1&{hI4*kZHX48nL1m*7pn-b%tS25 zdgb4EMG8SOTK3L=Qept|mkN1GF5>d0`tT|Fuq`hK5&*foM68lsWg{;CaUbm}3r%CR zYT8|+Ob*_i?(!pqtMTt48xhms%2ilDxg+>Uo$e>M(EOO>^Y@n-Nt9|WItr%@sL_Mt zVfapd7tS6s=`dWCxFKt3S9;7zk*ul>l*QDPrB)S9hZ()lf2!fm8X(6qx8XoJn5lh( zWGbudnuFylaEx)@=&n`_ljYUAVKNf`Jft?gB_p_;=$;=YA?ii!K?c>hEG>{kB1rB; zss$XOI87;w84#Xp)DgQ>}Nw~-q znJ>xrh!%8M_L95^`*1;hIzfVkJg;_7kYiCZaR?3I8GEWt{W6Rn@$21)DO%Wn=mADz|8#Kl?)7k>&TfXRs~Hl*|G6`PtmEcLj2PANC7|RT zpH^k4CSAWzw&9j+mTWqYtLNqcjPiLhnak#?<~cjY?UN^`$iRZ4)a`{tN0N8{n=&IN z#rVO1TDv}M6nhwQHgTX6uNc3n__rkHqQT$;&rOv7lrUjMcoG&cKJvx`E!Q7tx%NQI z)dyOxJkWCaftJdDKhSjXftE{Z@*;Vd%WTzev8)4@!kxcZjwHQDPIK8Ma%+HqU%}N~ z@t({g8kb%nchLhB@tZ5;O1RO1`A2d!1*xc4ef#p0)QV3erVN(7zEbK3$j_Afq9Rhr z9y#j5Dj93-F!&XANUhE^qk)P`WljliKXqXV9m?xivD^X(`)c)Oks+o{X86UBz(G6} zxYSJ#RFAy!DGPy;l#<@Q>Z?y>A3_v^#F%HDqy&Br*r>;h3zfes^d7Ps6SQ%M#%a!^ z(u+Fvblt)at3eRGeblkla$PMXB5NBQh6}p|H>F0PcS5QEEY%bp1zV zXQBsLsmiA$l~xix&YzMSX!~*1{y$_!A|Bq7A0EgLV0OTG1M&k~S&FX80!O`@D!+~e z%Ch=~noySYF$K7(N5Bccc1DJ)`e$Wr$QZ~vYS>x$^ZqQSr#4a}7eKs_;chuCL7%Rx z0ViNp&dMCfrDsW zH!qRM1#&HyWjW0^NU)1Hn{qQYQe52bmh{r}D2eB8$&&f+vsXzr z3HG1_@LLK#&{Yy-AZIVtMGNEwu6qlF(7TMi^tm*3zB`z~^{AJbIS(NhwY24?#u`~Q zM3@*VQ@mOr%sz-TG0@&IU}2wjOsEzBs$e|aHpJvwU7)(|7i#*65)esaMvDy8@8JZk za5CF1C~js_x1#xbM4FgUqFKVDOmdrXl~5UAmJY$`XiTajN+`;ex)@fmCx3+_)d5na|N`h%-9XgT%wrwKRQ6QfoH=qNUU} zv!OGrFWTKw+iXosBGcD$ua{d6b3@wi~w#GnnE6sxXfBz()SoJ}At5GYu1%u3hh~e`F zLrqd0M3~t2u+EfW<^&i{rmbL@IiQdV``?4}!k$^?R0K{N^iLLX3v`8@7-6=Mpa6b1 zRI%sHvQiun=%_DXk~c9b9;hyB zq8LC0%q%m-?J>gy?bkU?Cc0m|VMcMa+$&P@XPY{|Pv8fY0yXk6OzfO$yk9R0iF$${enzz+GsZ zZ&GwmJAWzPB-4r~NbVNbtWD6bt~Y-{`jM<&c^zg6*{Bs3a=Kfv5v2p^i!PM2$t0ys zH|cAjv$NTpfsZjm=Wg6=_QJ_Ym{WIXm3nZ8`5pCW1{E^?ugp(-wzCP z3wE1NQ4eB`qo>svb0_Fhliy z(^FE{gryxaE%(=hW)0*fQVnr?0P)sGh;&5D8%KyWwVB#{)XdVniOJb@r=jnV-vW9E z4dI|O;8vuZAc52A+=3HkLro);&d^gPc_d^t|2SoC4@~QB{=@90RXJ{r?<@fBcFCMW z%L5ggciCJ9zugZno8N0_=nTxbrd2K(xa8|526tM_ubXswLMuA(8UUMiS92)|O!a#v z1;ME2+`sMxScx*cY1~4Ip*VpH>jOjpT@trISfDN84$S(G!B%zJvdAWry({oo?TXA% zLZr1G`vLDnt3M+xl6d_-N0bGOhmX*BjB`K>EsU{Vrxs-A{n*ko)_R$yAsl0^6vh%;#a<0@*jeyL~yxwQPlYbjP+4Fo5NPqc`~00*eTNVJIi zHCHEDI$2JP?8XB!ty3+MJvu)6JJmu>M15 zVSbuopW-WAD-ZxvXYd0OOSl6xA+cYz&>uuJTcthqq?n@Y1Y`aD7 zKl&P*wp)*r`;Uii>2u(Wy!AKMiNgM#Mxb{G@6;`nzcR|-eZe_g1{DO&q>`XuP5KQ;ZAPqPk7zF|_BEi`%?>imO>ZyX zX3BnC4pJF0;NI?hT-H@#P=Ty^U9^74zhZ$OgEXR^1t77^;HPOGL_i${92cM<6!A4iXl^6V?> z;+FOtEktGWHLFE_yk|<$p-M|KU4i>GxppEAsKGMr;>-n|l(Uwm4kLr9s z&DM4I02AqX;cZ9HZ$)!`(aS?_hF)54&*7p+oBDYU6piZdN%sV2R&m2T6eY%-ewXR+f%JF!Nz6jQ}g(TNR*YMD{+V!~;$PF-v) zx`7B+epswCERyc$ya1GO?`Zh@h_?a2ALxa*+Syuf9QG9hhk7%iPbF_Cv^?Q`2L3+dod7M> zruKrTFRQPocq)~A$~zuz{8PumXi+86Kh=+FRVVXo&u(N^7E>^X!mY3twd+$Z-pmuShPC+{O$1*g8T z)N{OW%<}t?%KHczBmnGVPa5$N@b;Zxqw2EOGdXyZ|CwiK5zyKB#MCatT}dRrIIr5Y z!81PCU-jNtbdz7;NeF_ktXgdIEWvI?DTw0RQH=y;WV^>An8BdV19?ynIrATW<>>)u zaZo>mO1JPuAW*sC>sT{=BK7kbk<7&yxBEywhN*F3;9gZ3#aAeMG`|r@MO3s>InT)d z)>8wKB8&XkkDj*zI7O@ThdhzW+3O+as1OTx!(NZ}1u0YV(|3>%YQ4`h4~LOocVimp z-ZJ|=6c5rX%KrVH?nJagAgar^u-z{n^muVtQS-xxispA=eyvr>CQmz}AyNcHs5Lq2 zf$RYg5ULg+tLmYEmn?siQ=aAY1TFimQ<|x${KfwV2zEG2Hb8v8P~oRNrwYS#x7Hbt z2aUWgChKtCv$PO~b3tos?L}PhQ1D7qtvd$mA9~HBeHe5r?p@Q9LjY9L-E`eEf|j0k z)1%#Kw9of$>MlB|bIUWDcpL6qobjw>Qv^kE!bXoxXJiy*3WA#`&Ks@mgeZ4~RZtwr6`g<3YnZ~zgouWe_<(TVLW z@<8y_nBZG+Jyd)<0dK8fre{z|Sw+{Tw0jfM;$Q#97pVm&K)Z(*7NVwZ6HpUZ66#VG zm6Sdfj;9~R;jD}$v!0kd-C};KxdnahK@Fg^Ik6LjO^FTcX6{=J?A&5R4vgx5t`5G< zke2pDY&qL9upJhhdHOT#i$%1F3xj@V+vw*yE^KQ*OV23?Ytn(pN`$%9!6xOXH~n#Y zxCVm}ZsSh+5%QC|+sR%*h)~|Rvptju2%HN^koWd<No8dG>gKg zW>m&ddszS$Z1*s|-N?Ld!7%#<0jBN#Cd($5LDOYK5J8}#YCXz+uMpdPnSfyOXiaR* zU8C(*GCT}+4aFQQD_Kt!Kf`RgwtS0FUA3YHCP4M8iWhs_w-Uf&=f7-gFRA(jTMH`6 z@|_8qMii3yC)%U5pia{pM&$V^2gx(NkUX%8=`ed@2~f_uMxMz7j14Xdn4uA5_T?$%Q`)X4`wzzB6W$`(>^@4wQE;Pjr>fUdaDlpVBuQIQiK; z6s3S1-pSJl6ZaG-yA>$A6gojt;q&c33YDG8AMPZ(Jr>wMNgBI2up-n>aj)Tu-qtHZ zmU0W;wlC4X5aaA4rd>iK+z;QgU!zBqFln)rb`v%|THRf0e+aPM50}|b%j$fSk!HMW zi1XsG*eTY5r!-gOh>@b7cv3tD#kWZ!PB{D~KgSRA?^KPC?V6znjjxQ)U<~{RWU8c= zy$Q*9OgOvCKDJ|dS&~lp6ZoHgB_r^DFubUFc%?lEv(k^Pvi;?6!*_$gTiIaz4sT}F z0~ImVc%OaBzA)yppN)y^gsS+doyX-p)oZmKueSVR7ju96)Ml)de#4~GV~@6I|5z#( z|BcQZH70YEbkyzD_VnB*Oja@X(I|!QFuF_E*i*=4r{&x03)%Bden1{UvfKjKCVQaO z`G%q=>fQ#eLA5w;v>^zAO%1RZB%jx9vinm@;KYKgvgY>9!QH{UECW& zB*7z1{L^o6IC*xb-fBuu6zsCc>3I<9U%Nql^!(m_2j_t-d`VTWy{xS2@766khq&AB zK_e&)_RNp=ChVNBHy|{W!rN=dX`skWrT5tzunS>uW}oh_MS8#8UW3AN&9VncsgtTZ zcTihEN-Px|vfJYth-77d(*S7p%SV7w<4)MK2tKO2f)jQdY5#%6#UcDTH~h5y8GIbe zlDvjd4$d*unKO20F1n~DXYJZp5}Zlj@=$mJZ^_@FAz$r|5GCDJXYF@rC&6i;B$dt< zA$_V{u*tilRO!YG_G*iDgjcKW;wEdZUb$t*SrT3wH{PI^Db(6qc3qa^p1o!7z@@?( zqSe@6!5RDWu00UZE&@FMp4}_h0;yWNBV{T&z5wNPu{dHpb4Ue~3>M=Kxr#I#zC{(S z3ZTRjhZAM02nSQDl%3ir9DOi{L67QWIg6;9OWI*EN@mEy!DmfWc;h1r-`ASd%}s; z^*n=~pce#*b$sVJ6ipNs+7*@TwI>|M*KRk9SPq}6xY-2` z%lBF$PTlGv!*JQ?l8pWD&nrw=u0g`*6#^ft2z*`PkBU(Ddbf)V2}{8$IAPqu4{h95 ztLF*)c*g>n{gQW#x)I@Yz@0#WS9QA}We>ntHgZcme<&|V+3FQEWj}gRS7>Y~0YyXy zl0>*l*&ssIg%XY_MtNUQ7m7Kp{%stv-1k2|v0=^AAfc4?;sU2Wd|u-i6lvO2V;bUp z9;;z1sb3Dl3QC4M$;2vj0CE@$V;?9XDRsUO8qlCVW|MY@JLU7Eoo9+vK&%7lp9L`v z$_)`TIo2V6S*HYJo!FwjXG%E~6OiJYQrek>7|>0*GWrQ95~x`!So6~ zbU06$Q+2(9Ltz2LVsv(!P7qMPZK()#VE~Q-m-3aIsfDopm7I)Xf{if5vup(K#ZQC5 z$#A|s-#IC#x4%xQEV6DFv{wNU*PMD88%0;2gr*uM|j{=Ia5@T)P!>#AZ& z@Bun_Z!W$}?cg^o90sN!IW__-=hchX;^EV`#UR{@mUG0j<9%){AHgB|6n!V8{5 zz>WW44RcQgUtH)!(;))y?X_UDANb~rtWKLf_97G_(|16C8-w3`afS`RZ?ly4KBG!} z0FrcOk<%RE5qEDLfceO;4Y~8R&q6-zALMSQBf%S2?ncq~NqEh`H)QY|N-QP+5yzGm z)E6%@`of-cHgoDvtrVwT?F(+^uPvQSoPyTaHN*K(Equ%w1udkj);$75Y}L|vMfGdp zkb^|1#6Akmr5oJnq!i z&Y=8uPAe?n_uXC%@2ouT%n0S|g2C^>pHl|rervE5fXj0C>;U0m>hCt1Q%EN2bb!@N zYwL_r7g{@d|L~ygsW#5S;}2nF_4J#do;p6~lnO5J-`{fC-NQ+U z`;S8RlpK$DobSl{@a?B4ry-7}Lgspt2@_gcE&1vtqx<<`AEk@93of^LzL&8;C22QunW`KeE2(89m#Qq zA$;;`dQEbcsE8Z~(kTIMjP=64jL&ffs2bUNJrtTv%yx3~dODwggs$xAJPb?*pNraH zrK-}sowFgrm~QYB>@#CJO!11$sXVor>NDSgI8Sr*Pa7=IF@>MD3`!CEQ@z6fP_18=g_lw>=_bM0{u~i} z=%#F3{rfzQzz0L{%S5PZo~e=@;hhi%Uxx{E%H3Dx3w8b3F$Q=x`g=t10}%}z%2b1> z0Nx`$vW($J#wvMVhA$JKB?Maw$!Nt>P_G^VC2^_Jgd^30OM%Z~O8EX+cGp08C=_%i zusSM!rBg94!h!ST=bhHLcP==Cj^0F^Z{!$fKY>R3+;gn6JR$_%^DKH-hrtnHHSr$A zyv+h{YKZa9Y#r~RmR~->IUb~rg3khGhH&yr!;Rg%RS?#VPwQGto z@%}OpjSNYbyJ;+(RjLkk<-)wl&MIW6$(o^Fqe~WCkWp%0gT%`1*PNMnQ{ybWhP2^z zCsF_#eCKsGp%kR1ci9qok zp?WWLO2wsQr+CBvtKfUsn@+_b37taNMFrnHy_P!vrW51J+0GoKygPqxP!4emf$G)N z`cz<H<6#wxsGt%prICbRo_ zC#uOCw7S8McBsZaRG$yL6!ZvGrU%%q!Y~$(uc0KU$2U5qpIWPqy9VJlE-S9EwmE?XKFNkaK&oQBz3|+#AEHTlqWqo+ l>H(<=X{SLotzUH7={vYIQXT8XEaifGfiJX%!wN#!{{Wh^Q_}zd diff --git a/CMakeLists.txt b/CMakeLists.txt index 95ddc66..ce5de17 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,8 +3,6 @@ SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/3rdparty/CMakeModules" ${CMAKE_MODULE project (3DTK) - - #include_directories(OPENGL_INCLUDE_DIR) IF(WIN32) set(Boost_USE_STATIC_LIBS TRUE) @@ -15,9 +13,9 @@ ENDIF(WIN32) SET(Boost_ADDITIONAL_VERSIONS "1.42" "1.42.0" "1.44" "1.44.0" "1.45.0" "1.45" "1.46" "1.46.1" "1.47.0" "1.47" "1.48" "1.49") IF(WIN32) # for some unknown reason no one variant works on all windows platforms - find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time REQUIRED) + find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time program_options REQUIRED) ELSE(WIN32) - find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time REQUIRED) + find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time program_options REQUIRED) ENDIF(WIN32) if(Boost_FOUND) @@ -26,6 +24,10 @@ if(Boost_FOUND) add_definitions(${Boost_LIB_DIAGNOSTIC_DEFINITIONS}) endif() +FIND_PACKAGE(OpenCV REQUIRED) +include("3rdparty/CMakeModules/OpenCV.cmake") +SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_OPENCV_FLAGS}") + ################################################# # Declare Options and modify build accordingly ## ################################################# @@ -155,10 +157,6 @@ endif(WITH_MODEL) ## Thermo OPT_DEP(WITH_THERMO "Whether to build executables for mutual calibration of laser scanner and camera. ON/OFF" OFF "WITH_SHAPE_DETECTION") IF(WITH_THERMO) - #for OpenCV 2.1 - FIND_PACKAGE(OpenCV REQUIRED) - include("3rdparty/CMakeModules/OpenCV.cmake") - SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_OPENCV_FLAGS}") add_subdirectory(3rdparty/cvblob) include_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob) link_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob) @@ -237,16 +235,6 @@ ELSE(WITH_SEGMENTATION) MESSAGE(STATUS "Without segmentation") ENDIF(WITH_SEGMENTATION) -## Normals - -OPT_DEP(WITH_NORMALS "Whether to build program for normal computation ON/OFF" OFF "WITH_FBR") - -IF(WITH_NORMALS) - MESSAGE(STATUS "With normals") -ELSE(WITH_NORMALS) - MESSAGE(STATUS "Without normals") -ENDIF(WITH_NORMALS) - ## RivLib OPT_DEP(WITH_RIVLIB "Whether the RIEGL rivlib is present ON/OFF" OFF "") @@ -290,9 +278,6 @@ ENDIF(WITH_PMD) OPT_DEP(WITH_FBR "Whether to compile feature based registration. ON/OFF" OFF "") IF(WITH_FBR) - FIND_PACKAGE(OpenCV REQUIRED) - include("3rdparty/CMakeModules/OpenCV.cmake") - SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_OPENCV_FLAGS}") MESSAGE(STATUS "With FBR ") ELSE(WITH_FBR) MESSAGE(STATUS "Without FBR") diff --git a/README.scanserver b/README.scanserver index 022fa73..cc9a0a6 100644 --- a/README.scanserver +++ b/README.scanserver @@ -41,7 +41,7 @@ If you receive a bus_error, the size of your shared memory is too small and the 4. Locking the memory to avoid swapping (Linux only) -If a great portion of the RAM is used for the cache data, swapping will usually occur after 50% of usage. To avoid this, the scanserver tries to lock the whole memory in place. This will fail without superuser rights, as well as on a full shared memory (see 3.) even with rights. Add these two additional lines to '/etc/security/limits.conf': +If a great portion of the RAM is used for the cache data, swapping will usually occur after 50% of usage. To avoid this, the scanserver tries to lock the whole memory in place. This will fail without superuser rights, as well as on a full shared memory (see 3.) even with rights. To solve this problem, add these two additional lines to '/etc/security/limits.conf': * soft memlock unlimited * hard memlock unlimited diff --git a/include/normals/normals.h b/include/normals/normals.h new file mode 100644 index 0000000..b8c3ce0 --- /dev/null +++ b/include/normals/normals.h @@ -0,0 +1,155 @@ +#ifndef NORMALS_H +#define NORMALS_H + +#include +#include +#if (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#include +#else +#include +#endif + +void calculateNormalsApxKNN(std::vector &normals, + vector &points, + int k, + const double _rPos[3], + double eps = 0.0); + +void calculateNormalsAdaptiveApxKNN(std::vector &normals, + vector &points, + int kmin, + int kmax, + const double _rPos[3], + double eps = 0.0); + +void calculateNormalsKNN(std::vector &normals, + vector &points, + int k, + const double _rPos[3] ); + + +void calculateNormalsAdaptiveKNN(vector &normals, + vector &points, + int kmin, int kmax, + const double _rPos[3]); + +void calculateNormalsPANORAMA(vector &normals, + vector &points, + vector< vector< vector< cv::Vec3f > > > extendedMap, + const double _rPos[3]); + +// TODO should be exported to separate library +/* + * retrieve a cv::Mat with x,y,z,r from a scan object + * functionality borrowed from scan_cv::convertScanToMat but this function + * does not allow a scanserver to be used, prints to stdout and can only + * handle a single scan + */ +static inline cv::Mat scan2mat(Scan *source) +{ + DataXYZ xyz = source->get("xyz"); + + DataReflectance xyz_reflectance = source->get("reflectance"); + + unsigned int nPoints = xyz.size(); + cv::Mat scan(nPoints,1,CV_32FC(4)); + scan = cv::Scalar::all(0); + + cv::MatIterator_ it; + + it = scan.begin(); + for(unsigned int i = 0; i < nPoints; i++){ + float x, y, z, reflectance; + x = xyz[i][0]; + y = xyz[i][1]; + z = xyz[i][2]; + if(xyz_reflectance.size() != 0) + { + reflectance = xyz_reflectance[i]; + + //normalize the reflectance + reflectance += 32; + reflectance /= 64; + reflectance -= 0.2; + reflectance /= 0.3; + if (reflectance < 0) reflectance = 0; + if (reflectance > 1) reflectance = 1; + } + + (*it)[0] = x; + (*it)[1] = y; + (*it)[2] = z; + if(xyz_reflectance.size() != 0) + (*it)[3] = reflectance; + else + (*it)[3] = 0; + + ++it; + } + return scan; +} +// TODO should be exported to separate library +/* + * convert a matrix of float values (range image) to a matrix of unsigned + * eight bit characters using different techniques + */ +static inline cv::Mat float2uchar(cv::Mat &source, bool logarithm, float cutoff) +{ + cv::Mat result(source.size(), CV_8U, cv::Scalar::all(0)); + float max = 0; + // find maximum value + if (cutoff == 0.0) { + // without cutoff, just iterate through all values to find the largest + for (cv::MatIterator_ it = source.begin(); + it != source.end(); ++it) { + float val = *it; + if (val > max) { + max = val; + } + } + } else { + // when a cutoff is specified, sort all the points by value and then + // specify the max so that values are larger than it + vector sorted(source.cols*source.rows); + int i = 0; + for (cv::MatIterator_ it = source.begin(); + it != source.end(); ++it, ++i) { + sorted[i] = *it; + } + std::sort(sorted.begin(), sorted.end()); + max = sorted[(int)(source.cols*source.rows*(1.0-cutoff))]; + cout << "A cutoff of " << cutoff << " resulted in a max value of " << max << endl; + } + + cv::MatIterator_ src = source.begin(); + cv::MatIterator_ dst = result.begin(); + cv::MatIterator_ end = source.end(); + if (logarithm) { + // stretch values from 0 to max logarithmically over 0 to 255 + // using the logarithm allows to represent smaller values with more + // precision and larger values with less + max = log(max+1); + for (; src != end; ++src, ++dst) { + float val = (log(*src+1)*255.0)/max; + if (val > 255) + *dst = 255; + else + *dst = (uchar)val; + } + } else { + // stretch values from 0 to max linearly over 0 to 255 + for (; src != end; ++src, ++dst) { + float val = (*src*255.0)/max; + if (val > 255) + *dst = 255; + else + *dst = (uchar)val; + } + } + return result; +} + + + +#endif // NORMALS_H diff --git a/include/scanio/scan_io_ply.h b/include/scanio/scan_io_ply.h new file mode 100644 index 0000000..5acc395 --- /dev/null +++ b/include/scanio/scan_io_ply.h @@ -0,0 +1,39 @@ +/** + * @file + * @brief IO of a 3D scan given in ply format + * @author Andreas Nuechter + */ + +#ifndef __SCAN_IO_PLY_H__ +#define __SCAN_IO_PLY_H__ + +#include "scan_io.h" + + +/** + * @brief 3D scan loader for UOS scans + * + * The compiled class is available as shared object file + */ +class ScanIO_ply : public ScanIO { +public: + virtual std::list readDirectory(const char* dir_path, + unsigned int start, + unsigned int end); + virtual void readPose(const char* dir_path, + const char* identifier, + double* pose); + virtual void readScan(const char* dir_path, + const char* identifier, + PointFilter& filter, + std::vector* xyz, + std::vector* rgb, + std::vector* reflectance, + std::vector* temperature, + std::vector* amplitude, + std::vector* type, + std::vector* deviation); + virtual bool supports(IODataType type); +}; + +#endif diff --git a/include/scanio/scan_io_xyzr.h b/include/scanio/scan_io_xyzr.h new file mode 100644 index 0000000..6daa88c --- /dev/null +++ b/include/scanio/scan_io_xyzr.h @@ -0,0 +1,42 @@ +/** + * @file scan_io_uosr.h + * @brief IO of a 3D scan in xyz file format plus an intensity + + * @author Billy Okal + */ + +#ifndef __SCAN_IO_XYZR_H__ +#define __SCAN_IO_XYZR_H__ + +#include "scan_io.h" + + + +/** + * @brief IO of a 3D scan in uos file format plus a + * reflectance/intensity/temperature value + * + * The compiled class is available as shared object file + */ +class ScanIO_xyzr : public ScanIO { +public: + virtual std::list readDirectory(const char* dir_path, + unsigned int start, + unsigned int end); + virtual void readPose(const char* dir_path, + const char* identifier, + double* pose); + virtual void readScan(const char* dir_path, + const char* identifier, + PointFilter& filter, + std::vector* xyz, + std::vector* rgb, + std::vector* reflectance, + std::vector* temperature, + std::vector* amplitude, + std::vector* type, + std::vector* deviation); + virtual bool supports(IODataType type); +}; + +#endif diff --git a/include/slam6d/basicScan.h b/include/slam6d/basicScan.h index 98595db..83bcd93 100644 --- a/include/slam6d/basicScan.h +++ b/include/slam6d/basicScan.h @@ -42,6 +42,7 @@ public: protected: virtual void createSearchTreePrivate(); virtual void calcReducedOnDemandPrivate(); + virtual void calcNormalsOnDemandPrivate(); virtual void addFrame(AlgoType type); private: diff --git a/include/slam6d/data_types.h b/include/slam6d/data_types.h index 3625728..43a1de4 100644 --- a/include/slam6d/data_types.h +++ b/include/slam6d/data_types.h @@ -217,6 +217,7 @@ private: typedef TripleArray DataXYZ; typedef TripleArray DataXYZFloat; typedef TripleArray DataRGB; +typedef TripleArray DataNormal; typedef SingleArray DataReflectance; typedef SingleArray DataTemperature; typedef SingleArray DataAmplitude; diff --git a/include/slam6d/globals.icc b/include/slam6d/globals.icc index 803cda3..f0c0200 100644 --- a/include/slam6d/globals.icc +++ b/include/slam6d/globals.icc @@ -273,8 +273,8 @@ inline void M4identity( T *M ) */ template inline void MMult(const T *M1, - const T *M2, - T *Mout) + const T *M2, + T *Mout) { Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3]; Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3]; @@ -296,8 +296,8 @@ inline void MMult(const T *M1, template inline void MMult(const T *M1, - const T *M2, - float *Mout) + const T *M2, + float *Mout) { Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3]; Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3]; @@ -326,9 +326,9 @@ inline void MMult(const T *M1, template inline void VTrans(const T *M, const T *V, T *P) { - P[0] = M[0] * V[0] + M[4] * V[1] + M[8] * V[2] + M[12]; - P[1] = M[1] * V[0] + M[5] * V[1] + M[9] * V[2] + M[13]; - P[2] = M[2] * V[0] + M[6] * V[1] + M[10] * V[2] + M[14]; + P[0] = M[0] * V[0] + M[4] * V[1] + M[8] * V[2] + M[12]; + P[1] = M[1] * V[0] + M[5] * V[1] + M[9] * V[2] + M[13]; + P[2] = M[2] * V[0] + M[6] * V[1] + M[10] * V[2] + M[14]; } /** @@ -368,8 +368,8 @@ inline double M3det( const T *M ) { double det; det = (double)(M[0] * ( M[4]*M[8] - M[7]*M[5] ) - - M[1] * ( M[3]*M[8] - M[6]*M[5] ) - + M[2] * ( M[3]*M[7] - M[6]*M[4] )); + - M[1] * ( M[3]*M[8] - M[6]*M[5] ) + + M[2] * ( M[3]*M[7] - M[6]*M[4] )); return ( det ); } @@ -457,7 +457,7 @@ static inline void Matrix4ToEuler(const double *alignxf, double *rPosTheta, doub rPosTheta[1] = M_PI - asin(alignxf[8]); } // rPosTheta[1] = asin( alignxf[8]); // Calculate Y-axis angle - + double C = cos( rPosTheta[1] ); if ( fabs( C ) > 0.005 ) { // Gimball lock? _trX = alignxf[10] / C; // No, so get X-axis angle @@ -476,7 +476,7 @@ static inline void Matrix4ToEuler(const double *alignxf, double *rPosTheta, doub rPosTheta[0] = rPosTheta[0]; rPosTheta[1] = rPosTheta[1]; rPosTheta[2] = rPosTheta[2]; - + if (rPos != 0) { rPos[0] = alignxf[12]; rPos[1] = alignxf[13]; @@ -545,12 +545,12 @@ inline unsigned char randUC(unsigned char rnd) inline double polardist(double* p, double *p2) { double stheta = sin(p[0]) * sin(p2[0]); double myd2 = acos( stheta * cos(p[1]) * cos(p2[1]) - + stheta * sin(p[1]) * sin(p2[1]) - + cos(p[0]) * cos(p2[0])); + + stheta * sin(p[1]) * sin(p2[1]) + + cos(p[0]) * cos(p2[0])); return myd2; } - + inline void toKartesian(double *polar, double *kart) { kart[0] = polar[2] * cos( polar[1] ) * sin( polar[0] ); @@ -569,8 +569,8 @@ inline void toPolar(double *n, double *polar) { rho = Len(n); Normalize3(n); - // if(fabs(1 - fabs(n[1])) < 0.001) { - // cout << "Y " << n[0] << " " << n[1] << " " << n[2] << endl; + // if(fabs(1 - fabs(n[1])) < 0.001) { + // cout << "Y " << n[0] << " " << n[1] << " " << n[2] << endl; phi = acos(n[2]); //if ( fabs(phi) < 0.0001) phi = 0.0001; @@ -591,9 +591,9 @@ inline void toPolar(double *n, double *polar) { } } else { theta0 = acos(n[0]/sin(phi)); - + } - + double sintheta = n[1]/sin(phi); @@ -609,7 +609,7 @@ inline void toPolar(double *n, double *polar) { } } - /* } else { + /* } else { theta = 0.0; phi = 0.0; }*/ @@ -634,11 +634,11 @@ static inline void M4_submat(const T *Min, T *Mout, int i, int j ) { // loop through 3x3 submatrix for( di = 0; di < 3; di ++ ) { for( dj = 0; dj < 3; dj ++ ) { - // map 3x3 element (destination) to 4x4 element (source) - si = di + ( ( di >= i ) ? 1 : 0 ); - sj = dj + ( ( dj >= j ) ? 1 : 0 ); - // copy element - Mout[di * 3 + dj] = Min[si * 4 + sj]; + // map 3x3 element (destination) to 4x4 element (source) + si = di + ( ( di >= i ) ? 1 : 0 ); + sj = dj + ( ( dj >= j ) ? 1 : 0 ); + // copy element + Mout[di * 3 + dj] = Min[si * 4 + sj]; } } } @@ -684,9 +684,9 @@ static inline int M4inv(const T *Min, T *Mout ) int i, j, sign; for ( i = 0; i < 4; i++ ) { for ( j = 0; j < 4; j++ ) { - sign = 1 - ( (i +j) % 2 ) * 2; - M4_submat( Min, mtemp, i, j ); - Mout[i+j*4] = ( M3det( mtemp ) * sign ) / mdet; + sign = 1 - ( (i +j) % 2 ) * 2; + M4_submat( Min, mtemp, i, j ); + Mout[i+j*4] = ( M3det( mtemp ) * sign ) / mdet; } } return( 1 ); @@ -787,9 +787,9 @@ static inline bool choldc(unsigned int n, double **A, double *diag) * Solve Ax=B after choldc * +++++++++-------------++++++++++++ */ static inline void cholsl(double A[3][3], - double diag[3], - double B[3], - double x[3]) + double diag[3], + double B[3], + double x[3]) { for (int i=0; i < 3; i++) { double sum = B[i]; @@ -813,10 +813,10 @@ static inline void cholsl(double A[3][3], * Solve Ax=B after choldc * +++++++++-------------++++++++++++ */ static inline void cholsl(unsigned int n, - double **A, - double *diag, - double *B, - double *x) + double **A, + double *diag, + double *B, + double *x) { for (unsigned int i=0; i < n; i++) { double sum = B[i]; @@ -1090,62 +1090,62 @@ inline int LU_factor( double A[4][4], int indx[4]) int minMN = 4; for (j = 0; j < minMN; j++) + { + + // find pivot in column j and test for singularity. + + jp = j; + t = fabs(A[j][j]); + for (i = j+1; i < M; i++) + if ( fabs(A[i][j]) > t) + { + jp = i; + t = fabs(A[i][j]); + } + + indx[j] = jp; + + // jp now has the index of maximum element + // of column j, below the diagonal + + if ( A[jp][j] == 0 ) + return 1; // factorization failed because of zero pivot + + + if (jp != j) // swap rows j and jp + for (k = 0; k < N; k++) + { + t = A[j][k]; + A[j][k] = A[jp][k]; + A[jp][k] =t; + } + + if (j < M) // compute elements j+1:M of jth column + { + // note A(j,j), was A(jp,p) previously which was + // guarranteed not to be zero (Label #1) + // + double recp = 1.0 / A[j][j]; + + for (k = j+1; k < M; k++) + A[k][j] *= recp; + } + + if (j < minMN) { + // rank-1 update to trailing submatrix: E = E - x*y; + // + // E is the region A(j+1:M, j+1:N) + // x is the column vector A(j+1:M,j) + // y is row vector A(j,j+1:N) - // find pivot in column j and test for singularity. - - jp = j; - t = fabs(A[j][j]); - for (i = j+1; i < M; i++) - if ( fabs(A[i][j]) > t) - { - jp = i; - t = fabs(A[i][j]); - } - - indx[j] = jp; - - // jp now has the index of maximum element - // of column j, below the diagonal - - if ( A[jp][j] == 0 ) - return 1; // factorization failed because of zero pivot - - - if (jp != j) // swap rows j and jp - for (k = 0; k < N; k++) - { - t = A[j][k]; - A[j][k] = A[jp][k]; - A[jp][k] =t; - } - - if (j < M) // compute elements j+1:M of jth column - { - // note A(j,j), was A(jp,p) previously which was - // guarranteed not to be zero (Label #1) - // - double recp = 1.0 / A[j][j]; - - for (k = j+1; k < M; k++) - A[k][j] *= recp; - } - - if (j < minMN) - { - // rank-1 update to trailing submatrix: E = E - x*y; - // - // E is the region A(j+1:M, j+1:N) - // x is the column vector A(j+1:M,j) - // y is row vector A(j,j+1:N) - - int ii,jj; - - for (ii = j+1; ii < M; ii++) - for (jj = j+1; jj < N; jj++) - A[ii][jj] -= A[ii][j]*A[j][jj]; - } + int ii,jj; + + for (ii = j+1; ii < M; ii++) + for (jj = j+1; jj < N; jj++) + A[ii][jj] -= A[ii][j]*A[j][jj]; } + } return 0; } @@ -1167,23 +1167,23 @@ inline int LU_solve(const double A[4][4], const int indx[4], double b[4]) double sum = 0.0; for (i = 0; i < n; i++) - { - ip=indx[i]; - sum=b[ip]; - b[ip]=b[i]; - if (ii) - for (j = ii;j <= i-1; j++) - sum -= A[i][j]*b[j]; - else if (sum) ii=i; - b[i]=sum; - } + { + ip=indx[i]; + sum=b[ip]; + b[ip]=b[i]; + if (ii) + for (j = ii;j <= i-1; j++) + sum -= A[i][j]*b[j]; + else if (sum) ii=i; + b[i]=sum; + } for (i = n-1; i >= 0; i--) - { - sum=b[i]; - for (j = i+1; j < n; j++) - sum -= A[i][j]*b[j]; - b[i]=sum/A[i][i]; - } + { + sum=b[i]; + for (j = i+1; j < n; j++) + sum -= A[i][j]*b[j]; + b[i]=sum/A[i][i]; + } return 0; } @@ -1273,10 +1273,10 @@ static inline void RPYEulerQuat(const double *euler, double *quat) double cpsi = cos(euler[2]); double _r[3][3] = { //create rotational Matrix - {cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi}, - {spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi}, - { -stheta, ctheta*sphi, ctheta*cphi} - }; + {cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi}, + {spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi}, + { -stheta, ctheta*sphi, ctheta*cphi} + }; #define MY_MAX(a,b) (((a)>(b))?(a):(b)) double _w = sqrt(MY_MAX(0, 1 + _r[0][0] + _r[1][1] + _r[2][2]))/2.0; @@ -1301,6 +1301,18 @@ inline void transform3(const double *alignxf, double *point) point[2] = z_neu + alignxf[14]; } +inline void transform3normal(const double *alignxf, double *normal) +{ + double x, y, z; + + x = normal[0] * alignxf[0] + normal[1] * alignxf[1] + normal[2] * alignxf[2]; + y = normal[0] * alignxf[4] + normal[1] * alignxf[5] + normal[2] * alignxf[6]; + z = normal[0] * alignxf[8] + normal[1] * alignxf[9] + normal[2] * alignxf[10]; + normal[0] = x; + normal[1] = y; + normal[2] = z; +} + inline void transform3(const double *alignxf, const double *point, double *tpoint) { tpoint[0] = point[0] * alignxf[0] + point[1] * alignxf[4] + point[2] * alignxf[8] + alignxf[12]; @@ -1308,6 +1320,28 @@ inline void transform3(const double *alignxf, const double *point, double *tpoin tpoint[2] = point[0] * alignxf[2] + point[1] * alignxf[6] + point[2] * alignxf[10] + alignxf[14]; } +inline void scal_mul3(const double *vec_in, const double scalar, double *vec_out) +{ + vec_out[0] = vec_in[0] * scalar; + vec_out[1] = vec_in[1] * scalar; + vec_out[2] = vec_in[2] * scalar; +} + +inline void sub3(const double *vec1_in, const double *vec2_in, double *vec_out) +{ + vec_out[0] = vec1_in[0] - vec2_in[0]; + vec_out[1] = vec1_in[1] - vec2_in[1]; + vec_out[2] = vec1_in[2] - vec2_in[2]; +} + +inline void add3(const double *vec1_in, const double *vec2_in, double *vec_out) +{ + vec_out[0] = vec1_in[0] + vec2_in[0]; + vec_out[1] = vec1_in[1] + vec2_in[1]; + vec_out[2] = vec1_in[2] + vec2_in[2]; +} + + inline std::string trim(const std::string& source) { unsigned int start = 0, end = source.size() - 1; diff --git a/include/slam6d/icp6D.h b/include/slam6d/icp6D.h index 524a171..e303893 100644 --- a/include/slam6d/icp6D.h +++ b/include/slam6d/icp6D.h @@ -15,6 +15,7 @@ using std::vector; #include "slam6d/scan.h" #include "slam6d/icp6Dminimizer.h" +#include "slam6d/pairingMode.h" /** * @brief Representation of 3D scan matching with ICP. @@ -38,15 +39,15 @@ public: double epsilonICP = 0.0000001, int nns_method = simpleKD, bool cuda_enabled = false, - bool cad_matching = false); + bool cad_matching = false); /** * Destructor (empty, but needed, because virtual) */ virtual ~icp6D() {}; - void doICP(vector allScans); - virtual int match(Scan* PreviousScan, Scan* CurrentScan); + void doICP(vector allScans, PairingMode pairing_mode = CLOSEST_POINT); + virtual int match(Scan* PreviousScan, Scan* CurrentScan, PairingMode pairing_mode = CLOSEST_POINT); void covarianceEuler(Scan *scan1, Scan *scan2, NEWMAT::Matrix *C); void covarianceQuat(Scan *scan1, Scan *scan2, NEWMAT::Matrix *C); double Point_Point_Error(Scan* PreviousScan, Scan* CurrentScan, double max_dist_match, unsigned int *nrp=0); diff --git a/include/slam6d/kd.h b/include/slam6d/kd.h index 3545c97..532b0c2 100644 --- a/include/slam6d/kd.h +++ b/include/slam6d/kd.h @@ -38,13 +38,31 @@ struct PtrAccessor { * capabilities (find nearest point to * a given point, or to a ray). **/ -class KDtree : public SearchTree, private KDTreeImpl { +class KDtree : public SearchTree, private KDTreeImpl +{ public: KDtree(double **pts, int n); virtual ~KDtree(); - virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const; + virtual double *FindClosest(double *_p, + double maxdist2, + int threadNum = 0) const; + + virtual double *FindClosestAlongDir(double *_p, + double *_dir, + double maxdist2, + int threadNum = 0) const; + + virtual vector kNearestNeighbors(double *_p, + int k, + double sqRad2, + int threadNum = 0) const; + + virtual vector fixedRangeSearch(double *_p, + double sqRad2, + int threadNum = 0) const; + }; #endif diff --git a/include/slam6d/kdManaged.h b/include/slam6d/kdManaged.h index 389bdeb..7ea584a 100644 --- a/include/slam6d/kdManaged.h +++ b/include/slam6d/kdManaged.h @@ -55,6 +55,8 @@ public: //! Aquires cached data first to pass on to the usual KDtree to process virtual double* FindClosest(double *_p, double maxdist2, int threadNum = 0) const; + + virtual double *FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum = 0) const; private: Scan* m_scan; DataXYZ* m_data; diff --git a/include/slam6d/kdMeta.h b/include/slam6d/kdMeta.h index 3c53adb..8a45365 100644 --- a/include/slam6d/kdMeta.h +++ b/include/slam6d/kdMeta.h @@ -62,6 +62,8 @@ public: //! Aquires cached data first to pass on to the usual KDtree to process virtual double* FindClosest(double *_p, double maxdist2, int threadNum = 0) const; + + virtual double *FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum = 0) const; private: Scan** m_scans; DataXYZ** m_data; diff --git a/include/slam6d/kdTreeImpl.h b/include/slam6d/kdTreeImpl.h index 54a7a53..6067c9d 100755 --- a/include/slam6d/kdTreeImpl.h +++ b/include/slam6d/kdTreeImpl.h @@ -2,9 +2,9 @@ * @brief Representation of the optimized k-d tree. * @author Remus Dumitru. Jacobs University Bremen, Germany * @author Corneliu-Claudiu Prodescu. Jacobs University Bremen, Germany - * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. - * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. - * @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Jacobs University Bremen, Germany + * @author Kai Lingemann. Inst. of CS, University of Osnabrueck, Germany + * @author Thomas Escher. Inst. of CS, University of Osnabrueck, Germany */ #ifndef __KD_TREE_IMPL_H__ @@ -23,6 +23,15 @@ #include #endif +class PointCompare { +public: + bool operator() (const std::pair& left, + const std::pair& right) + { + return left.second > right.second; + } +}; + /** * @brief The optimized k-d tree. * @@ -76,10 +85,10 @@ public: } return; } - + // Else, interior nodes npts = 0; - + node.center[0] = 0.5 * (xmin+xmax); node.center[1] = 0.5 * (ymin+ymax); node.center[2] = 0.5 * (zmin+zmax); @@ -126,7 +135,7 @@ public: break; std::swap(*left, *right); } - + // Build subtrees int i; #ifdef WITH_OPENMP_KD // does anybody know the reason why this is slower ?? --Andreas @@ -178,9 +187,9 @@ protected: struct { double center[3]; ///< storing the center of the voxel (R^3) double dx, ///< defining the voxel itself - dy, ///< defining the voxel itself - dz, ///< defining the voxel itself - r2; ///< defining the voxel itself + dy, ///< defining the voxel itself + dz, ///< defining the voxel itself + r2; ///< defining the voxel itself int splitaxis; ///< defining the kind of splitaxis KDTreeImpl *child1; ///< pointers to the childs KDTreeImpl *child2; ///< pointers to the childs @@ -213,10 +222,12 @@ protected: } // Quick check of whether to abort - double approx_dist_bbox = max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx, - fabs(params[threadNum].p[1]-node.center[1])-node.dy), - fabs(params[threadNum].p[2]-node.center[2])-node.dz); - if (approx_dist_bbox >= 0 && sqr(approx_dist_bbox) >= params[threadNum].closest_d2) + double approx_dist_bbox = + max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx, + fabs(params[threadNum].p[1]-node.center[1])-node.dy), + fabs(params[threadNum].p[2]-node.center[2])-node.dz); + if (approx_dist_bbox >= 0 && + sqr(approx_dist_bbox) >= params[threadNum].closest_d2) return; // Recursive case @@ -233,6 +244,143 @@ protected: } } } + + void _FindClosestAlongDir(const PointData& pts, int threadNum) const { + AccessorFunc point; + + // Leaf nodes + if (npts) { + for (int i=0; i < npts; i++) { + double p2p[] = { params[threadNum].p[0] - point(pts, leaf.p[i])[0], + params[threadNum].p[1] - point(pts, leaf.p[i])[1], + params[threadNum].p[2] - point(pts, leaf.p[i])[2] }; + double myd2 = Len2(p2p) - sqr(Dot(p2p, params[threadNum].dir)); + if ((myd2 < params[threadNum].closest_d2)) { + params[threadNum].closest_d2 = myd2; + params[threadNum].closest = point(pts, leaf.p[i]); + } + } + return; + } + + + // Quick check of whether to abort + double p2c[] = { params[threadNum].p[0] - node.center[0], + params[threadNum].p[1] - node.center[1], + params[threadNum].p[2] - node.center[2] }; + double myd2center = Len2(p2c) - sqr(Dot(p2c, params[threadNum].dir)); + if (myd2center > node.r2 + params[threadNum].closest_d2 + 2.0f * max(node.r2, params[threadNum].closest_d2)) + return; + + + // Recursive case + if (params[threadNum].p[node.splitaxis] < node.center[node.splitaxis] ) { + node.child1->_FindClosestAlongDir(pts, threadNum); + node.child2->_FindClosestAlongDir(pts, threadNum); + } else { + node.child2->_FindClosestAlongDir(pts, threadNum); + node.child1->_FindClosestAlongDir(pts, threadNum); + } + } + + void _FixedRangeSearch(const PointData& pts, int threadNum) const { + AccessorFunc point; + + // Leaf nodes + if (npts) { + for (int i = 0; i < npts; i++) { + double myd2 = Dist2(params[threadNum].p, point(pts, leaf.p[i])); + if (myd2 < params[threadNum].closest_d2) { + params[threadNum].closest = point(pts, leaf.p[i]); + + Point newPt; + double* currPt = point(pts, leaf.p[i]); + newPt.x = currPt[0]; + newPt.y = currPt[1]; + newPt.z = currPt[2]; + params[threadNum].heap.push_back(std::make_pair(newPt, myd2)); + std::push_heap(params[threadNum].heap.begin(), + params[threadNum].heap.end(), + PointCompare()); + } + } + return; + } + + // Quick check of whether to abort + double approx_dist_bbox = + max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx, + fabs(params[threadNum].p[1]-node.center[1])-node.dy), + fabs(params[threadNum].p[2]-node.center[2])-node.dz); + if (approx_dist_bbox >= 0 && + sqr(approx_dist_bbox) >= params[threadNum].closest_d2) + return; + + // Recursive case + double myd = node.center[node.splitaxis] - params[threadNum].p[node.splitaxis]; + if (myd >= 0.0) { + node.child1->_FixedRangeSearch(pts, threadNum); + if (sqr(myd) < params[threadNum].closest_d2) { + node.child2->_FixedRangeSearch(pts, threadNum); + } + } else { + node.child2->_FixedRangeSearch(pts, threadNum); + if (sqr(myd) < params[threadNum].closest_d2) { + node.child1->_FixedRangeSearch(pts, threadNum); + } + } + } + + + void _KNNSearch(const PointData& pts, int threadNum) const { + AccessorFunc point; + + // Leaf nodes + if (npts) { + for (int i = 0; i < npts; i++) { + double myd2 = Dist2(params[threadNum].p, point(pts, leaf.p[i])); + + if (myd2 < params[threadNum].closest_d2) { + Point newPt; + double* currPt = point(pts, leaf.p[i]); + newPt.x = currPt[0]; + newPt.y = currPt[1]; + newPt.z = currPt[2]; + params[threadNum].heap.push_back(std::make_pair(newPt, myd2)); + std::push_heap(params[threadNum].heap.begin(), + params[threadNum].heap.end(), + PointCompare()); + + params[threadNum].closest = point(pts, leaf.p[i]); + } + } + return; + } + + // Quick check of whether to abort + double approx_dist_bbox = + max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx, + fabs(params[threadNum].p[1]-node.center[1])-node.dy), + fabs(params[threadNum].p[2]-node.center[2])-node.dz); + if (approx_dist_bbox >= 0 && + sqr(approx_dist_bbox) >= params[threadNum].closest_d2) + return; + + // Recursive case + double myd = node.center[node.splitaxis] - params[threadNum].p[node.splitaxis]; + if (myd >= 0.0) { + node.child1->_KNNSearch(pts, threadNum); + if (sqr(myd) < params[threadNum].closest_d2) { + node.child2->_KNNSearch(pts, threadNum); + } + } else { + node.child2->_KNNSearch(pts, threadNum); + if (sqr(myd) < params[threadNum].closest_d2) { + node.child1->_KNNSearch(pts, threadNum); + } + } + } }; + #endif diff --git a/include/slam6d/kdparams.h b/include/slam6d/kdparams.h index 31fc7cf..734be75 100644 --- a/include/slam6d/kdparams.h +++ b/include/slam6d/kdparams.h @@ -1,14 +1,18 @@ /** * @file * @brief Representation of the parameter of a k-d tree - * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. jacobs University Bremen, Germany. */ #ifndef __KDPARAMS_H__ #define __KDPARAMS_H__ +#include "slam6d/point.h" + +#include + /** - * @brief Contains the intermediate (static) values of a k-d tree or a cached k-d tree + * @brief Contains the intermediate (static) values of a k-d tree * * A parameter class for the latter k-d tree. * Includes the padding for parallelizetion @@ -32,6 +36,16 @@ public: */ double *p; + /** + * pointer to direction vector, if we're using FindClosestAlongDir + */ + double *dir; + + /** + * heap for KNN. + */ + std::vector > heap; + /** * expand to 128 bytes to avoid false-sharing, 16 bytes from above + 28*4 bytes = 128 bytes */ diff --git a/include/slam6d/managedScan.h b/include/slam6d/managedScan.h index 48bd1cc..2a39243 100644 --- a/include/slam6d/managedScan.h +++ b/include/slam6d/managedScan.h @@ -43,6 +43,7 @@ public: protected: virtual void createSearchTreePrivate(); virtual void calcReducedOnDemandPrivate(); + virtual void calcNormalsOnDemandPrivate() {}; virtual void addFrame(AlgoType type); private: diff --git a/include/slam6d/metaScan.h b/include/slam6d/metaScan.h index 507ae01..6edcbe1 100644 --- a/include/slam6d/metaScan.h +++ b/include/slam6d/metaScan.h @@ -34,6 +34,7 @@ public: protected: virtual void createSearchTreePrivate(); virtual void calcReducedOnDemandPrivate() {} + virtual void calcNormalsOnDemandPrivate() {} virtual void addFrame(AlgoType type) {} private: std::vector m_scans; diff --git a/include/slam6d/pairingMode.h b/include/slam6d/pairingMode.h new file mode 100644 index 0000000..ab9f970 --- /dev/null +++ b/include/slam6d/pairingMode.h @@ -0,0 +1,10 @@ +#ifndef __PAIRINGMODE_H__ +#define __PAIRINGMODE_H__ + +enum PairingMode { + CLOSEST_POINT, + CLOSEST_POINT_ALONG_NORMAL, + CLOSEST_PLANE +}; + +#endif // PAIRINGMODE_H diff --git a/include/slam6d/point.h b/include/slam6d/point.h index 24e4772..4062428 100644 --- a/include/slam6d/point.h +++ b/include/slam6d/point.h @@ -101,6 +101,12 @@ public: double y; /// z coordinate in 3D space double z; + /// normal x direction in 3D space + double nx; + /// normal x direction in 3D space + double ny; + /// normal x direction in 3D space + double nz; /// additional information about the point, e.g., semantic /// also used in veloscan for distiuguish moving or static int type; diff --git a/include/slam6d/point_type.h b/include/slam6d/point_type.h index 5c0fd82..bfdd82b 100644 --- a/include/slam6d/point_type.h +++ b/include/slam6d/point_type.h @@ -30,6 +30,7 @@ public: static const unsigned int USE_NONE; static const unsigned int USE_REFLECTANCE; + static const unsigned int USE_NORMAL; static const unsigned int USE_TEMPERATURE; static const unsigned int USE_AMPLITUDE; static const unsigned int USE_DEVIATION; @@ -44,6 +45,7 @@ public: PointType(unsigned int _types); bool hasReflectance(); + bool hasNormal(); bool hasTemperature(); bool hasAmplitude(); bool hasDeviation(); @@ -114,6 +116,7 @@ private: unsigned int getScanSize(Scan* scan); DataXYZ* m_xyz; + DataXYZ* m_normal; DataRGB* m_rgb; DataReflectance* m_reflectance; DataTemperature* m_temperature; @@ -134,6 +137,11 @@ T *PointType::createPoint(const Point &P, unsigned int index ) { if (types & USE_REFLECTANCE) { p[counter++] = P.reflectance; } + if (types & USE_NORMAL) { + p[counter++] = P.nx; + p[counter++] = P.ny; + p[counter++] = P.nz; + } if (types & USE_TEMPERATURE) { p[counter++] = P.temperature; } @@ -171,6 +179,11 @@ Point PointType::createPoint(T *p) { if (types & USE_REFLECTANCE) { P.reflectance = p[counter++]; } + if (types & USE_NORMAL) { + p[counter++] = P.nx; + p[counter++] = P.ny; + p[counter++] = P.nz; + } if (types & USE_TEMPERATURE) { P.temperature = p[counter++]; } @@ -206,6 +219,10 @@ T *PointType::createPoint(unsigned int i, unsigned int index) { if (types & USE_REFLECTANCE) { p[counter++] = (m_reflectance? (*m_reflectance)[i]: 0); } + if (types & USE_NORMAL) { + for(unsigned int j = 0; j < 3; ++j) + p[counter++] = (*m_normal)[i][j]; + } if (types & USE_TEMPERATURE) { p[counter++] = (m_temperature? (*m_temperature)[i]: 0); } diff --git a/include/slam6d/scan.h b/include/slam6d/scan.h index bf7c9b4..19b25a8 100644 --- a/include/slam6d/scan.h +++ b/include/slam6d/scan.h @@ -5,6 +5,7 @@ #include "data_types.h" #include "point_type.h" #include "ptpair.h" +#include "pairingMode.h" #include #include @@ -97,10 +98,7 @@ Last, if program ends, clean up class Scan { //friend class SearchTree; // TODO: is this neccessary? public: - enum AlgoType { - INVALID, ICP, ICPINACTIVE, LUM, ELCH, LOOPTORO, LOOPHOGMAN, GRAPHTORO, - GRAPHHOGMAN - }; + enum AlgoType { INVALID, ICP, ICPINACTIVE, LUM, ELCH }; // delete copy-ctor and assignment, scans shouldn't be copied by basic class Scan(const Scan& other) = delete; @@ -108,7 +106,8 @@ public: virtual ~Scan(); - //! Holder of all scans - also used in transform for adding frames for each scan at the same time + //! Holder of all scans + // also used in transform for adding frames for each scan at the same time static std::vector allScans; /** @@ -286,7 +285,7 @@ public: Scan* Source, Scan* Target, int thread_num, int rnd, double max_dist_match2, double &sum, - double *centroid_m, double *centroid_d); + double *centroid_m, double *centroid_d, PairingMode pairing_mode = CLOSEST_POINT); static void getNoPairsSimple(std::vector &diff, Scan* Source, Scan* Target, int thread_num, @@ -302,7 +301,8 @@ public: int rnd, double max_dist_match2, double *sum, double centroid_m[OPENMP_NUM_THREADS][3], - double centroid_d[OPENMP_NUM_THREADS][3]); + double centroid_d[OPENMP_NUM_THREADS][3], + PairingMode pairing_mode); protected: /** @@ -351,6 +351,9 @@ protected: //! Flag whether "xyz reduced" has been initialized for this Scan yet bool m_has_reduced; + + //! Flag whether "normals" has been initialized for this Scan yet + bool m_has_normals; //! Reduction value used for octtree input double octtree_reduction_voxelSize; @@ -372,19 +375,31 @@ protected: /** * This function handles the reduction of points. It builds a lock for - * multithread-safety and calls caldReducedOnDemandPrivate. + * multithread-safety and calls calcReducedOnDemandPrivate. * * The intention is to reduce points, transforme them to the initial pose and * then copy them to original for the SearchTree. */ void calcReducedOnDemand(); + /** + * This function handles the computation of the normals. It builds a lock for + * multithread-safety and calls caldNormalsOnDemandPrivate. + */ + void calcNormalsOnDemand(); + //! Create specific SearchTree variants matching the capability of the Scan virtual void createSearchTreePrivate() = 0; //! Create reduced points in a multithread-safe environment matching the capability of the Scan virtual void calcReducedOnDemandPrivate() = 0; + //! Create normals in a multithread-safe environment matching the capability of the Scan + virtual void calcNormalsOnDemandPrivate() = 0; + + //! Creating normals + void calcNormals(); + //! Internal function of transform which alters the reduced points void transformReduced(const double alignxf[16]); @@ -392,11 +407,11 @@ protected: void transformMatrix(const double alignxf[16]); //@FIXME - public: +public: //! Creating reduced points void calcReducedPoints(); - protected: +protected: //! Copies reduced points to original points without any transformation. void copyReducedToOriginal(); @@ -410,7 +425,7 @@ private: public: //! Mutex for safely reducing points and creating the search tree just once in a multithreaded environment // it can not be compiled in win32 use boost 1.48, therefore we remeove it temporarily - boost::mutex m_mutex_reduction, m_mutex_create_tree; + boost::mutex m_mutex_reduction, m_mutex_create_tree, m_mutex_normals; }; #include "scan.icc" diff --git a/include/slam6d/searchTree.h b/include/slam6d/searchTree.h index dd04f6b..0e964de 100644 --- a/include/slam6d/searchTree.h +++ b/include/slam6d/searchTree.h @@ -12,6 +12,7 @@ using std::vector; #include "ptpair.h" #include "data_types.h" +#include "pairingMode.h" /** * @brief The tree structure @@ -38,6 +39,7 @@ class Scan; class SearchTree : public Tree { friend class Scan; public: + /** * Constructor (default) */ @@ -77,7 +79,9 @@ public: * @param threadNum If parallel threads share the search tree the thread num must be given * @return Pointer to closest point */ - virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const = 0 ; + virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const = 0; + + virtual double *FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const; virtual void getPtPairs(vector *pairs, double *source_alignxf, @@ -88,10 +92,10 @@ public: virtual void getPtPairs(vector *pairs, double *source_alignxf, - const DataXYZ& xyz_r, unsigned int startindex, unsigned int endindex, + const DataXYZ& xyz_r, const DataNormal& normal_r, unsigned int startindex, unsigned int endindex, int thread_num, int rnd, double max_dist_match2, double &sum, - double *centroid_m, double *centroid_d); + double *centroid_m, double *centroid_d, PairingMode pairing_mode = CLOSEST_POINT); }; #endif diff --git a/know_issues.txt b/know_issues.txt new file mode 100644 index 0000000..1184149 --- /dev/null +++ b/know_issues.txt @@ -0,0 +1,35 @@ +1. +--metascan segfaults when destroying the allscanlist. + +2. +scanserver segfaults whith reflectances sometimes. +E.g., + +bin/scanserver -c 3500 +and +bin/show -s 0 -e 1 -f riegl_txt --reflectance ~/dat/bremen_city --scanserver + +or +bin/scanserver +and +bin/slam6D -s 0 -e 1 -f uosr dat --scanserver + +3. +scan_red with panorama range image and cylindrical coordinates does +not work correctly. + +4. +fast_normals does not work on SRI yet. Commented out. + +5. +reflectance_reduced not in managedScan/scanserver + +6. +normals not integrated managedScan/scanserver + +7. +kdMeta/kdManaged does not support kNN nor range search yet + +8. +Make our own knn efficient, then get rid of ANN ;-) + diff --git a/src/normals/CMakeLists.txt b/src/normals/CMakeLists.txt index 2ad9074..ebb4bae 100755 --- a/src/normals/CMakeLists.txt +++ b/src/normals/CMakeLists.txt @@ -1,7 +1,7 @@ -IF(WITH_NORMALS) - add_executable(normals normals.cc) +add_library(normals normals.cc) +target_link_libraries(normals newmat scan ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS}) - FIND_PACKAGE(OpenCV REQUIRED) - - target_link_libraries(normals newmat scan ANN fbr_cv_io fbr_panorama ${Boost_LIBRARIES} ${OpenCV_LIBS}) -ENDIF(WITH_NORMALS) +IF(WITH_TOOLS) + add_executable(calc_normals calc_normals.cc) + target_link_libraries(calc_normals normals ${Boost_LIBRARIES}) +ENDIF(WITH_TOOLS) diff --git a/src/normals/calc_normals.cc b/src/normals/calc_normals.cc new file mode 100644 index 0000000..db01dc1 --- /dev/null +++ b/src/normals/calc_normals.cc @@ -0,0 +1,275 @@ +/** + * + * Copyright (C) Jacobs University Bremen + * + * @author Vaibhav Kumar Mehta + * @file calc_normals.cc + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include "slam6d/fbr/panorama.h" +#include + +#include + + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + +namespace po = boost::program_options; +using namespace std; + +enum normal_method {AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST}; + +/* + * validates normal calculation method specification + */ +void validate(boost::any& v, const std::vector& values, + normal_method*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + if(strcasecmp(arg.c_str(), "AKNN") == 0) v = AKNN; + else if(strcasecmp(arg.c_str(), "ADAPTIVE_AKNN") == 0) v = ADAPTIVE_AKNN; + else if(strcasecmp(arg.c_str(), "PANORAMA") == 0) v = PANORAMA; + else if(strcasecmp(arg.c_str(), "PANORAMA_FAST") == 0) v = PANORAMA_FAST; + else throw std::runtime_error(std::string("normal calculation method ") + arg + std::string(" is unknown")); +} + +/// validate IO types +void validate(boost::any& v, const std::vector& values, + IOType*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + try { + v = formatname_to_io_type(arg.c_str()); + } catch (...) { // runtime_error + throw std::runtime_error("Format " + arg + " unknown."); + } +} + +/// Parse commandline options +void parse_options(int argc, char **argv, int &start, int &end, bool &scanserver, int &max_dist, int &min_dist, string &dir, + IOType &iotype, int &k1, int &k2, normal_method &ntype,int &width,int &height) +{ + /// ---------------------------------- + /// set up program commandline options + /// ---------------------------------- + po::options_description cmd_options("Usage: calculateNormals where options are (default values in brackets)"); + cmd_options.add_options() + ("help,?", "Display this help message") + ("start,s", po::value(&start)->default_value(0), "Start at scan number ") + ("end,e", po::value(&end)->default_value(-1), "Stop at scan number ") + ("scanserver,S", po::value(&scanserver)->default_value(false), "Use the scanserver as an input method") + ("format,f", po::value(&iotype)->default_value(UOS), + "using shared library for input. (chose format from [uos|uosr|uos_map|" + "uos_rgb|uos_frames|uos_map_frames|old|rts|rts_map|ifp|" + "riegl_txt|riegl_rgb|riegl_bin|zahn|ply])") + ("max,M", po::value(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than 'units") + ("min,m", po::value(&min_dist)->default_value(-1),"neglegt all data points with a distance smaller than 'units") + ("normal,g", po::value(&ntype)->default_value(AKNN), "normal calculation method " + "(AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST)") + ("K1,k", po::value(&k1)->default_value(20), " value of K value used in the nearest neighbor search of ANN or" "kmin for k-adaptation") + ("K2,K", po::value(&k2)->default_value(20), " value of Kmax for k-adaptation") + ("width,w", po::value(&width)->default_value(1280),"width of panorama image") + ("height,h", po::value(&height)->default_value(960),"height of panorama image") + ; + + po::options_description hidden("Hidden options"); + hidden.add_options() + ("input-dir", po::value(&dir), "input dir"); + + po::positional_options_description pd; + pd.add("input-dir", 1); + + po::options_description all; + all.add(cmd_options).add(hidden); + + po::variables_map vmap; + po::store(po::command_line_parser(argc, argv).options(all).positional(pd).run(), vmap); + po::notify(vmap); + + if (vmap.count("help")) { + cout << cmd_options << endl << endl; + cout << "SAMPLE COMMAND FOR CALCULATING NORMALS" << endl; + cout << " bin/normals -s 0 -e 0 -f UOS -g AKNN -k 20 dat/" < &points, vector &normals, int scanNumber) +{ + string ofilename = dir + "/scan" + to_string(scanNumber, 3) + ".3d"; + ofstream normptsout(ofilename.c_str()); + + for (size_t i=0; i::iterator it = Scan::allScans.begin(); + int scanNumber = 0; + + for( ; it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + + // apply optional filtering + scan->setRangeFilter(max_dist, min_dist); + + const double* rPos = scan->get_rPos(); + const double* rPosTheta = scan->get_rPosTheta(); + + /// read scan into points + DataXYZ xyz(scan->get("xyz")); + vector points; + points.reserve(xyz.size()); + vector normals; + normals.reserve(xyz.size()); + + for(unsigned int j = 0; j < xyz.size(); j++) { + points.push_back(Point(xyz[j][0], xyz[j][1], xyz[j][2])); + } + + if(ntype == AKNN) + calculateNormalsApxKNN(normals,points, k1, rPos); + else if(ntype == ADAPTIVE_AKNN) + calculateNormalsAdaptiveApxKNN(normals,points, k1, k2, rPos); + else + { + // create panorama + fbr::panorama fPanorama(width, height, fbr::EQUIRECTANGULAR, 1, 0, fbr::EXTENDED); + fPanorama.createPanorama(scan2mat(scan)); + + // the range image has to be converted from float to uchar + img = fPanorama.getRangeImage(); + img = float2uchar(img, 0, 0.0); + + if(ntype == PANORAMA) + calculateNormalsPANORAMA(normals,points,fPanorama.getExtendedMap(), rPos); + else if(ntype == PANORAMA_FAST) + cout << "PANORAMA_FAST is not working yet" << endl; + // calculateNormalsFAST(normals,points,img,fPanorama.getExtendedMap()); + } + + // pose file (repeated for the number of segments + writePoseFiles(normdir, rPos, rPosTheta, scanNumber); + // scan files for all segments + writeScanFiles(normdir, points,normals,scanNumber); + + scanNumber++; + } + + // shutdown everything + if (scanserver) + ClientInterface::destroy(); + + Scan::closeDirectory(); + + cout << "Normal program end" << endl; + + return 0; +} diff --git a/src/normals/normals.cc b/src/normals/normals.cc index 846583c..b1f9264 100644 --- a/src/normals/normals.cc +++ b/src/normals/normals.cc @@ -1,129 +1,225 @@ /** - * + * * Copyright (C) Jacobs University Bremen * * @author Vaibhav Kumar Mehta + * @author Corneliu Claudiu Prodescu * @file normals.cc */ -#include -#include -#include -#include - -#include - -#include -#include -#include +#include +#include +#include "slam6d/io_types.h" +#include "slam6d/globals.icc" +#include "slam6d/scan.h" #include "slam6d/fbr/panorama.h" +#include "slam6d/kd.h" #include - -#include #include "newmat/newmat.h" #include "newmat/newmatap.h" +#include "normals/normals.h" + using namespace NEWMAT; +using namespace std; -#ifdef _MSC_VER -#define strcasecmp _stricmp -#define strncasecmp _strnicmp -#else -#include -#endif +////////////////////////////////////////////////////// +/////////////NORMALS USING AKNN METHOD //////////////// +/////////////////////////////////////////////////////// +void calculateNormalsApxKNN(vector &normals, + vector &points, + int k, + const double _rPos[3], + double eps) +{ + cout<<"Total number of points: "<& values, - normal_method*, int) { - if (values.size() == 0) - throw std::runtime_error("Invalid model specification"); - string arg = values.at(0); - if(strcasecmp(arg.c_str(), "AKNN") == 0) v = AKNN; - else if(strcasecmp(arg.c_str(), "ADAPTIVE_AKNN") == 0) v = ADAPTIVE_AKNN; - else if(strcasecmp(arg.c_str(), "PANORAMA") == 0) v = PANORAMA; - else if(strcasecmp(arg.c_str(), "PANORAMA_FAST") == 0) v = PANORAMA_FAST; - else throw std::runtime_error(std::string("normal calculation method ") + arg + std::string(" is unknown")); -} + } + ANNkd_tree t(pa, points.size(), 3); + ANNidxArray nidx = new ANNidx[nr_neighbors]; + ANNdistArray d = new ANNdist[nr_neighbors]; + + for (size_t i=0; i& values, - IOType*, int) { - if (values.size() == 0) - throw std::runtime_error("Invalid model specification"); - string arg = values.at(0); - try { - v = formatname_to_io_type(arg.c_str()); - } catch (...) { // runtime_error - throw std::runtime_error("Format " + arg + " unknown."); + A << 1.0/nr_neighbors * X.t() * X; + + EigenValues(A, D, U); + + //normal = eigenvector corresponding to lowest + //eigen value that is the 1st column of matrix U + ColumnVector n(3); + n(1) = U(1,1); + n(2) = U(2,1); + n(3) = U(3,1); + ColumnVector point_vector(3); + point_vector(1) = p[0] - rPos(1); + point_vector(2) = p[1] - rPos(2); + point_vector(3) = p[2] - rPos(3); + point_vector = point_vector / point_vector.NormFrobenius(); + Real angle = (n.t() * point_vector).AsScalar(); + if (angle < 0) { + n *= -1.0; + } + n = n / n.NormFrobenius(); + normals.push_back(Point(n(1), n(2), n(3))); } -} -/// Parse commandline options -void parse_options(int argc, char **argv, int &start, int &end, bool &scanserver, int &max_dist, int &min_dist, string &dir, - IOType &iotype, int &k1, int &k2, normal_method &ntype,int &width,int &height) + delete[] nidx; + delete[] d; + annDeallocPts(pa); +} +//////////////////////////////////////////////////////////////// +/////////////NORMALS USING ADAPTIVE AKNN METHOD //////////////// +//////////////////////////////////////////////////////////////// +void calculateNormalsAdaptiveApxKNN(vector &normals, + vector &points, + int kmin, + int kmax, + const double _rPos[3], + double eps) { - /// ---------------------------------- - /// set up program commandline options - /// ---------------------------------- - po::options_description cmd_options("Usage: calculateNormals where options are (default values in brackets)"); - cmd_options.add_options() - ("help,?", "Display this help message") - ("start,s", po::value(&start)->default_value(0), "Start at scan number ") - ("end,e", po::value(&end)->default_value(-1), "Stop at scan number ") - ("scanserver,S", po::value(&scanserver)->default_value(false), "Use the scanserver as an input method") - ("format,f", po::value(&iotype)->default_value(UOS), - "using shared library for input. (chose format from [uos|uosr|uos_map|" - "uos_rgb|uos_frames|uos_map_frames|old|rts|rts_map|ifp|" - "riegl_txt|riegl_rgb|riegl_bin|zahn|ply])") - ("max,M", po::value(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than 'units") - ("min,m", po::value(&min_dist)->default_value(-1),"neglegt all data points with a distance smaller than 'units") - ("normal,g", po::value(&ntype)->default_value(AKNN), "normal calculation method " - "(AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST)") - ("K1,k", po::value(&k1)->default_value(20), " value of K value used in the nearest neighbor search of ANN or" "kmin for k-adaptation") - ("K2,K", po::value(&k2)->default_value(20), " value of Kmax for k-adaptation") - ("width,w", po::value(&width)->default_value(1280),"width of panorama image") - ("height,h", po::value(&height)->default_value(960),"height of panorama image") - ; - - po::options_description hidden("Hidden options"); - hidden.add_options() - ("input-dir", po::value(&dir), "input dir"); - - po::positional_options_description pd; - pd.add("input-dir", 1); - - po::options_description all; - all.add(cmd_options).add(hidden); - - po::variables_map vmap; - po::store(po::command_line_parser(argc, argv).options(all).positional(pd).run(), vmap); - po::notify(vmap); - - if (vmap.count("help")) { - cout << cmd_options << endl << endl; - cout << "SAMPLE COMMAND FOR CALCULATING NORMALS" << endl; - cout << " bin/normals -s 0 -e 0 -f UOS -g AKNN -k 20 dat/" < 0.25 * e2) && (fabs(1.0 - (double)e2/(double)e3) < 0.25)) + break; + } + //normal = eigenvector corresponding to lowest + //eigen value that is the 1rd column of matrix U + ColumnVector n(3); + n(1) = U(1,1); + n(2) = U(2,1); + n(3) = U(3,1); + ColumnVector point_vector(3); + point_vector(1) = p[0] - rPos(1); + point_vector(2) = p[1] - rPos(2); + point_vector(3) = p[2] - rPos(3); + point_vector = point_vector / point_vector.NormFrobenius(); + Real angle = (n.t() * point_vector).AsScalar(); + if (angle < 0) { + n *= -1.0; + } + n = n / n.NormFrobenius(); + normals.push_back(Point(n(1), n(2), n(3))); + } + annDeallocPts(pa); } + + + /////////////////////////////////////////////////////// /////////////NORMALS USING AKNN METHOD //////////////// /////////////////////////////////////////////////////// -void calculateNormalsAKNN(vector &normals,vector &points, int k, const double _rPos[3] ) +void calculateNormalsKNN(vector &normals, + vector &points, + int k, + const double _rPos[3] ) { cout<<"Total number of points: "< &normals,vector &points, int k, c for (int i = 0; i < 3; ++i) rPos(i+1) = _rPos[i]; - ANNpointArray pa = annAllocPts(points.size(), 3); - for (size_t i=0; i temp = t.kNearestNeighbors(p, + nr_neighbors, + numeric_limits::max()); + nr_neighbors = temp.size(); + Point mean(0.0,0.0,0.0); Matrix X(nr_neighbors,3); SymmetricMatrix A(3); Matrix U(3,3); DiagonalMatrix D(3); + //calculate mean for all the points - for (int j=0; j &normals,vector &points, int k, c normals.push_back(Point(n(1), n(2), n(3))); } - delete[] nidx; - delete[] d; - annDeallocPts(pa); + + for (size_t i = 0; i < points.size(); ++i) + { + delete[] pa[i]; + } + delete[] pa; + } //////////////////////////////////////////////////////////////// /////////////NORMALS USING ADAPTIVE AKNN METHOD //////////////// //////////////////////////////////////////////////////////////// -void calculateNormalsAdaptiveAKNN(vector &normals,vector &points, - int kmin, int kmax, const double _rPos[3]) +void calculateNormalsAdaptiveKNN(vector &normals, + vector &points, + int kmin, + int kmax, + const double _rPos[3]) { ColumnVector rPos(3); for (int i = 0; i < 3; ++i) @@ -211,39 +322,47 @@ void calculateNormalsAdaptiveAKNN(vector &normals,vector &points, cout<<"Total number of points: "< temp = t.kNearestNeighbors(p, + nr_neighbors, + numeric_limits::max()); + nr_neighbors = temp.size(); + mean.x=0,mean.y=0,mean.z=0; //calculate mean for all the points for (int j=0; j &normals,vector &points, //calculate covariance = A for all the points for (int j = 0; j < nr_neighbors; ++j) { - X(j+1, 1) = points[nidx[j]].x - mean.x; - X(j+1, 2) = points[nidx[j]].y - mean.y; - X(j+1, 3) = points[nidx[j]].z - mean.z; + X(j+1, 1) = temp[j].x - mean.x; + X(j+1, 2) = temp[j].y - mean.y; + X(j+1, 3) = temp[j].z - mean.z; } A << 1.0/nr_neighbors * X.t() * X; @@ -268,9 +387,6 @@ void calculateNormalsAdaptiveAKNN(vector &normals,vector &points, e2 = D(2); e3 = D(3); - delete[] nidx; - delete[] d; - //We take the particular k if the second maximum eigen value //is at least 25 percent of the maximum eigen value if ((e1 > 0.25 * e2) && (fabs(1.0 - (double)e2/(double)e3) < 0.25)) @@ -295,120 +411,126 @@ void calculateNormalsAdaptiveAKNN(vector &normals,vector &points, n = n / n.NormFrobenius(); normals.push_back(Point(n(1), n(2), n(3))); } - annDeallocPts(pa); + + for (size_t i = 0; i < points.size(); ++i) + { + delete[] pa[i]; + } + delete[] pa; + } + /////////////////////////////////////////////////////// /////////////NORMALS USING IMAGE NEIGHBORS //////////// /////////////////////////////////////////////////////// void calculateNormalsPANORAMA(vector &normals, - vector &points, - vector< vector< vector< cv::Vec3f > > > extendedMap, - const double _rPos[3]) + vector &points, + vector< vector< vector< cv::Vec3f > > > extendedMap, + const double _rPos[3]) { ColumnVector rPos(3); for (int i = 0; i < 3; ++i) rPos(i+1) = _rPos[i]; - + cout<<"Total number of points: "< neighbors; for (size_t i=0; i< extendedMap.size(); i++) + { + for (size_t j=0; j= 0 && x < (int)extendedMap.size() && - y >= 0 && y < (int)extendedMap[x].size()) { - for (unsigned int k = 0; k < extendedMap[x][y].size(); k++) { - neighbors.push_back(extendedMap[x][y][k]); - } - } - } - - nr_neighbors = neighbors.size(); - cv::Vec3f p = extendedMap[i][j][0]; - - //if no neighbor point is found in the 4-neighboring pixels then normal is set to zero - if (nr_neighbors < 3) - { - points.push_back(Point(p[0], p[1], p[2])); - normals.push_back(Point(0.0,0.0,0.0)); - continue; - } - - //calculate mean for all the points - Matrix X(nr_neighbors,3); - SymmetricMatrix A(3); - Matrix U(3,3); - DiagonalMatrix D(3); - - //calculate mean for all the points - for(int k = 0; k < nr_neighbors; k++) - { - cv::Vec3f pp = neighbors[k]; - mean.x += pp[0]; - mean.y += pp[1]; - mean.z += pp[2]; - } - - mean.x /= nr_neighbors; - mean.y /= nr_neighbors; - mean.z /= nr_neighbors; - //calculate covariance = A for all the points - for (int i = 0; i < nr_neighbors; ++i) { - cv::Vec3f pp = neighbors[i]; - X(i+1, 1) = pp[0] - mean.x; - X(i+1, 2) = pp[1] - mean.y; - X(i+1, 3) = pp[2] - mean.z; - } - - A << 1.0/nr_neighbors * X.t() * X; - - EigenValues(A, D, U); - //normal = eigenvector corresponding to lowest - //eigen value that is the 1st column of matrix U - ColumnVector n(3); - n(1) = U(1,1); - n(2) = U(2,1); - n(3) = U(3,1); - ColumnVector point_vector(3); - point_vector(1) = p[0] - rPos(1); - point_vector(2) = p[1] - rPos(2); - point_vector(3) = p[2] - rPos(3); - point_vector = point_vector / point_vector.NormFrobenius(); - Real angle = (n.t() * point_vector).AsScalar(); - if (angle < 0) { - n *= -1.0; - } - n = n / n.NormFrobenius(); - - for (unsigned int k = 0; k < extendedMap[i][j].size(); k++) { - cv::Vec3f p = extendedMap[i][j][k]; - points.push_back(Point(p[0], p[1], p[2])); - normals.push_back(Point(n(1), n(2), n(3))); - } - } - } + if (extendedMap[i][j].size() == 0) continue; + neighbors.clear(); + Point mean(0.0,0.0,0.0); + + // Offset for neighbor computation + int offset[2][5] = {{-1,0,1,0,0},{0,-1,0,1,0}}; + + // Traversing all the cells in the extended map + for (int n = 0; n < 5; ++n) { + int x = i + offset[0][n]; + int y = j + offset[1][n]; + + // Copy the neighboring buckets into the vector + if (x >= 0 && x < (int)extendedMap.size() && + y >= 0 && y < (int)extendedMap[x].size()) { + for (unsigned int k = 0; k < extendedMap[x][y].size(); k++) { + neighbors.push_back(extendedMap[x][y][k]); + } + } + } + + nr_neighbors = neighbors.size(); + cv::Vec3f p = extendedMap[i][j][0]; + + //if no neighbor point is found in the 4-neighboring pixels then normal is set to zero + if (nr_neighbors < 3) + { + points.push_back(Point(p[0], p[1], p[2])); + normals.push_back(Point(0.0,0.0,0.0)); + continue; + } + + //calculate mean for all the points + Matrix X(nr_neighbors,3); + SymmetricMatrix A(3); + Matrix U(3,3); + DiagonalMatrix D(3); + + //calculate mean for all the points + for(int k = 0; k < nr_neighbors; k++) + { + cv::Vec3f pp = neighbors[k]; + mean.x += pp[0]; + mean.y += pp[1]; + mean.z += pp[2]; + } + + mean.x /= nr_neighbors; + mean.y /= nr_neighbors; + mean.z /= nr_neighbors; + //calculate covariance = A for all the points + for (int n = 0; n < nr_neighbors; ++n) { + cv::Vec3f pp = neighbors[n]; + X(n+1, 1) = pp[0] - mean.x; + X(n+1, 2) = pp[1] - mean.y; + X(n+1, 3) = pp[2] - mean.z; + } + + A << 1.0/nr_neighbors * X.t() * X; + + EigenValues(A, D, U); + //normal = eigenvector corresponding to lowest + //eigen value that is the 1st column of matrix U + ColumnVector n(3); + n(1) = U(1,1); + n(2) = U(2,1); + n(3) = U(3,1); + ColumnVector point_vector(3); + point_vector(1) = p[0] - rPos(1); + point_vector(2) = p[1] - rPos(2); + point_vector(3) = p[2] - rPos(3); + point_vector = point_vector / point_vector.NormFrobenius(); + Real angle = (n.t() * point_vector).AsScalar(); + if (angle < 0) { + n *= -1.0; + } + n = n / n.NormFrobenius(); + + for (unsigned int k = 0; k < extendedMap[i][j].size(); k++) { + cv::Vec3f p = extendedMap[i][j][k]; + points.push_back(Point(p[0], p[1], p[2])); + normals.push_back(Point(n(1), n(2), n(3))); + } + } + } } ////////////////////////////////////////////////////////////////////////////////////////////// ///////////FAST NORMALS USING PANORAMA EQUIRECTANGULAR RANGE IMAGE ////////////////////////// @@ -419,7 +541,7 @@ void calculateNormalsPANORAMA(vector &normals, cout<<"Total number of points: "< &normals, double n[3],m; nr_points = extendedMap[i][j].size(); if (nr_points == 0 ) continue; - + for (int k = 0; k< nr_points; k++) { cv::Vec3f p = extendedMap[i][j][k]; x = p[0]; y = p[1]; - z = p[2]; + z = p[2]; rho = sqrt(x*x + y*y + z*z); theta = atan(y/x); phi = atan(z/x); - + //Sobel Filter for the derivative dRdTheta = dRdPhi = 0.0; if (i == 0 || i == extendedMap.size()-1 || j == 0 || j == extendedMap[0].size()-1) - { + { points.push_back(Point(x, y, z)); - normals.push_back(Point(0.0,0.0,0.0)); - continue; - } + normals.push_back(Point(0.0,0.0,0.0)); + continue; + } dRdPhi += 10*img.at(i-1,j); dRdPhi += 3 *img.at(i-1,j-1); dRdPhi += 3 *img.at(i-1,j+1); dRdPhi -= 10*img.at(i+1,j); dRdPhi -= 3 *img.at(i+1,j-1); dRdPhi -= 3 *img.at(i+1,j+1); - + dRdTheta += 10*img.at(i,j-1); dRdTheta += 3 *img.at(i-1,j-1); dRdTheta += 3 *img.at(i+1,j-1); dRdTheta -= 10*img.at(i,j+1); dRdTheta -= 3 *img.at(i-1,j+1); - dRdTheta -= 3 *img.at(i+1,j+1); - - n[0] = cos(theta) * sin(phi) - sin(theta) * dRdTheta / rho / sin(phi) + + dRdTheta -= 3 *img.at(i+1,j+1); + + n[0] = cos(theta) * sin(phi) - sin(theta) * dRdTheta / rho / sin(phi) + cos(theta) * cos(phi) * dRdPhi / rho; - n[1] = sin(theta) * sin(phi) + cos(theta) * dRdTheta / rho / sin(phi) + + n[1] = sin(theta) * sin(phi) + cos(theta) * dRdTheta / rho / sin(phi) + sin(theta) * cos(phi) * dRdPhi / rho; - + n[2] = cos(phi) - sin(phi) * dRdPhi / rho; //n[2] = -n[2]; - + m = sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]); n[0] /= m; n[1] /= m; n[2] /= m; - + points.push_back(Point(x, y, z)); normals.push_back(Point(n[0],n[1],n[2])); - } - } - } - } -*/ - -/* - * retrieve a cv::Mat with x,y,z,r from a scan object - * functionality borrowed from scan_cv::convertScanToMat but this function - * does not allow a scanserver to be used, prints to stdout and can only - * handle a single scan - */ -cv::Mat scan2mat(Scan *source) -{ - DataXYZ xyz = source->get("xyz"); - - DataReflectance xyz_reflectance = source->get("reflectance"); - - unsigned int nPoints = xyz.size(); - cv::Mat scan(nPoints,1,CV_32FC(4)); - scan = cv::Scalar::all(0); - - cv::MatIterator_ it; - - it = scan.begin(); - for(unsigned int i = 0; i < nPoints; i++){ - float x, y, z, reflectance; - x = xyz[i][0]; - y = xyz[i][1]; - z = xyz[i][2]; - if(xyz_reflectance.size() != 0) - { - reflectance = xyz_reflectance[i]; - - //normalize the reflectance - reflectance += 32; - reflectance /= 64; - reflectance -= 0.2; - reflectance /= 0.3; - if (reflectance < 0) reflectance = 0; - if (reflectance > 1) reflectance = 1; - } - - (*it)[0] = x; - (*it)[1] = y; - (*it)[2] = z; - if(xyz_reflectance.size() != 0) - (*it)[3] = reflectance; - else - (*it)[3] = 0; - - ++it; } - return scan; -} -/* - * convert a matrix of float values (range image) to a matrix of unsigned - * eight bit characters using different techniques - */ -cv::Mat float2uchar(cv::Mat &source, bool logarithm, float cutoff) -{ - cv::Mat result(source.size(), CV_8U, cv::Scalar::all(0)); - float max = 0; - // find maximum value - if (cutoff == 0.0) { - // without cutoff, just iterate through all values to find the largest - for (cv::MatIterator_ it = source.begin(); - it != source.end(); ++it) { - float val = *it; - if (val > max) { - max = val; - } - } - } else { - // when a cutoff is specified, sort all the points by value and then - // specify the max so that values are larger than it - vector sorted(source.cols*source.rows); - int i = 0; - for (cv::MatIterator_ it = source.begin(); - it != source.end(); ++it, ++i) { - sorted[i] = *it; - } - std::sort(sorted.begin(), sorted.end()); - max = sorted[(int)(source.cols*source.rows*(1.0-cutoff))]; - cout << "A cutoff of " << cutoff << " resulted in a max value of " << max << endl; } - - cv::MatIterator_ src = source.begin(); - cv::MatIterator_ dst = result.begin(); - cv::MatIterator_ end = source.end(); - if (logarithm) { - // stretch values from 0 to max logarithmically over 0 to 255 - // using the logarithm allows to represent smaller values with more - // precision and larger values with less - max = log(max+1); - for (; src != end; ++src, ++dst) { - float val = (log(*src+1)*255.0)/max; - if (val > 255) - *dst = 255; - else - *dst = (uchar)val; - } - } else { - // stretch values from 0 to max linearly over 0 to 255 - for (; src != end; ++src, ++dst) { - float val = (*src*255.0)/max; - if (val > 255) - *dst = 255; - else - *dst = (uchar)val; - } } - return result; -} -/// Write a pose file with the specofied name -void writePoseFiles(string dir, const double* rPos, const double* rPosTheta,int scanNumber) -{ - string poseFileName = dir + "/scan" + to_string(scanNumber, 3) + ".pose"; - ofstream posout(poseFileName.c_str()); - - posout << rPos[0] << " " - << rPos[1] << " " - << rPos[2] << endl - << deg(rPosTheta[0]) << " " - << deg(rPosTheta[1]) << " " - << deg(rPosTheta[2]) << endl; - posout.clear(); - posout.close(); -} - -/// write scan files for all segments -void writeScanFiles(string dir, vector &points, vector &normals, int scanNumber) -{ - string ofilename = dir + "/scan" + to_string(scanNumber, 3) + ".3d"; - ofstream normptsout(ofilename.c_str()); - - for (size_t i=0; i::iterator it = Scan::allScans.begin(); - int scanNumber = 0; - - for( ; it != Scan::allScans.end(); ++it) { - Scan* scan = *it; - - // apply optional filtering - scan->setRangeFilter(max_dist, min_dist); - - const double* rPos = scan->get_rPos(); - const double* rPosTheta = scan->get_rPosTheta(); - - /// read scan into points - DataXYZ xyz(scan->get("xyz")); - vector points; - points.reserve(xyz.size()); - vector normals; - normals.reserve(xyz.size()); - - for(unsigned int j = 0; j < xyz.size(); j++) { - points.push_back(Point(xyz[j][0], xyz[j][1], xyz[j][2])); - } - - if(ntype == AKNN) - calculateNormalsAKNN(normals,points, k1, rPos); - else if(ntype == ADAPTIVE_AKNN) - calculateNormalsAdaptiveAKNN(normals,points, k1, k2, rPos); - else - { - // create panorama - fbr::panorama fPanorama(width, height, fbr::EQUIRECTANGULAR, 1, 0, fbr::EXTENDED); - fPanorama.createPanorama(scan2mat(scan)); - - // the range image has to be converted from float to uchar - img = fPanorama.getRangeImage(); - img = float2uchar(img, 0, 0.0); - - if(ntype == PANORAMA) - calculateNormalsPANORAMA(normals,points,fPanorama.getExtendedMap(), rPos); - else if(ntype == PANORAMA_FAST) - cout << "PANORAMA_FAST is not working yet" << endl; - // calculateNormalsFAST(normals,points,img,fPanorama.getExtendedMap()); - } - - // pose file (repeated for the number of segments - writePoseFiles(normdir, rPos, rPosTheta, scanNumber); - // scan files for all segments - writeScanFiles(normdir, points,normals,scanNumber); - - scanNumber++; - } - - // shutdown everything - if (scanserver) - ClientInterface::destroy(); - else - Scan::closeDirectory(); - - cout << "Normal program end" << endl; - - return 0; -} - +*/ diff --git a/src/scanio/CMakeLists.txt b/src/scanio/CMakeLists.txt index d70c639..bd48c5a 100644 --- a/src/scanio/CMakeLists.txt +++ b/src/scanio/CMakeLists.txt @@ -5,7 +5,7 @@ else(WIN32) endif(WIN32) set(SCANIO_LIBNAMES - uos uosr uos_rgb uos_rrgbt ks ks_rgb riegl_txt riegl_rgb rts velodyne + uos uosr uos_rgb uos_rrgbt xyzr ply ks ks_rgb riegl_txt riegl_rgb rts velodyne ) if(WITH_RIVLIB) diff --git a/src/scanio/scan_io_ply.cc b/src/scanio/scan_io_ply.cc new file mode 100644 index 0000000..3f9c228 --- /dev/null +++ b/src/scanio/scan_io_ply.cc @@ -0,0 +1,227 @@ +/* + * scan_io_ply implementation + * + * Copyright (C) Dorit Borrmann, Thomas Escher, Kai Lingemann, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + + +/** + * @file + * @brief Implementation of reading 3D scans + * @author Dorit Borrmann. Jacobs University Bremen, Germany. + * @author Kai Lingemann. Inst. of CS, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Jacobs University Bremen, Germany. + * @author Thomas Escher. Inst. of CS, University of Osnabrueck, Germany. + */ + +#include "scanio/scan_io_ply.h" +#include "slam6d/point.h" + +#include +using std::cout; +using std::cerr; +using std::endl; +#include +#include + +#ifdef _MSC_VER +#include +#endif + +#include +#include +using namespace boost::filesystem; + +#include "slam6d/globals.icc" + +#define DATA_PATH_PREFIX "scan" +#define DATA_PATH_SUFFIX ".ply" + +std::list ScanIO_ply::readDirectory(const char* dir_path, + unsigned int start, + unsigned int end) +{ + std::list identifiers; + for (unsigned int i = start; i <= end; ++i) { + // identifier is /d/d/d (000-999) + std::string identifier(to_string(i,3)); + // scan consists of data (.3d) and pose (.pose) files + path data(dir_path); + data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX); + // stop if part of a scan is missing or end by absence is detected + if (!exists(data)) + break; + identifiers.push_back(identifier); + } + return identifiers; +} + +void ScanIO_ply::readPose(const char* dir_path, + const char* identifier, + double* pose) +{ + for (unsigned int i = 0; i < 6; ++i) pose[i] = 0.0; +} + +bool ScanIO_ply::supports(IODataType type) +{ + return !!(type & (DATA_XYZ | DATA_REFLECTANCE | DATA_RGB)); +} + +void ScanIO_ply::readScan(const char* dir_path, + const char* identifier, + PointFilter& filter, + std::vector* xyz, + std::vector* rgb, + std::vector* reflectance, + std::vector* temperature, + std::vector* amplitude, + std::vector* type, + std::vector* deviation) +{ + unsigned int i; + + // error handling + path data_path(dir_path); + data_path /= path(std::string(DATA_PATH_PREFIX) + + identifier + DATA_PATH_SUFFIX); + if(!exists(data_path)) + throw std::runtime_error(std::string("There is no scan file for [") + + identifier + "] in [" + dir_path + "]"); + + // open data file + ifstream data_file; + data_file.open(data_path); + data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit); + + if(xyz != 0 && rgb != 0 && reflectance != 0) { + + // read ply file + bool binary = false; + char dummy[256]; + char str[20]; // whatever size + double matrix[16]; + int matrixPos = 0; + int nr; + float d1,d2,d3,d4; + + // header + int counter = -2; + do { + if (counter > -2) counter++; + if (data_file.good()) { + data_file.getline(dummy, 255); + } + if (strncmp(dummy, "format", 6) == 0) { + if (dummy[7] == 'a') binary = false; + else if (dummy[7] == 'b') binary = true; + else { cerr << "Don't recognize the format!" << endl; exit(1); } + } + else if (strncmp(dummy, "element vertex", 14) == 0) { + sscanf(dummy,"%s %*s %d",str,&nr); + counter++; + } + else if (strncmp(dummy, "matrix", 6) == 0) { + sscanf(dummy,"%s %f %f %f %f", str, &d1, &d2, &d3, &d4); + matrix[matrixPos++] = d1; + matrix[matrixPos++] = d2; + matrix[matrixPos++] = d3; + matrix[matrixPos++] = d4; + } + } while (!(strncmp(dummy, "end_header",10) == 0 || !data_file.good())); + + if (matrixPos > 0) { + double rPosTheta[3]; + double rPos[3]; + Matrix4ToEuler(matrix, rPosTheta, rPos); + } + + for (int i=0; i < nr; i++) { + Point p; + float data, confidence, intensity; + float dummy; + int r, g, b; + if (!binary) { + switch(counter) { + case 6: + case 12: + data_file >> p.z >> p.y >> p.x >> r >> g >> b; + break; + case 9: + data_file >> p.z >> p.y >> p.x + >> dummy >> dummy >> dummy + >> r >> g >> b; + break; + default: + data_file >> p.z >> p.x >> p.y >> confidence >> intensity; + break; + } + if(counter == 6 || counter == 9 || counter == 12) { + p.rgb[0] = (char)r; + p.rgb[1] = (char)g; + p.rgb[2] = (char)b; + } else { + p.reflectance = intensity; + } + } else { + data_file.read((char*)&data, sizeof(float)); + p.z = (double)data; + data_file.read((char*)&data, sizeof(float)); + p.x = (double)data; + data_file.read((char*)&data, sizeof(float)); + p.y = (double)data; + data_file.read((char*)&confidence, sizeof(float)); + data_file.read((char*)&intensity, sizeof(float)); + } + + reflectance->push_back(p.reflectance); + xyz->push_back(p.x * 100); + xyz->push_back(p.y * 100); + xyz->push_back(p.z * 100); + rgb->push_back(static_cast(p.rgb[0])); + rgb->push_back(static_cast(p.rgb[1])); + rgb->push_back(static_cast(p.rgb[2])); + } + } +} + + + +/** + * class factory for object construction + * + * @return Pointer to new object + */ +#ifdef _MSC_VER +extern "C" __declspec(dllexport) ScanIO* create() +#else +extern "C" ScanIO* create() +#endif +{ + return new ScanIO_ply; +} + + +/** + * class factory for object construction + * + * @return Pointer to new object + */ +#ifdef _MSC_VER +extern "C" __declspec(dllexport) void destroy(ScanIO *sio) +#else +extern "C" void destroy(ScanIO *sio) +#endif +{ + delete sio; +} + +#ifdef _MSC_VER +BOOL APIENTRY DllMain(HANDLE hModule, DWORD dwReason, LPVOID lpReserved) +{ + return TRUE; +} +#endif diff --git a/src/scanio/scan_io_xyzr.cc b/src/scanio/scan_io_xyzr.cc new file mode 100644 index 0000000..e8d1e58 --- /dev/null +++ b/src/scanio/scan_io_xyzr.cc @@ -0,0 +1,194 @@ +/* + * scan_io_xyzr implementation + * + * Copyright (C) Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + + +/** + * @file scan_io_xyzr.cc + * @brief IO of a 3D scan in xyz file format plus a reflectance/intensity + * @author Andreas Nuechter. Jacobs University Bremen, Germany. + */ + +#include "scanio/scan_io_xyzr.h" + +#include +using std::cout; +using std::cerr; +using std::endl; +#include + +#ifdef _MSC_VER +#include +#endif + +#include +#include +using namespace boost::filesystem; + +#include "slam6d/globals.icc" + +#define DATA_PATH_PREFIX "scan" +#define DATA_PATH_SUFFIX ".3d" +#define POSE_PATH_PREFIX "scan" +#define POSE_PATH_SUFFIX ".pose" + +std::list ScanIO_xyzr::readDirectory(const char* dir_path, + unsigned int start, + unsigned int end) +{ + std::list identifiers; + for(unsigned int i = start; i <= end; ++i) { + // identifier is /d/d/d (000-999) + std::string identifier(to_string(i,3)); + // scan consists of data (.3d) and pose (.pose) files + path data(dir_path); + data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX); + path pose(dir_path); + pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX); + // stop if part of a scan is missing or end by absence is detected + if(!exists(data) || !exists(pose)) + break; + identifiers.push_back(identifier); + } + return identifiers; +} + +void ScanIO_xyzr::readPose(const char* dir_path, + const char* identifier, + double* pose) +{ + unsigned int i; + + path pose_path(dir_path); + pose_path /= path(std::string(POSE_PATH_PREFIX) + + identifier + + POSE_PATH_SUFFIX); + if(!exists(pose_path)) + throw std::runtime_error(std::string("There is no pose file for [") + + identifier + "] in [" + dir_path + "]"); + + // open pose file + ifstream pose_file(pose_path); + + // if the file is open, read contents + if(pose_file.good()) { + // read 6 plain doubles + for(i = 0; i < 6; ++i) pose_file >> pose[i]; + pose_file.close(); + + // convert angles from deg to rad + for(i = 3; i < 6; ++i) pose[i] = rad(pose[i]); + } else { + throw std::runtime_error(std::string("Pose file could not be opened for [") + + identifier + "] in [" + + dir_path + "]"); + } +} + +bool ScanIO_xyzr::supports(IODataType type) +{ + return !!(type & ( DATA_REFLECTANCE | DATA_XYZ )); +} + +void ScanIO_xyzr::readScan(const char* dir_path, + const char* identifier, + PointFilter& filter, + std::vector* xyz, + std::vector* rgb, + std::vector* reflectance, + std::vector* temperature, + std::vector* amplitude, + std::vector* type, + std::vector* deviation) +{ + unsigned int i; + + // error handling + path data_path(dir_path); + data_path /= path(std::string(DATA_PATH_PREFIX) + + identifier + + DATA_PATH_SUFFIX); + if(!exists(data_path)) + throw std::runtime_error(std::string("There is no scan file for [") + + identifier + "] in [" + + dir_path + "]"); + + if(xyz != 0) { + // open data file + ifstream data_file(data_path); + data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit); + + // overread the first line ignoring the header information + char dummy[255]; + data_file.getline(dummy, 255); + + // read points and reflectance/intensity/temperature value + double point[3]; + float reflection; + while(data_file.good()) { + try { + for(i = 0; i < 3; ++i) data_file >> point[i]; +/* + point[0] -= 485531.0; + point[1] -= 5882078.400; + point[2] -= 52; +*/ + std::swap(point[2], point[1]); + data_file >> reflection; + } catch(std::ios_base::failure& e) { + break; + } + + // apply filter then insert point and reflectance + if(filter.check(point)) { + for(i = 0; i < 3; ++i) xyz->push_back(point[i]); + reflectance->push_back(reflection); + } + } + data_file.close(); + } +} + + + +/** + * class factory for object construction + * + * @return Pointer to new object + */ +#ifdef _MSC_VER +extern "C" __declspec(dllexport) ScanIO* create() +#else +extern "C" ScanIO* create() +#endif +{ + return new ScanIO_xyzr; +} + + +/** + * class factory for object construction + * + * @return Pointer to new object + */ +#ifdef _MSC_VER +extern "C" __declspec(dllexport) void destroy(ScanIO *sio) +#else +extern "C" void destroy(ScanIO *sio) +#endif +{ + delete sio; +} + +#ifdef _MSC_VER +BOOL APIENTRY DllMain(HANDLE hModule, DWORD dwReason, LPVOID lpReserved) +{ + return TRUE; +} +#endif + diff --git a/src/segmentation/CMakeLists.txt b/src/segmentation/CMakeLists.txt index 820ddb7..0ef4583 100644 --- a/src/segmentation/CMakeLists.txt +++ b/src/segmentation/CMakeLists.txt @@ -1,10 +1,12 @@ IF(WITH_SEGMENTATION) +IF(OpenCV_VERSION_MAJOR GREATER 1 AND OpenCV_VERSION_MINOR GREATER 2) add_executable(scan2segments scan2segments.cc ../slam6d/fbr/fbr_global.cc) + target_link_libraries(scan2segments scan ANN fbr_cv_io fbr_panorama fbr_feature fbr_feature_matcher fbr_registration ${Boost_LIBRARIES} ${OpenCV_LIBS}) +ELSE(OpenCV_VERSION_MAJOR GREATER 1 AND OpenCV_VERSION_MINOR GREATER 2) + MESSAGE("OpenCV Version > 2.2 required for scan2segmentation") +ENDIF(OpenCV_VERSION_MAJOR GREATER 1 AND OpenCV_VERSION_MINOR GREATER 2) add_executable(fhsegmentation fhsegmentation.cc FHGraph.cc disjoint-set.cc segment-graph.cc) - FIND_PACKAGE(OpenCV REQUIRED) - - target_link_libraries(scan2segments scan ANN fbr_cv_io fbr_panorama fbr_feature fbr_feature_matcher fbr_registration ${Boost_LIBRARIES} ${OpenCV_LIBS}) target_link_libraries(fhsegmentation scan ANN ${Boost_LIBRARIES} ${OpenCV_LIBS}) diff --git a/src/show/CMakeLists.txt b/src/show/CMakeLists.txt index 4e43fdf..73e2fac 100644 --- a/src/show/CMakeLists.txt +++ b/src/show/CMakeLists.txt @@ -1,4 +1,4 @@ -SET(SHOW_LIBS glui scan ANN ${OPENGL_LIBRARIES}) +SET(SHOW_LIBS glui scan ANN ${OPENGL_LIBRARIES} normals newmat) IF(WIN32) IF( CMAKE_SIZEOF_VOID_P EQUAL 8 ) SET(SHOW_LIBS ${SHOW_LIBS} ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/freeglut.lib XGetopt) diff --git a/src/show/compacttree.cc b/src/show/compacttree.cc index 11318b0..cb952f7 100644 --- a/src/show/compacttree.cc +++ b/src/show/compacttree.cc @@ -686,7 +686,9 @@ shortpointrep* compactTree::createPoints(lint length) { shortpointrep *points = alloc->allocate (POINTDIM*length); return points; } -void compactTree::deserialize(std::string filename ) { + +void compactTree::deserialize(std::string filename) +{ char buffer[sizeof(float) * 20]; float *p = reinterpret_cast(buffer); diff --git a/src/show/scancolormanager.cc b/src/show/scancolormanager.cc index 7d44198..eaf97d4 100644 --- a/src/show/scancolormanager.cc +++ b/src/show/scancolormanager.cc @@ -160,18 +160,6 @@ ScanColorManager::ScanColorManager(unsigned int _buckets, PointType type, bool a case Scan::ELCH: glColor4d(0.0, 1.0,0.0, 1.0); break; - case Scan::LOOPTORO: - glColor4d(0.0, 0.0, 1.0, 1.0); - break; - case Scan::LOOPHOGMAN: - glColor4d(0.0, 1.0, 1.0, 1.0); - break; - case Scan::GRAPHTORO: - glColor4d(1.0, 0.0, 1.0, 1.0); - break; - case Scan::GRAPHHOGMAN: - glColor4d(1.0, 1.0, 0.0, 1.0); - break; default: glColor4d(1.0, 1.0, 1.0, 1.0); break; diff --git a/src/show/show_common.cc b/src/show/show_common.cc index 8cd6cbb..489e7ce 100644 --- a/src/show/show_common.cc +++ b/src/show/show_common.cc @@ -626,9 +626,7 @@ void setResetView(int origin) { // set origin to the center of mass of all scans for (size_t i = 0; i < octpts.size(); ++i) { vector points; -#ifdef USE_COMPACT_TREE - ((compactTree*)octpts[i])->AllPoints( points ); -#else +#ifndef USE_COMPACT_TREE BOctTree* cur_tree = ((Show_BOctTree*)octpts[i])->getTree(); cur_tree->AllPoints( points ); #endif @@ -849,8 +847,6 @@ void initShow(int argc, char **argv){ string scanFileName = dir + "scan" + to_string(start,3) + ".oct"; cout << "Getting point information from " << scanFileName << endl; cout << "Attention! All subsequent oct-files must be of the same type!" << endl; - - pointtype = BOctTree::readType(scanFileName); } scan_dir = dir; @@ -893,7 +889,7 @@ void initShow(int argc, char **argv){ cout << "Creating display octrees.." << endl; #endif - if(loadOct) cout << "Loading octtrees from file where possible instead of creating them from scans." << endl; + if (loadOct) cout << "Loading octtrees from file where possible instead of creating them from scans." << endl; // for managed scans the input phase needs to know how much it can handle std::size_t free_mem = 0; @@ -907,14 +903,26 @@ void initShow(int argc, char **argv){ #ifdef USE_COMPACT_TREE // FIXME: change compact tree, then this case can be removed compactTree* tree; try { - if (red > 0) { // with reduction, only xyz points - DataXYZ xyz_r(scan->get("xyz reduced show")); - tree = new compactTree(PointerArray(xyz_r).get(), xyz_r.size(), voxelSize, pointtype, cm); - } else { // without reduction, xyz + attribute points - sfloat** pts = pointtype.createPointArray(scan); - unsigned int nrpts = scan->size("xyz"); - tree = new compactTree(pts, nrpts, voxelSize, pointtype, cm); - for(unsigned int i = 0; i < nrpts; ++i) delete[] pts[i]; delete[] pts; + if (loadOct) { + string sfName = dir + "scan" + to_string(i,3) + ".oct"; + cout << "Load " << sfName; + tree = new compactTree(sfName, cm); + cout << " done." << endl; + } else { + if (red > 0) { // with reduction, only xyz points + DataXYZ xyz_r(scan->get("xyz reduced show")); + tree = new compactTree(PointerArray(xyz_r).get(), xyz_r.size(), voxelSize, pointtype, cm); + } else { // without reduction, xyz + attribute points + sfloat** pts = pointtype.createPointArray(scan); + unsigned int nrpts = scan->size("xyz"); + tree = new compactTree(pts, nrpts, voxelSize, pointtype, cm); + for(unsigned int i = 0; i < nrpts; ++i) delete[] pts[i]; + delete[] pts; + if (saveOct) { + string sfName = dir + "scan" + to_string(i,3) + ".oct"; + tree->serialize(sfName); + } + } } } catch(...) { cout << "Scan " << i << " could not be loaded into memory, stopping here." << endl; @@ -1026,7 +1034,7 @@ set heuristic, do locking, catch exception, reset heuristic to default or old // load frames now that we know how many scans we actually loaded unsigned int real_end = min((unsigned int)(end), - (unsigned int)(start + octpts.size() - 1)); + (unsigned int)(start + octpts.size() - 1)); if(readFrames(dir, start, real_end, readInitial, type)) generateFrames(start, real_end, true); diff --git a/src/slam6d/CMakeLists.txt b/src/slam6d/CMakeLists.txt index a60eb03..a595d0e 100644 --- a/src/slam6d/CMakeLists.txt +++ b/src/slam6d/CMakeLists.txt @@ -1,7 +1,6 @@ ### TOOLS IF(WITH_TOOLS) - FIND_PACKAGE(OpenCV REQUIRED) ### SCAN_RED add_executable(scan_red scan_red.cc fbr/fbr_global.cc fbr/panorama.cc fbr/scan_cv.cc) @@ -55,13 +54,14 @@ IF(WITH_TOOLS) target_link_libraries(riegl2frames XGetopt ${Boost_LIBRARIES}) target_link_libraries(toGlobal XGetopt ${Boost_LIBRARIES}) ENDIF(WIN32) + ENDIF(WITH_TOOLS) ### SCANLIB SET(SCANLIB_SRCS kd.cc kdManaged.cc kdMeta.cc graphSlam6D.cc - graph.cc icp6Dapx.cc icp6D.cc icp6Dsvd.cc + graph.cc icp6D.cc icp6Dapx.cc icp6Dsvd.cc icp6Dortho.cc icp6Dquat.cc icp6Dhelix.cc icp6Dlumeuler.cc icp6Dlumquat.cc icp6Ddual.cc lum6Deuler.cc lum6Dquat.cc ghelix6DQ2.cc gapx6D.cc ann_kd.cc elch6D.cc @@ -77,7 +77,9 @@ endif(WITH_METRICS) add_library(scan STATIC ${SCANLIB_SRCS}) -target_link_libraries(scan scanclient scanio) +FIND_PACKAGE(OpenCV REQUIRED) + +target_link_libraries(scan scanclient scanio normals) IF(UNIX) target_link_libraries(scan dl) diff --git a/src/slam6d/basicScan.cc b/src/slam6d/basicScan.cc index 4d56468..2849db2 100644 --- a/src/slam6d/basicScan.cc +++ b/src/slam6d/basicScan.cc @@ -242,6 +242,8 @@ DataPointer BasicScan::get(const std::string& identifier) if(identifier == "amplitude") get(DATA_AMPLITUDE); else if(identifier == "type") get(DATA_TYPE); else if(identifier == "deviation") get(DATA_DEVIATION); else + // normals on demand + if(identifier == "normal") calcNormalsOnDemand(); else // reduce on demand if(identifier == "xyz reduced") calcReducedOnDemand(); else if(identifier == "xyz reduced original") calcReducedOnDemand(); else @@ -326,12 +328,20 @@ void BasicScan::createSearchTreePrivate() void BasicScan::calcReducedOnDemandPrivate() { - // create reduced points and transform to initial position, save a copy of this for SearchTree + // create reduced points and transform to initial position, + // save a copy of this for SearchTree calcReducedPoints(); transformReduced(transMatOrg); copyReducedToOriginal(); } +void BasicScan::calcNormalsOnDemandPrivate() +{ + // create normals + calcNormals(); +} + + void BasicScan::createANNTree() { // TODO: metrics diff --git a/src/slam6d/fbr/CMakeLists.txt b/src/slam6d/fbr/CMakeLists.txt index 000bf21..9f41007 100644 --- a/src/slam6d/fbr/CMakeLists.txt +++ b/src/slam6d/fbr/CMakeLists.txt @@ -1,14 +1,11 @@ -IF(WITH_FBR) -FIND_PACKAGE(OpenCV REQUIRED) - SET(FBR_IO_SRC scan_cv.cc) add_library(fbr_cv_io STATIC ${FBR_IO_SRC}) SET(FBR_PANORAMA_SRC panorama.cc) -#add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC}) add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC} fbr_global.cc) - +IF(WITH_FBR) +IF(OpenCV_VERSION_MAJOR GREATER 1 AND OpenCV_VERSION_MINOR GREATER 2) SET(FBR_FEATURE_SRC feature.cc) add_library(fbr_feature STATIC ${FBR_FEATURE_SRC}) @@ -34,4 +31,7 @@ add_library(fbr_s SHARED ${FBR_SRC}) target_link_libraries(fbr_s scan_s ANN_s ${OpenCV_LIBS}) ENDIF(EXPORT_SHARED_LIBS) +ELSE(OpenCV_VERSION_MAJOR GREATER 1 AND OpenCV_VERSION_MINOR GREATER 2) + MESSAGE("OpenCV Version > 2.2 required for FBR") +ENDIF(OpenCV_VERSION_MAJOR GREATER 1 AND OpenCV_VERSION_MINOR GREATER 2) ENDIF(WITH_FBR) diff --git a/src/slam6d/icp6D.cc b/src/slam6d/icp6D.cc index baf8cbd..52a726b 100644 --- a/src/slam6d/icp6D.cc +++ b/src/slam6d/icp6D.cc @@ -92,7 +92,8 @@ icp6D::icp6D(icp6Dminimizer *my_icp6Dminimizer, double max_dist_match, * @param CurrentScan The current scan thas is to be matched * @return The number of iterations done in this matching run */ -int icp6D::match(Scan* PreviousScan, Scan* CurrentScan) +int icp6D::match(Scan* PreviousScan, Scan* CurrentScan, + PairingMode pairing_mode) { // If ICP shall not be applied, then just write // the identity matrix @@ -148,7 +149,7 @@ int icp6D::match(Scan* PreviousScan, Scan* CurrentScan) Scan::getPtPairsParallel(pairs, PreviousScan, CurrentScan, thread_num, step, rnd, max_dist_match2, - sum, centroid_m, centroid_d); + sum, centroid_m, centroid_d, pairing_mode); n[thread_num] = (unsigned int)pairs[thread_num].size(); @@ -214,7 +215,7 @@ int icp6D::match(Scan* PreviousScan, Scan* CurrentScan) vector pairs; Scan::getPtPairs(&pairs, PreviousScan, CurrentScan, 0, rnd, - max_dist_match2, ret, centroid_m, centroid_d); + max_dist_match2, ret, centroid_m, centroid_d, pairing_mode); // do we have enough point pairs? if (pairs.size() > 3) { @@ -281,7 +282,7 @@ double icp6D::Point_Point_Error(Scan* PreviousScan, Scan* CurrentScan, double ma Scan::getPtPairsParallel(pairs, PreviousScan, CurrentScan, thread_num, step, rnd, sqr(max_dist_match), - sum, centroid_m, centroid_d); + sum, centroid_m, centroid_d, CLOSEST_POINT); } @@ -299,7 +300,8 @@ double icp6D::Point_Point_Error(Scan* PreviousScan, Scan* CurrentScan, double ma double centroid_d[3] = {0.0, 0.0, 0.0}; vector pairs; - Scan::getPtPairs(&pairs, PreviousScan, CurrentScan, 0, rnd, sqr(max_dist_match),error, centroid_m, centroid_d); + Scan::getPtPairs(&pairs, PreviousScan, CurrentScan, 0, rnd, sqr(max_dist_match), + error, centroid_m, centroid_d, CLOSEST_POINT); // getPtPairs computes error as sum of squared distances error = 0; @@ -322,7 +324,7 @@ double icp6D::Point_Point_Error(Scan* PreviousScan, Scan* CurrentScan, double ma * * @param allScans Contains all necessary scans. */ -void icp6D::doICP(vector allScans) +void icp6D::doICP(vector allScans, PairingMode pairing_mode) { double id[16]; M4identity(id); @@ -346,12 +348,12 @@ void icp6D::doICP(vector allScans) if (i > 0) { if (meta) { - match(my_MetaScan, CurrentScan); + match(my_MetaScan, CurrentScan, pairing_mode); } else if (cad_matching) { - match(allScans[0], CurrentScan); + match(allScans[0], CurrentScan, pairing_mode); } else { - match(PreviousScan, CurrentScan); + match(PreviousScan, CurrentScan, pairing_mode); } } diff --git a/src/slam6d/kd.cc b/src/slam6d/kd.cc index cea3058..7577d9c 100644 --- a/src/slam6d/kd.cc +++ b/src/slam6d/kd.cc @@ -9,9 +9,11 @@ /** @file * @brief An optimized k-d tree implementation - * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. - * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. - * @author Thomas Escher Institute of Computer Science, University of Osnabrueck, Germany. + * @author Remus Dumitru. Jacobs University Bremen, Germany + * @author Corneliu-Claudiu Prodescu. Jacobs University Bremen, Germany + * @author Andreas Nuechter. Jacobs University Bremen, Germany. + * @author Kai Lingemann. Inst. of CS, University of Osnabrueck, Germany. + * @author Thomas Escher Inst. of CS, University of Osnabrueck, Germany. */ #ifdef _MSC_VER @@ -29,6 +31,8 @@ using std::endl; using std::swap; #include #include +#include +#include // KDtree class static variables template @@ -59,7 +63,9 @@ KDtree::~KDtree() * @param threadNum Thread number, for parallelization * @return Pointer to the closest point */ -double *KDtree::FindClosest(double *_p, double maxdist2, int threadNum) const +double *KDtree::FindClosest(double *_p, + double maxdist2, + int threadNum) const { params[threadNum].closest = 0; params[threadNum].closest_d2 = maxdist2; @@ -67,3 +73,57 @@ double *KDtree::FindClosest(double *_p, double maxdist2, int threadNum) const _FindClosest(Void(), threadNum); return params[threadNum].closest; } + +double *KDtree::FindClosestAlongDir(double *_p, + double *_dir, + double maxdist2, + int threadNum) const +{ + params[threadNum].closest = NULL; + params[threadNum].closest_d2 = maxdist2; + params[threadNum].p = _p; + params[threadNum].dir = _dir; + _FindClosestAlongDir(Void(), threadNum); + return params[threadNum].closest; +} + +vector KDtree::kNearestNeighbors(double *_p, + int k, + double sqRad2, + int threadNum) const +{ + vector result; + params[threadNum].closest = 0; + params[threadNum].closest_d2 = sqRad2; + params[threadNum].p = _p; + params[threadNum].heap.clear(); + _KNNSearch(Void(), threadNum); + + while (k > 0 && params[threadNum].heap.empty() == false) { + Point pt = params[threadNum].heap.front().first; + result.push_back(pt); + std::pop_heap(params[threadNum].heap.begin(), params[threadNum].heap.end(), PointCompare()); + params[threadNum].heap.pop_back(); + k--; + } + + return result; +} + +vector KDtree::fixedRangeSearch(double *_p, + double sqRad2, + int threadNum) const +{ + vector result; + params[threadNum].closest = 0; + params[threadNum].closest_d2 = sqRad2; + params[threadNum].p = _p; + params[threadNum].heap.clear(); + _FixedRangeSearch(Void(), threadNum); + + for (vector >::iterator it = params[threadNum].heap.begin(); it != params[threadNum].heap.end(); ++it) { + result.push_back(it->first); + } + + return result; +} diff --git a/src/slam6d/kdManaged.cc b/src/slam6d/kdManaged.cc index 562ac44..ec7c584 100644 --- a/src/slam6d/kdManaged.cc +++ b/src/slam6d/kdManaged.cc @@ -60,6 +60,16 @@ double* KDtreeManaged::FindClosest(double *_p, double maxdist2, int threadNum) c return params[threadNum].closest; } +double* KDtreeManaged::FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const +{ + params[threadNum].closest = NULL; + params[threadNum].closest_d2 = maxdist2; + params[threadNum].p = _p; + params[threadNum].dir = _dir; + _FindClosestAlongDir(*m_data, threadNum); + return params[threadNum].closest; +} + void KDtreeManaged::lock() { boost::lock_guard lock(m_mutex_locking); diff --git a/src/slam6d/kdMeta.cc b/src/slam6d/kdMeta.cc index 8247522..65e0b52 100644 --- a/src/slam6d/kdMeta.cc +++ b/src/slam6d/kdMeta.cc @@ -97,6 +97,16 @@ double* KDtreeMetaManaged::FindClosest(double *_p, double maxdist2, int threadNu return params[threadNum].closest; } +double* KDtreeMetaManaged::FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const +{ + params[threadNum].closest = NULL; + params[threadNum].closest_d2 = maxdist2; + params[threadNum].p = _p; + params[threadNum].dir = _dir; + _FindClosestAlongDir(m_data, threadNum); + return params[threadNum].closest; +} + void KDtreeMetaManaged::lock() { boost::lock_guard lock(m_mutex_locking); diff --git a/src/slam6d/point_type.cc b/src/slam6d/point_type.cc index 893841f..be32fb5 100644 --- a/src/slam6d/point_type.cc +++ b/src/slam6d/point_type.cc @@ -46,18 +46,25 @@ PointType::PointType(unsigned int _types) : types(_types) { pointdim = 3; if (types & PointType::USE_REFLECTANCE) dimensionmap[1] = pointdim++; - if (types & PointType::USE_TEMPERATURE) dimensionmap[2] = pointdim++; - if (types & PointType::USE_AMPLITUDE) dimensionmap[3] = pointdim++; - if (types & PointType::USE_DEVIATION) dimensionmap[4] = pointdim++; - if (types & PointType::USE_TYPE) dimensionmap[5] = pointdim++; - if (types & PointType::USE_COLOR) dimensionmap[6] = pointdim++; - if (types & PointType::USE_TIME) dimensionmap[7] = pointdim++; - if (types & PointType::USE_INDEX) dimensionmap[8] = pointdim++; + if (types & PointType::USE_NORMAL) { + pointdim += 3; + dimensionmap[2] = pointdim; + } + if (types & PointType::USE_TEMPERATURE) dimensionmap[3] = pointdim++; + if (types & PointType::USE_AMPLITUDE) dimensionmap[4] = pointdim++; + if (types & PointType::USE_DEVIATION) dimensionmap[5] = pointdim++; + if (types & PointType::USE_TYPE) dimensionmap[6] = pointdim++; + if (types & PointType::USE_COLOR) dimensionmap[7] = pointdim++; + if (types & PointType::USE_TIME) dimensionmap[8] = pointdim++; + if (types & PointType::USE_INDEX) dimensionmap[9] = pointdim++; } bool PointType::hasReflectance() { return hasType(USE_REFLECTANCE); } +bool PointType::hasNormal() { + return hasType(USE_NORMAL); +} bool PointType::hasTemperature() { return hasType(USE_TEMPERATURE); } @@ -116,18 +123,20 @@ unsigned int PointType::getType(unsigned int type) { return dimensionmap[0]; } else if (type == USE_REFLECTANCE) { return dimensionmap[1]; - } else if (type == USE_TEMPERATURE) { + } else if (type == USE_NORMAL) { return dimensionmap[2]; - } else if (type == USE_AMPLITUDE) { + } else if (type == USE_TEMPERATURE) { return dimensionmap[3]; - } else if (type == USE_DEVIATION) { + } else if (type == USE_AMPLITUDE) { return dimensionmap[4]; - } else if (type == USE_TYPE) { + } else if (type == USE_DEVIATION) { return dimensionmap[5]; - } else if (type == USE_COLOR) { + } else if (type == USE_TYPE) { return dimensionmap[6]; - } else if (type == USE_TIME) { + } else if (type == USE_COLOR) { return dimensionmap[7]; + } else if (type == USE_TIME) { + return dimensionmap[8]; } else { return 0; } @@ -155,14 +164,15 @@ bool PointType::hasType(unsigned int type) { const unsigned int PointType::USE_NONE = 0; const unsigned int PointType::USE_REFLECTANCE = 1; -const unsigned int PointType::USE_TEMPERATURE = 2; -const unsigned int PointType::USE_AMPLITUDE = 4; -const unsigned int PointType::USE_DEVIATION = 8; -const unsigned int PointType::USE_HEIGHT = 16; -const unsigned int PointType::USE_TYPE = 32; -const unsigned int PointType::USE_COLOR = 64; -const unsigned int PointType::USE_TIME = 128; -const unsigned int PointType::USE_INDEX = 256; +const unsigned int PointType::USE_NORMAL = 2; +const unsigned int PointType::USE_TEMPERATURE = 4; +const unsigned int PointType::USE_AMPLITUDE = 8; +const unsigned int PointType::USE_DEVIATION = 16; +const unsigned int PointType::USE_HEIGHT = 32; +const unsigned int PointType::USE_TYPE = 64; +const unsigned int PointType::USE_COLOR = 128; +const unsigned int PointType::USE_TIME = 256; +const unsigned int PointType::USE_INDEX = 512; void PointType::useScan(Scan* scan) diff --git a/src/slam6d/scan.cc b/src/slam6d/scan.cc index 94bc096..54dea9e 100644 --- a/src/slam6d/scan.cc +++ b/src/slam6d/scan.cc @@ -17,6 +17,8 @@ #include "slam6d/Boctree.h" #include "slam6d/globals.icc" +#include "normals/normals.h" + #ifdef WITH_METRICS #include "slam6d/metrics.h" #endif @@ -37,7 +39,7 @@ bool Scan::scanserver = false; void Scan::openDirectory(bool scanserver, const std::string& path, IOType type, - int start, int end) + int start, int end) { Scan::scanserver = scanserver; if(scanserver) @@ -145,7 +147,9 @@ void Scan::toGlobal() { */ void Scan::createSearchTree() { - // multiple threads will call this function at the same time because they all work on one pair of Scans, just let the first one (who sees a nullpointer) do the creation + // multiple threads will call this function at the same time because they + // all work on one pair of Scans, just let the first one (who sees a nullpointer) + // do the creation boost::lock_guard lock(m_mutex_create_tree); if(kd != 0) return; @@ -165,7 +169,9 @@ void Scan::createSearchTree() void Scan::calcReducedOnDemand() { - // multiple threads will call this function at the same time because they all work on one pair of Scans, just let the first one (who sees count as zero) do the reduction + // multiple threads will call this function at the same time + // because they all work on one pair of Scans, + // just let the first one (who sees count as zero) do the reduction boost::lock_guard lock(m_mutex_reduction); if(m_has_reduced) return; @@ -182,6 +188,17 @@ void Scan::calcReducedOnDemand() #endif //WITH_METRICS } +void Scan::calcNormalsOnDemand() +{ + // multiple threads will call this function at the same time + // because they all work on one pair of Scans, + // just let the first one (who sees count as zero) do the reduction + boost::lock_guard lock(m_mutex_normals); + if(m_has_normals) return; + calcNormalsOnDemandPrivate(); + m_has_normals = true; +} + void Scan::copyReducedToOriginal() { #ifdef WITH_METRICS @@ -223,7 +240,33 @@ void Scan::copyOriginalToReduced() } - +/** + * Computes normals for all points + */ +void Scan::calcNormals() +{ + cout << "calcNormals" << endl; + DataXYZ xyz(get("xyz")); + DataNormal xyz_normals(create("normal", sizeof(double)*3*xyz.size())); + if(xyz.size() == 0) + throw runtime_error("Could not calculate reduced points, XYZ data is empty"); + + vector points; + points.reserve(xyz.size()); + vector normals; + normals.reserve(xyz.size()); + for(unsigned int j = 0; j < xyz.size(); j++) { + points.push_back(Point(xyz[j][0], xyz[j][1], xyz[j][2])); + } + const int K_NEIGHBOURS = 10; + calculateNormalsApxKNN(normals, points, K_NEIGHBOURS, get_rPos(), 1.0); + for (unsigned int i = 0; i < normals.size(); ++i) { + xyz_normals[i][0] = normals[i].x; + xyz_normals[i][1] = normals[i].y; + xyz_normals[i][2] = normals[i].z; + } +} + /** * Computes an octtree of the current scan, then getting the * reduced points as the centers of the octree voxels. @@ -236,137 +279,110 @@ void Scan::calcReducedPoints() // get xyz to start the scan load, separated here for time measurement - DataXYZ xyz(get("xyz")); - DataReflectance reflectance(get("reflectance")); - if(xyz.size() == 0) - throw runtime_error("Could not calculate reduced points, XYZ data is empty"); - - if (reflectance.size()==0) { - - // no reduction needed - // copy vector of points to array of points to avoid - // further copying - if(reduction_voxelSize <= 0.0) { - // copy the points - DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size())); - for(unsigned int i = 0; i < xyz.size(); ++i) { - for(unsigned int j = 0; j < 3; ++j) { - xyz_reduced[i][j] = xyz[i][j]; - } - } - } else { - // start reduction - // build octree-tree from CurrentScan - // put full data into the octtree - BOctTree *oct = new BOctTree(PointerArray(xyz).get(), - xyz.size(), reduction_voxelSize, reduction_pointtype); - - vector center; - center.clear(); - if (reduction_nrpts > 0) { - if (reduction_nrpts == 1) { - oct->GetOctTreeRandom(center); - } else { - oct->GetOctTreeRandom(center, reduction_nrpts); - } - } else { - oct->GetOctTreeCenter(center); - } - - // storing it as reduced scan - unsigned int size = center.size(); - DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); - for(unsigned int i = 0; i < size; ++i) { - for(unsigned int j = 0; j < 3; ++j) { - xyz_reduced[i][j] = center[i][j]; - } - } - delete oct; - } - } else { - if(xyz.size() != reflectance.size()) - throw runtime_error("Could not calculate reduced reflectance, reflectance size is different from points size"); - double **xyz_reflectance = new double*[xyz.size()]; - for (unsigned int i = 0; i < xyz.size(); ++i) { - xyz_reflectance[i] = new double[4]; - for (unsigned int j = 0; j < 3; ++j) - xyz_reflectance[i][j] = xyz[i][j]; - xyz_reflectance[i][3] = reflectance[i]; - } - + DataXYZ xyz_normals(get("")); + if (reduction_pointtype.hasNormal()) { + DataXYZ my_xyz_normals(get("normal")); + xyz_normals = my_xyz_normals; + } + DataReflectance reflectance(get("")); + if (reduction_pointtype.hasReflectance()) { + DataReflectance my_reflectance(get("reflectance")); + reflectance = my_reflectance; + } + #ifdef WITH_METRICS - ClientMetric::scan_load_time.end(t); - Timer tl = ClientMetric::calc_reduced_points_time.start(); + ClientMetric::scan_load_time.end(t); + Timer tl = ClientMetric::calc_reduced_points_time.start(); #endif //WITH_METRICS - - // no reduction needed - // copy vector of points to array of points to avoid - // further copying + if(reduction_voxelSize <= 0.0) { // copy the points - if (reduction_pointtype.hasReflectance()) { - DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size())); - DataReflectance reflectance_reduced(create("reflectance reduced", sizeof(double)*reflectance.size())); - for(unsigned int i = 0; i < xyz.size(); ++i) { - for(unsigned int j = 0; j < 3; ++j) - xyz_reduced[i][j] = xyz[i][j]; - reflectance_reduced[i] = reflectance[i]; - } - } else { - DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size())); - for(unsigned int i = 0; i < xyz.size(); ++i) { - for(unsigned int j = 0; j < 3; ++j) { - xyz_reduced[i][j] = xyz[i][j]; + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size())); + for(unsigned int i = 0; i < xyz.size(); ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_reduced[i][j] = xyz[i][j]; + } + } + if (reduction_pointtype.hasReflectance()) { + DataReflectance reflectance_reduced(create("reflectance reduced", sizeof(float)*reflectance.size())); + for(unsigned int i = 0; i < xyz.size(); ++i) { + reflectance_reduced[i] = reflectance[i]; } - } } + if (reduction_pointtype.hasNormal()) { + DataNormal normal_reduced(create("normal reduced", sizeof(double)*3*xyz.size())); + for(unsigned int i = 0; i < xyz.size(); ++i) { + for(unsigned int j = 0; j < 3; ++j) { + normal_reduced[i][j] = xyz_normals[i][j]; + } + } + } + } else { + + double **xyz_in = new double*[xyz.size()]; + for (unsigned int i = 0; i < xyz.size(); ++i) { + xyz_in[i] = new double[reduction_pointtype.getPointDim()]; + unsigned int j = 0; + for (; j < 3; ++j) + xyz_in[i][j] = xyz[i][j]; + if (reduction_pointtype.hasReflectance()) + xyz_in[i][j++] = reflectance[i]; + if (reduction_pointtype.hasNormal()) + for (unsigned int l = 0; l < 3; ++l) + xyz_in[i][j] = xyz_normals[i][l]; + } + // start reduction // build octree-tree from CurrentScan // put full data into the octtree - BOctTree *oct = new BOctTree(xyz_reflectance, xyz.size(), reduction_voxelSize, reduction_pointtype); - vector reduced; - reduced.clear(); + BOctTree *oct = new BOctTree(xyz_in, + xyz.size(), + reduction_voxelSize, + reduction_pointtype); + vector center; + center.clear(); if (reduction_nrpts > 0) { - if (reduction_nrpts == 1) { - oct->GetOctTreeRandom(reduced); - } else { - oct->GetOctTreeRandom(reduced, reduction_nrpts); - } + if (reduction_nrpts == 1) { + oct->GetOctTreeRandom(center); + } else { + oct->GetOctTreeRandom(center, reduction_nrpts); + } } else { - oct->GetOctTreeCenter(reduced); + oct->GetOctTreeCenter(center); } - + // storing it as reduced scan - unsigned int size = reduced.size(); + unsigned int size = center.size(); + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); + DataReflectance reflectance_reduced(get("")); + DataNormal normal_reduced(get("")); if (reduction_pointtype.hasReflectance()) { - DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); - for(unsigned int i = 0; i < size; ++i) { - for(unsigned int j = 0; j < 3; ++j) { - xyz_reduced[i][j] = reduced[i][j]; - } - } - DataReflectance reflectance_reduced(create("reflectance reduced", sizeof(float)*size)); - for(unsigned int i = 0; i < size; ++i) - reflectance_reduced[i] = reduced[i][3]; - } else { - DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); - for(unsigned int i = 0; i < size; ++i) - for(unsigned int j = 0; j < 3; ++j) - xyz_reduced[i][j] = reduced[i][j]; + DataReflectance my_reflectance_reduced(create("reflectance reduced", + sizeof(float)*size)); + reflectance_reduced = my_reflectance_reduced; + } + if (reduction_pointtype.hasNormal()) { + DataNormal my_normal_reduced(create("normal reduced", sizeof(double)*3*size)); + normal_reduced = my_normal_reduced; + } + for(unsigned int i = 0; i < size; ++i) { + unsigned int j = 0; + for (; j < 3; ++j) + xyz_reduced[i][j] = center[i][j]; + if (reduction_pointtype.hasReflectance()) + reflectance_reduced[i] = center[i][j++]; + if (reduction_pointtype.hasNormal()) + for (unsigned int l = 0; l < 3; ++l) + normal_reduced[i][l] = center[i][j++]; } - delete oct; - } - for (unsigned int i = 0; i < xyz.size(); ++i) { - delete[] xyz_reflectance[i]; } - delete[] xyz_reflectance; + #ifdef WITH_METRICS - ClientMetric::calc_reduced_points_time.end(tl); -#endif //WITH_METRICS - } + ClientMetric::calc_reduced_points_time.end(tl); +#endif //WITH_METRICS } @@ -399,7 +415,7 @@ void Scan::transformAll(const double alignxf[16]) { DataXYZ xyz(get("xyz")); unsigned int i=0 ; -// #pragma omp parallel for + // #pragma omp parallel for for(; i < xyz.size(); ++i) { transform3(alignxf, xyz[i]); } @@ -415,11 +431,17 @@ void Scan::transformReduced(const double alignxf[16]) DataXYZ xyz_reduced(get("xyz reduced")); unsigned int i=0; - // #pragma omp parallel for + // #pragma omp parallel for for( ; i < xyz_reduced.size(); ++i) { transform3(alignxf, xyz_reduced[i]); } + DataNormal normal_reduced(get("normal reduced")); + for (unsigned int i = 0; i < normal_reduced.size(); ++i) { + transform3normal(alignxf, normal_reduced[i]); + } + + #ifdef WITH_METRICS ClientMetric::transform_time.end(t); #endif //WITH_METRICS @@ -438,7 +460,7 @@ void Scan::transformMatrix(const double alignxf[16]) #ifdef DEBUG cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", " - << rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ")" << endl; + << rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ")" << endl; cerr << transMat << endl; #endif @@ -480,7 +502,7 @@ void Scan::transform(const double alignxf[16], const AlgoType type, int islum) #ifdef DEBUG cerr << alignxf << endl; cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", " - << rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ") ---> "; + << rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ") ---> "; #endif // transform points @@ -572,7 +594,7 @@ void Scan::transform(const double alignxf[16], const AlgoType type, int islum) * 2 LUM transformation, last scan only */ void Scan::transform(const double alignQuat[4], const double alignt[3], - const AlgoType type, int islum) + const AlgoType type, int islum) { double alignxf[16]; QuatToMatrix4(alignQuat, alignt, alignxf); @@ -655,9 +677,9 @@ void Scan::transformToQuat(double rP[3], double rPQ[4], const AlgoType type, int */ void Scan::getNoPairsSimple(vector &diff, - Scan* Source, Scan* Target, - int thread_num, - double max_dist_match2) + Scan* Source, Scan* Target, + int thread_num, + double max_dist_match2) { DataXYZ xyz_reduced(Source->get("xyz reduced")); KDtree* kd = new KDtree(PointerArray(Target->get("xyz reduced")).get(), Target->size("xyz reduced")); @@ -694,10 +716,10 @@ void Scan::getNoPairsSimple(vector &diff, * @param max_dist_match2 maximal allowed distance for matching */ void Scan::getPtPairsSimple(vector *pairs, - Scan* Source, Scan* Target, - int thread_num, - int rnd, double max_dist_match2, - double *centroid_m, double *centroid_d) + Scan* Source, Scan* Target, + int thread_num, + int rnd, double max_dist_match2, + double *centroid_m, double *centroid_d) { KDtree* kd = new KDtree(PointerArray(Source->get("xyz reduced")).get(), Source->size("xyz reduced")); DataXYZ xyz_reduced(Target->get("xyz reduced")); @@ -751,10 +773,10 @@ void Scan::getPtPairsSimple(vector *pairs, * @return a set of corresponding point pairs */ void Scan::getPtPairs(vector *pairs, - Scan* Source, Scan* Target, - int thread_num, - int rnd, double max_dist_match2, double &sum, - double *centroid_m, double *centroid_d) + Scan* Source, Scan* Target, + int thread_num, + int rnd, double max_dist_match2, double &sum, + double *centroid_m, double *centroid_d, PairingMode pairing_mode) { // initialize centroids for(unsigned int i = 0; i < 3; ++i) { @@ -764,10 +786,12 @@ void Scan::getPtPairs(vector *pairs, // get point pairs DataXYZ xyz_reduced(Target->get("xyz reduced")); + DataNormal normal_reduced(Target->get("normal reduced")); Source->getSearchTree()->getPtPairs(pairs, Source->dalignxf, - xyz_reduced, 0, xyz_reduced.size(), - thread_num, - rnd, max_dist_match2, sum, centroid_m, centroid_d); + xyz_reduced, normal_reduced, 0, xyz_reduced.size(), + thread_num, + rnd, max_dist_match2, sum, centroid_m, centroid_d, + pairing_mode); // normalize centroids unsigned int size = pairs->size(); @@ -801,11 +825,14 @@ void Scan::getPtPairs(vector *pairs, * by Langis / Greenspan / Godin, IEEE 3DIM 2001 * */ -void Scan::getPtPairsParallel(vector *pairs, Scan* Source, Scan* Target, - int thread_num, int step, - int rnd, double max_dist_match2, - double *sum, - double centroid_m[OPENMP_NUM_THREADS][3], double centroid_d[OPENMP_NUM_THREADS][3]) +void Scan::getPtPairsParallel(vector *pairs, + Scan* Source, Scan* Target, + int thread_num, int step, + int rnd, double max_dist_match2, + double *sum, + double centroid_m[OPENMP_NUM_THREADS][3], + double centroid_d[OPENMP_NUM_THREADS][3], + PairingMode pairing_mode) { // initialize centroids for(unsigned int i = 0; i < 3; ++i) { @@ -822,22 +849,26 @@ void Scan::getPtPairsParallel(vector *pairs, Scan* Source, Scan* Target for(unsigned int i = 0; i < meta->size(); ++i) { // determine step for each scan individually DataXYZ xyz_reduced(meta->getScan(i)->get("xyz reduced")); + DataNormal normal_reduced(Target->get("normal reduced")); unsigned int max = xyz_reduced.size(); unsigned int step = max / OPENMP_NUM_THREADS; // call ptpairs for each scan and accumulate ptpairs, centroids and sum search->getPtPairs(&pairs[thread_num], Source->dalignxf, - xyz_reduced, step * thread_num, step * thread_num + step, - thread_num, - rnd, max_dist_match2, sum[thread_num], - centroid_m[thread_num], centroid_d[thread_num]); + xyz_reduced, normal_reduced, + step * thread_num, step * thread_num + step, + thread_num, + rnd, max_dist_match2, sum[thread_num], + centroid_m[thread_num], centroid_d[thread_num], pairing_mode); } } else { DataXYZ xyz_reduced(Target->get("xyz reduced")); + DataNormal normal_reduced(Target->get("normal reduced")); search->getPtPairs(&pairs[thread_num], Source->dalignxf, - xyz_reduced, thread_num * step, thread_num * step + step, - thread_num, - rnd, max_dist_match2, sum[thread_num], - centroid_m[thread_num], centroid_d[thread_num]); + xyz_reduced, normal_reduced, + thread_num * step, thread_num * step + step, + thread_num, + rnd, max_dist_match2, sum[thread_num], + centroid_m[thread_num], centroid_d[thread_num], pairing_mode); } // normalize centroids diff --git a/src/slam6d/searchTree.cc b/src/slam6d/searchTree.cc index 856716c..e8442ab 100644 --- a/src/slam6d/searchTree.cc +++ b/src/slam6d/searchTree.cc @@ -18,16 +18,24 @@ #include "slam6d/scan.h" #include "slam6d/globals.icc" +#include + +double *SearchTree::FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const +{ + throw std::runtime_error("Method FindClosestAlongDir is not implemented"); +} + void SearchTree::getPtPairs(vector *pairs, - double *source_alignxf, // source - double * const *q_points, unsigned int startindex, unsigned int endindex, // target - int thread_num, - int rnd, double max_dist_match2, double &sum, - double *centroid_m, double *centroid_d) + double *source_alignxf, // source + double * const *q_points, + unsigned int startindex, unsigned int endindex, // target + int thread_num, + int rnd, double max_dist_match2, double &sum, + double *centroid_m, double *centroid_d) { // prepare this tree for resource access in FindClosest lock(); - + double local_alignxf_inv[16]; M4inv(source_alignxf, local_alignxf_inv); @@ -63,7 +71,7 @@ void SearchTree::getPtPairs(vector *pairs, sum += Len2(p12); pairs->push_back(myPair); - /*cout << "PTPAIR" << i << " " + /*cout << "PTPAIR" << i << " " << p[0] << " " << p[1] << " " << p[2] << " - " @@ -81,11 +89,13 @@ void SearchTree::getPtPairs(vector *pairs, } void SearchTree::getPtPairs(vector *pairs, - double *source_alignxf, // source - const DataXYZ& xyz_r, unsigned int startindex, unsigned int endindex, // target - int thread_num, - int rnd, double max_dist_match2, double &sum, - double *centroid_m, double *centroid_d) + double *source_alignxf, // source + const DataXYZ& xyz_r, const DataNormal& normal_r, + unsigned int startindex, unsigned int endindex, // target + int thread_num, + int rnd, double max_dist_match2, double &sum, + double *centroid_m, double *centroid_d, + PairingMode pairing_mode) { // prepare this tree for resource access in FindClosest lock(); @@ -95,20 +105,53 @@ void SearchTree::getPtPairs(vector *pairs, // t is the original point from target, s is the (inverted) query point from target and then // the closest point in source - double t[3], s[3]; + double t[3], s[3], normal[3]; for (unsigned int i = startindex; i < endindex; i++) { if (rnd > 1 && rand(rnd) != 0) continue; // take about 1/rnd-th of the numbers only t[0] = xyz_r[i][0]; t[1] = xyz_r[i][1]; t[2] = xyz_r[i][2]; - + transform3(local_alignxf_inv, t, s); - - double *closest = this->FindClosest(s, max_dist_match2, thread_num); + + double *closest; + + if (pairing_mode != CLOSEST_POINT) { + normal[0] = normal_r[i][0]; + normal[1] = normal_r[i][1]; + normal[2] = normal_r[i][2]; + Normalize3(normal); + } + + if (pairing_mode == CLOSEST_POINT_ALONG_NORMAL) { + transform3normal(local_alignxf_inv, normal); + closest = this->FindClosestAlongDir(s, normal, max_dist_match2, thread_num); + + // discard points farther than 20 cm + if (closest && sqrt(Dist2(closest, s)) > 20) closest = NULL; + } else { + closest = this->FindClosest(s, max_dist_match2, thread_num); + } + if (closest) { transform3(source_alignxf, closest, s); - + + if (pairing_mode == CLOSEST_PLANE) { + // need to mutate s if we are looking for closest point-to-plane + // s_ = (n,s-t)*n + t + // to find the projection of s onto plane formed by normal n and point t + double tmp[3], s_[3]; + double dot; + sub3(s, t, tmp); + dot = Dot(normal, tmp); + scal_mul3(normal, dot, tmp); + add3(tmp, t, s_); + s[0] = s_[0]; + s[1] = s_[1]; + s[2] = s_[2]; + } + // This should be right, model=Source=First=not moving centroid_m[0] += s[0]; centroid_m[1] += s[1]; @@ -125,7 +168,7 @@ void SearchTree::getPtPairs(vector *pairs, sum += Len2(p12); pairs->push_back(myPair); - /*cout << "PTPAIR" << i << " " + /*cout << "PTPAIR" << i << " " << p[0] << " " << p[1] << " " << p[2] << " - " diff --git a/src/slam6d/slam6D.cc b/src/slam6d/slam6D.cc index 791237d..0174007 100644 --- a/src/slam6d/slam6D.cc +++ b/src/slam6d/slam6D.cc @@ -99,17 +99,17 @@ void sigSEGVhandler (int v) if(!segfault) { segfault = true; cout << endl - << "# **************************** #" << endl - << " Segmentation fault or Ctrl-C" << endl - << "# **************************** #" << endl - << endl; + << "# **************************** #" << endl + << " Segmentation fault or Ctrl-C" << endl + << "# **************************** #" << endl + << endl; - // save frames and close scans - for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { - (*it)->saveFrames(); - } - cout << "Frames saved." << endl; - Scan::closeDirectory(); + // save frames and close scans + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + (*it)->saveFrames(); + } + cout << "Frames saved." << endl; + Scan::closeDirectory(); } exit(-1); } @@ -128,145 +128,144 @@ void usage(char* prog) const string normal(""); #endif cout << endl - << bold << "USAGE " << normal << endl - << " " << prog << " [options] directory" << endl << endl; + << bold << "USAGE " << normal << endl + << " " << prog << " [options] directory" << endl << endl; cout << bold << "OPTIONS" << normal << endl - << bold << " -a" << normal << " NR, " << bold << "--algo=" << normal << "NR [default: 1]" << endl - << " selects the minimizazion method for the ICP matching algorithm" << endl - << " 1 = unit quaternion based method by Horn" << endl - << " 2 = singular value decomposition by Arun et al. " << endl - << " 3 = orthonormal matrices by Horn et al." << endl - << " 4 = dual quaternion method by Walker et al." << endl - << " 5 = helix approximation by Hofer & Potmann" << endl - << " 6 = small angle approximation" << endl - << " 7 = Lu & Milios style, i.e., uncertainty based, with Euler angles" << endl - << " 8 = Lu & Milios style, i.e., uncertainty based, with Quaternion" << endl - << " 9 = unit quaternion with scale method by Horn" << endl - << endl - << bold << " -A" << normal << " NR, " << bold << "--anim=" << normal << "NR [default: first and last frame only]" << endl - << " if specified, use only every NR-th frame for animation" << endl - << endl - << bold << " -c" << normal << " NR, " << bold << "--cldist=" << normal << "NR [default: 500]" << endl - << " specifies the maximal distance for closed loops" << endl - << endl - << bold << " -C" << normal << " NR, " << bold << "--clpairs=" << normal << "NR [default: 6]" << endl - << " specifies the minimal number of points for an overlap. If not specified" << endl - << " cldist is used instead" << endl - << endl - << bold << " --cache" << normal << endl - << " turns on cached k-d tree search" << endl - << endl - << bold << " -d" << normal << " NR, " << bold << "--dist=" << normal << "NR [default: 25]" << endl - << " sets the maximal point-to-point distance for matching with ICP to 'units'" << endl - << " (unit of scan data, e.g. cm)" << endl - << endl - << bold << " -D" << normal << " NR, " << bold << "--distSLAM=" - << normal << "NR [default: same value as -d option]" << endl - << " sets the maximal point-to-point distance for matching with SLAM to 'units'" << endl - << " (unit of scan data, e.g. cm)" << endl - << endl - << bold << " --DlastSLAM" << normal << " NR [default not set]" << endl - << " sets the maximal point-to-point distance for the final SLAM correction," << endl - << " if final SLAM is not required don't set it." << endl - << endl - << bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl - << " end after scan NR" << endl - << endl - << bold << " --exportAllPoints" << normal << endl - << " writes all registered reduced points to the file points.pts before" << endl - << " slam6D terminated" << endl - << endl - << bold << " --epsICP=" << normal << "NR [default: 0.00001]" << endl - << " stop ICP iteration if difference is smaller than NR" << endl - << endl - << bold << " --epsSLAM=" << normal << " NR [default: 0.5]" << endl - << " stop SLAM iteration if average difference is smaller than NR" << endl - << endl - << bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl - << " using shared library F for input" << endl - << " (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply, wrl, xyz, zuf, iais, front, x3d, rxp, ais })" << endl - << endl - << bold << " -G" << normal << " NR, " << bold << "--graphSlam6DAlgo=" << normal << "NR [default: 0]" << endl - << " selects the minimizazion method for the SLAM matching algorithm" << endl - << " 0 = no global relaxation technique" << endl - << " 1 = Lu & Milios extension using euler angles due to Borrmann et al." << endl - << " 2 = Lu & Milios extension using using unit quaternions" << endl - << " 3 = HELIX approximation by Hofer and Pottmann" << endl - << " 4 = small angle approximation" << endl - << endl - << bold << " -i" << normal << " NR, " << bold << "--iter=" << normal << "NR [default: 50]" << endl - << " sets the maximal number of ICP iterations to " << endl - << endl - << bold << " -I" << normal << " NR, " << bold << "--iterSLAM=" << normal << "NR [default: 0]" << endl - << " sets the maximal number of iterations for SLAM to " << endl - << " (if not set, graphSLAM is not executed)" << endl - << endl - << bold << " -l" << normal << " NR, " << bold << "--loopsize=" << normal << "NR [default: 20]" << endl - << " sets the size of a loop, i.e., a loop must exceed of scans" << endl - << endl - << bold << " -L" << normal << " NR, " << bold << "--loop6DAlgo=" << normal << "NR [default: 0]" << endl - << " selects the method for closing the loop explicitly" << endl - << " 0 = no loop closing technique" << endl - << " 1 = euler angles" << endl - << " 2 = quaternions " << endl - << " 3 = unit quaternions" << endl - << " 4 = SLERP (recommended)" << endl - << endl - << bold << " --metascan" << normal << endl - << " Match current scan against a meta scan of all previous scans (default match against the last scan only)" << endl - << endl - << bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl - << " neglegt all data points with a distance larger than NR 'units'" << endl - << endl - << bold << " -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl - << " neglegt all data points with a distance smaller than NR 'units'" << endl - << endl - << bold << " -n" << normal << " FILE, " << bold << "--net=" << normal << "FILE" << endl - << " specifies the file that includes the net structure for SLAM" << endl - << endl - << bold << " -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl - << " use randomized octree based point reduction (pts per voxel=)" << endl - << " requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl - << endl - << bold << " -p, --trustpose" << normal << endl - << " Trust the pose file, do not extrapolate the last transformation." << endl - << " (just for testing purposes, or gps input.)" << endl - << endl - << bold << " -q, --quiet" << normal << endl - << " Quiet mode. Suppress (most) messages" << endl - << endl - << bold << " -Q, --veryquiet" << normal << endl - << " Very quiet mode. Suppress all messages, except in case of error." << endl - << endl - << bold << " -S, --scanserver" << normal << endl - << " Use the scanserver as an input method and handling of scan data" << endl - << endl - << bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl - << " turns on octree based point reduction (voxel size=)" << endl - << endl - << bold << " -R" << normal << " NR, " << bold << "--random=" << normal << "NR" << endl - << " turns on randomized reduction, using about every -th point only" << endl - << endl - << bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl - << " start at scan NR (i.e., neglects the first NR scans)" << endl - << " [ATTENTION: counting naturally starts with 0]" << endl - << endl - << bold << " -t" << normal << " NR, " << bold << "--nns_method=" << normal << "NR [default: 1]" << endl - << " selects the Nearest Neighbor Search Algorithm" << endl - << " 0 = simple k-d tree " << endl - << " 1 = cached k-d tree " << endl - << " 2 = ANNTree " << endl - << " 3 = BOCTree " << endl - << endl - << bold << " -u" << normal <<", "<< bold<<"--cuda" << normal << endl - << " this option activates icp running on GPU instead of CPU"< 'units'" << endl + << " (unit of scan data, e.g. cm)" << endl + << endl + << bold << " -D" << normal << " NR, " << bold << "--distSLAM=" + << normal << "NR [default: same value as -d option]" << endl + << " sets the maximal point-to-point distance for matching with SLAM to 'units'" << endl + << " (unit of scan data, e.g. cm)" << endl + << endl + << bold << " --DlastSLAM" << normal << " NR [default not set]" << endl + << " sets the maximal point-to-point distance for the final SLAM correction," << endl + << " if final SLAM is not required don't set it." << endl + << endl + << bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl + << " end after scan NR" << endl + << endl + << bold << " --exportAllPoints" << normal << endl + << " writes all registered reduced points to the file points.pts before" << endl + << " slam6D terminated" << endl + << endl + << bold << " --epsICP=" << normal << "NR [default: 0.00001]" << endl + << " stop ICP iteration if difference is smaller than NR" << endl + << endl + << bold << " --epsSLAM=" << normal << " NR [default: 0.5]" << endl + << " stop SLAM iteration if average difference is smaller than NR" << endl + << endl + << bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl + << " using shared library F for input" << endl + << " (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply, wrl, xyz, zuf, iais, front, x3d, rxp, ais })" << endl + << endl + << bold << " -G" << normal << " NR, " << bold << "--graphSlam6DAlgo=" << normal << "NR [default: 0]" << endl + << " selects the minimizazion method for the SLAM matching algorithm" << endl + << " 0 = no global relaxation technique" << endl + << " 1 = Lu & Milios extension using euler angles due to Borrmann et al." << endl + << " 2 = Lu & Milios extension using using unit quaternions" << endl + << " 3 = HELIX approximation by Hofer and Pottmann" << endl + << " 4 = small angle approximation" << endl + << bold << " -i" << normal << " NR, " << bold << "--iter=" << normal << "NR [default: 50]" << endl + << " sets the maximal number of ICP iterations to " << endl + << endl + << bold << " -I" << normal << " NR, " << bold << "--iterSLAM=" << normal << "NR [default: 0]" << endl + << " sets the maximal number of iterations for SLAM to " << endl + << " (if not set, graphSLAM is not executed)" << endl + << endl + << bold << " -l" << normal << " NR, " << bold << "--loopsize=" << normal << "NR [default: 20]" << endl + << " sets the size of a loop, i.e., a loop must exceed of scans" << endl + << endl + << bold << " -L" << normal << " NR, " << bold << "--loop6DAlgo=" << normal << "NR [default: 0]" << endl + << " selects the method for closing the loop explicitly" << endl + << " 0 = no loop closing technique" << endl + << " 1 = euler angles" << endl + << " 2 = quaternions " << endl + << " 3 = unit quaternions" << endl + << " 4 = SLERP (recommended)" << endl + << endl + << bold << " --metascan" << normal << endl + << " Match current scan against a meta scan of all previous scans (default match against the last scan only)" << endl + << endl + << bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl + << " neglegt all data points with a distance larger than NR 'units'" << endl + << endl + << bold << " -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl + << " neglegt all data points with a distance smaller than NR 'units'" << endl + << endl + << bold << " -n" << normal << " FILE, " << bold << "--net=" << normal << "FILE" << endl + << " specifies the file that includes the net structure for SLAM" << endl + << endl + << bold << " -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl + << " use randomized octree based point reduction (pts per voxel=)" << endl + << " requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl + << endl + << bold << " -p, --trustpose" << normal << endl + << " Trust the pose file, do not extrapolate the last transformation." << endl + << " (just for testing purposes, or gps input.)" << endl + << endl + << bold << " -q, --quiet" << normal << endl + << " Quiet mode. Suppress (most) messages" << endl + << endl + << bold << " -Q, --veryquiet" << normal << endl + << " Very quiet mode. Suppress all messages, except in case of error." << endl + << endl + << bold << " -S, --scanserver" << normal << endl + << " Use the scanserver as an input method and handling of scan data" << endl + << endl + << bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl + << " turns on octree based point reduction (voxel size=)" << endl + << endl + << bold << " -R" << normal << " NR, " << bold << "--random=" << normal << "NR" << endl + << " turns on randomized reduction, using about every -th point only" << endl + << endl + << bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl + << " start at scan NR (i.e., neglects the first NR scans)" << endl + << " [ATTENTION: counting naturally starts with 0]" << endl + << endl + << bold << " -t" << normal << " NR, " << bold << "--nns_method=" << normal << "NR [default: 1]" << endl + << " selects the Nearest Neighbor Search Algorithm" << endl + << " 0 = simple k-d tree " << endl + << " 1 = cached k-d tree " << endl + << " 2 = ANNTree " << endl + << " 3 = BOCTree " << endl + << endl + << bold << " -u" << normal <<", "<< bold<<"--cuda" << normal << endl + << " this option activates icp running on GPU instead of CPU"< 9)) { - cerr << "Error: ICP Algorithm not available." << endl; - exit(1); - } - break; - case 't': - nns_method = atoi(optarg); - if ((nns_method < 0) || (nns_method > 3)) { - cerr << "Error: NNS Method not available." << endl; - exit(1); - } - break; - case 'L': - loopSlam6DAlgo = atoi(optarg); - if (loopSlam6DAlgo < 0 || loopSlam6DAlgo > 6) { - cerr << "Error: global loop closing algorithm not available." << endl; - exit(1); - } - break; - case 'G': - lum6DAlgo = atoi(optarg); - if ((lum6DAlgo < 0) || (lum6DAlgo > 6)) { - cerr << "Error: global relaxation algorithm not available." << endl; - exit(1); - } - break; - case 'c': - cldist = atof(optarg); - break; - case 'C': - clpairs = atoi(optarg); - break; - case 'l': - loopsize = atoi(optarg); - break; - case 'r': - red = atof(optarg); - break; - case 'O': - if (optarg) { - octree = atoi(optarg); - } else { - octree = 1; - } - break; - case 'R': - rand = atoi(optarg); - break; - case 'd': - mdm = atof(optarg); - break; - case 'D': - mdml = atof(optarg); - break; - case 'i': - mni = atoi(optarg); - break; - case 'I': - mni_lum = atoi(optarg); - break; - case 'n': - net = optarg; - break; - case 's': - w_start = atoi(optarg); - if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } - break; - case 'e': - w_end = atoi(optarg); - if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } - if (end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } - break; - case 'm': - maxDist = atoi(optarg); - break; - case 'M': - minDist = atoi(optarg); - break; - case 'q': - quiet = true; - break; - case 'Q': - quiet = veryQuiet = true; - break; - case 'p': - extrapolate_pose = false; - break; - case 'A': - anim = atoi(optarg); - break; - case '2': // = --metascan - meta = true; - break; - case '4': // = --DlastSLAM - mdmll = atof(optarg); - break; - case '5': // = --epsICP - epsilonICP = atof(optarg); - break; - case '6': // = --epsSLAM - epsilonSLAM = atof(optarg); - break; - case '8': // not used - exportPts = true; - break; - case '9': // = --distLoop - distLoop = atof(optarg); - break; - case '1': // = --iterLoop - iterLoop = atoi(optarg); - break; - case '3': // = --graphDist - graphDist = atof(optarg); - break; - case 'f': - try { - w_type = formatname_to_io_type(optarg); - } catch (...) { // runtime_error - cerr << "Format " << optarg << " unknown." << endl; - abort(); - } - break; - case 'u': - cuda_enabled = true; - break; - case 'S': - scanserver = true; - break; - case '?': - usage(argv[0]); - return 1; - default: - abort (); + case 'a': + algo = atoi(optarg); + if ((algo < 0) || (algo > 9)) { + cerr << "Error: ICP Algorithm not available." << endl; + exit(1); + } + break; + case 't': + nns_method = atoi(optarg); + if ((nns_method < 0) || (nns_method > 3)) { + cerr << "Error: NNS Method not available." << endl; + exit(1); + } + break; + case 'L': + loopSlam6DAlgo = atoi(optarg); + if (loopSlam6DAlgo < 0 || loopSlam6DAlgo > 6) { + cerr << "Error: global loop closing algorithm not available." << endl; + exit(1); + } + break; + case 'G': + lum6DAlgo = atoi(optarg); + if ((lum6DAlgo < 0) || (lum6DAlgo > 6)) { + cerr << "Error: global relaxation algorithm not available." << endl; + exit(1); + } + break; + case 'c': + cldist = atof(optarg); + break; + case 'C': + clpairs = atoi(optarg); + break; + case 'l': + loopsize = atoi(optarg); + break; + case 'r': + red = atof(optarg); + break; + case 'O': + if (optarg) { + octree = atoi(optarg); + } else { + octree = 1; + } + break; + case 'R': + rand = atoi(optarg); + break; + case 'd': + mdm = atof(optarg); + break; + case 'D': + mdml = atof(optarg); + break; + case 'i': + mni = atoi(optarg); + break; + case 'I': + mni_lum = atoi(optarg); + break; + case 'n': + net = optarg; + break; + case 's': + w_start = atoi(optarg); + if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } + break; + case 'e': + w_end = atoi(optarg); + if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } + if (end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } + break; + case 'm': + maxDist = atoi(optarg); + break; + case 'M': + minDist = atoi(optarg); + break; + case 'q': + quiet = true; + break; + case 'Q': + quiet = veryQuiet = true; + break; + case 'p': + extrapolate_pose = false; + break; + case 'A': + anim = atoi(optarg); + break; + case '2': // = --metascan + meta = true; + break; + case '4': // = --DlastSLAM + mdmll = atof(optarg); + break; + case '5': // = --epsICP + epsilonICP = atof(optarg); + break; + case '6': // = --epsSLAM + epsilonSLAM = atof(optarg); + break; + case '8': // not used + exportPts = true; + break; + case '9': // = --distLoop + distLoop = atof(optarg); + break; + case '1': // = --iterLoop + iterLoop = atoi(optarg); + break; + case '3': // = --graphDist + graphDist = atof(optarg); + break; + case '7': // = --normalshoot + pairing_mode = CLOSEST_POINT_ALONG_NORMAL; + break; + case 'z': // = --point-to-plane + pairing_mode = CLOSEST_PLANE; + break; + case 'f': + try { + w_type = formatname_to_io_type(optarg); + } catch (...) { // runtime_error + cerr << "Format " << optarg << " unknown." << endl; + abort(); + } + break; + case 'u': + cuda_enabled = true; + break; + case 'S': + scanserver = true; + break; + case '?': + usage(argv[0]); + return 1; + default: + abort (); } } @@ -533,10 +540,10 @@ int parseArgs(int argc, char **argv, string &dir, double &red, int &rand, * @param mdmll maximal distance match for global SLAM after all scans ar matched */ void matchGraph6Dautomatic(double cldist, int loopsize, vector allScans, icp6D *my_icp6D, - bool meta_icp, int nns_method, bool cuda_enabled, - loopSlam6D *my_loopSlam6D, graphSlam6D *my_graphSlam6D, int nrIt, - double epsilonSLAM, double mdml, double mdmll, double graphDist, - bool &eP, IOType type) + bool meta_icp, int nns_method, bool cuda_enabled, + loopSlam6D *my_loopSlam6D, graphSlam6D *my_graphSlam6D, int nrIt, + double epsilonSLAM, double mdml, double mdmll, double graphDist, + bool &eP, IOType type) { double cldist2 = sqr(cldist); @@ -580,18 +587,18 @@ void matchGraph6Dautomatic(double cldist, int loopsize, vector allScans delete meta_scan; } else { switch(type) { - case UOS_MAP: - case UOS_MAP_FRAMES: - my_icp6D->match(allScans[0], allScans[i]); - break; - case RTS_MAP: - //untested (and could not work) - //if(i < 220-22 && i > 250-22) match(allScans[0], CurrentScan); - my_icp6D->match(allScans[0], allScans[i]); - break; - default: - my_icp6D->match(allScans[i - 1], allScans[i]); - break; + case UOS_MAP: + case UOS_MAP_FRAMES: + my_icp6D->match(allScans[0], allScans[i]); + break; + case RTS_MAP: + //untested (and could not work) + //if(i < 220-22 && i > 250-22) match(allScans[0], CurrentScan); + my_icp6D->match(allScans[0], allScans[i]); + break; + default: + my_icp6D->match(allScans[i - 1], allScans[i]); + break; } } } else { @@ -693,7 +700,7 @@ int main(int argc, char **argv) cout << "slam6D - A highly efficient SLAM implementation based on scan matching" << endl << " with 6 degrees of freedom" << endl << "(c) Jacobs University Bremen gGmbH, Germany, since 2009" << endl - << " University of Osnabrueck, Germany, 2006 - 2009" << endl << endl; + << " University of Osnabrueck, Germany, since 2006" << endl << endl; if (argc <= 1) { usage(argv[0]); @@ -731,12 +738,13 @@ int main(int argc, char **argv) bool cuda_enabled = false; IOType type = UOS; bool scanserver = false; + PairingMode pairing_mode = CLOSEST_POINT; parseArgs(argc, argv, dir, red, rand, mdm, mdml, mdmll, mni, start, end, - maxDist, minDist, quiet, veryQuiet, eP, meta, algo, loopSlam6DAlgo, lum6DAlgo, anim, - mni_lum, net, cldist, clpairs, loopsize, epsilonICP, epsilonSLAM, - nns_method, exportPts, distLoop, iterLoop, graphDist, octree, cuda_enabled, type, - scanserver); + maxDist, minDist, quiet, veryQuiet, eP, meta, algo, loopSlam6DAlgo, lum6DAlgo, anim, + mni_lum, net, cldist, clpairs, loopsize, epsilonICP, epsilonSLAM, + nns_method, exportPts, distLoop, iterLoop, graphDist, octree, cuda_enabled, type, + scanserver, pairing_mode); cout << "slam6D will proceed with the following parameters:" << endl; //@@@ to do :-) @@ -752,39 +760,44 @@ int main(int argc, char **argv) for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { Scan* scan = *it; scan->setRangeFilter(maxDist, minDist); - scan->setReductionParameter(red, octree); + unsigned int types = 0; + if ((pairing_mode == CLOSEST_POINT_ALONG_NORMAL) || + (pairing_mode == CLOSEST_PLANE)) { + types = PointType::USE_NORMAL; + } + scan->setReductionParameter(red, octree, PointType(types)); scan->setSearchTreeParameter(nns_method, cuda_enabled); } icp6Dminimizer *my_icp6Dminimizer = 0; switch (algo) { - case 1 : - my_icp6Dminimizer = new icp6D_QUAT(quiet); - break; - case 2 : - my_icp6Dminimizer = new icp6D_SVD(quiet); - break; - case 3 : - my_icp6Dminimizer = new icp6D_ORTHO(quiet); - break; - case 4 : - my_icp6Dminimizer = new icp6D_DUAL(quiet); - break; - case 5 : - my_icp6Dminimizer = new icp6D_HELIX(quiet); - break; - case 6 : - my_icp6Dminimizer = new icp6D_APX(quiet); - break; - case 7 : - my_icp6Dminimizer = new icp6D_LUMEULER(quiet); - break; - case 8 : - my_icp6Dminimizer = new icp6D_LUMQUAT(quiet); - break; - case 9 : - my_icp6Dminimizer = new icp6D_QUAT_SCALE(quiet); - break; + case 1 : + my_icp6Dminimizer = new icp6D_QUAT(quiet); + break; + case 2 : + my_icp6Dminimizer = new icp6D_SVD(quiet); + break; + case 3 : + my_icp6Dminimizer = new icp6D_ORTHO(quiet); + break; + case 4 : + my_icp6Dminimizer = new icp6D_DUAL(quiet); + break; + case 5 : + my_icp6Dminimizer = new icp6D_HELIX(quiet); + break; + case 6 : + my_icp6Dminimizer = new icp6D_APX(quiet); + break; + case 7 : + my_icp6Dminimizer = new icp6D_LUMEULER(quiet); + break; + case 8 : + my_icp6Dminimizer = new icp6D_LUMQUAT(quiet); + break; + case 9 : + my_icp6Dminimizer = new icp6D_QUAT_SCALE(quiet); + break; } // match the scans and print the time used @@ -799,13 +812,13 @@ int main(int argc, char **argv) if (cuda_enabled) { #ifdef WITH_CUDA my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, - anim, epsilonICP, nns_method, cuda_enabled); + anim, epsilonICP, nns_method, cuda_enabled); #else cout << "slam6d was not compiled for excuting CUDA code" << endl; #endif } else { my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, - anim, epsilonICP, nns_method, cuda_enabled); + anim, epsilonICP, nns_method, cuda_enabled); } // check if CAD matching was selected as type @@ -814,7 +827,7 @@ int main(int argc, char **argv) my_icp->set_cad_matching (true); } - if (my_icp) my_icp->doICP(Scan::allScans); + if (my_icp) my_icp->doICP(Scan::allScans, pairing_mode); delete my_icp; } else if (clpairs > -1) { //!!!!!!!!!!!!!!!!!!!!!!!! @@ -822,38 +835,38 @@ int main(int argc, char **argv) if (cuda_enabled) { #ifdef WITH_CUDA my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, - anim, epsilonICP, nns_method, cuda_enabled); + anim, epsilonICP, nns_method, cuda_enabled); #else cout << "slam6d was not compiled for excuting CUDA code" << endl; #endif } else { my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, - anim, epsilonICP, nns_method, cuda_enabled); + anim, epsilonICP, nns_method, cuda_enabled); } - my_icp->doICP(Scan::allScans); + my_icp->doICP(Scan::allScans, pairing_mode); graphSlam6D *my_graphSlam6D = new lum6DEuler(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, - rand, eP, anim, epsilonICP, nns_method, epsilonSLAM); + rand, eP, anim, epsilonICP, nns_method, epsilonSLAM); my_graphSlam6D->matchGraph6Dautomatic(Scan::allScans, mni_lum, clpairs, loopsize); //!!!!!!!!!!!!!!!!!!!!!!!! } else { graphSlam6D *my_graphSlam6D = 0; switch (lum6DAlgo) { - case 1 : - my_graphSlam6D = new lum6DEuler(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, - anim, epsilonICP, nns_method, epsilonSLAM); - break; - case 2 : - my_graphSlam6D = new lum6DQuat(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, - anim, epsilonICP, nns_method, epsilonSLAM); - break; - case 3 : - my_graphSlam6D = new ghelix6DQ2(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, - anim, epsilonICP, nns_method, epsilonSLAM); - break; - case 4 : - my_graphSlam6D = new gapx6D(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, - anim, epsilonICP, nns_method, epsilonSLAM); - break; + case 1 : + my_graphSlam6D = new lum6DEuler(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, epsilonSLAM); + break; + case 2 : + my_graphSlam6D = new lum6DQuat(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, epsilonSLAM); + break; + case 3 : + my_graphSlam6D = new ghelix6DQ2(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, epsilonSLAM); + break; + case 4 : + my_graphSlam6D = new gapx6D(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, epsilonSLAM); + break; } // Construct Network if (net != "none") { @@ -861,15 +874,15 @@ int main(int argc, char **argv) if (cuda_enabled) { #ifdef WITH_CUDA my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, - anim, epsilonICP, nns_method); + anim, epsilonICP, nns_method); #else cout << "slam6d was not compiled for excuting CUDA code" << endl; #endif } else { my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, - anim, epsilonICP, nns_method); + anim, epsilonICP, nns_method); } - my_icp->doICP(Scan::allScans); + my_icp->doICP(Scan::allScans, pairing_mode); Graph* structure; structure = new Graph(net); @@ -885,38 +898,38 @@ int main(int argc, char **argv) if (cuda_enabled) { #ifdef WITH_CUDA my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, - anim, epsilonICP, nns_method); + anim, epsilonICP, nns_method); #else cout << "slam6d was not compiled for excuting CUDA code" << endl; #endif } else { my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, - anim, epsilonICP, nns_method); + anim, epsilonICP, nns_method); } loopSlam6D *my_loopSlam6D = 0; switch(loopSlam6DAlgo) { - case 1: - my_loopSlam6D = new elch6Deuler(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, - rand, eP, 10, epsilonICP, nns_method); - break; - case 2: - my_loopSlam6D = new elch6Dquat(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, - rand, eP, 10, epsilonICP, nns_method); - break; - case 3: - my_loopSlam6D = new elch6DunitQuat(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, - rand, eP, 10, epsilonICP, nns_method); - break; - case 4: - my_loopSlam6D = new elch6Dslerp(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, - rand, eP, 10, epsilonICP, nns_method); - break; + case 1: + my_loopSlam6D = new elch6Deuler(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, + rand, eP, 10, epsilonICP, nns_method); + break; + case 2: + my_loopSlam6D = new elch6Dquat(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, + rand, eP, 10, epsilonICP, nns_method); + break; + case 3: + my_loopSlam6D = new elch6DunitQuat(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, + rand, eP, 10, epsilonICP, nns_method); + break; + case 4: + my_loopSlam6D = new elch6Dslerp(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, + rand, eP, 10, epsilonICP, nns_method); + break; } matchGraph6Dautomatic(cldist, loopsize, Scan::allScans, my_icp, meta, - nns_method, cuda_enabled, my_loopSlam6D, my_graphSlam6D, - mni_lum, epsilonSLAM, mdml, mdmll, graphDist, eP, type); + nns_method, cuda_enabled, my_loopSlam6D, my_graphSlam6D, + mni_lum, epsilonSLAM, mdml, mdmll, graphDist, eP, type); delete my_icp; if(loopSlam6DAlgo > 0) { delete my_loopSlam6D; @@ -940,11 +953,16 @@ int main(int argc, char **argv) ofstream redptsout("points.pts"); for(unsigned int i = 0; i < Scan::allScans.size(); i++) { DataXYZ xyz_r(Scan::allScans[i]->get("xyz reduced")); + DataNormal normal_r(Scan::allScans[i]->get("normal reduced")); for(unsigned int i = 0; i < xyz_r.size(); ++i) { - redptsout << xyz_r[i][0] << ' ' << xyz_r[i][1] << ' ' << xyz_r[i][2] << '\n'; + int r,g,b; + r = (int)(normal_r[i][0] * (127.5) + 127.5); + g = (int)(normal_r[i][1] * (127.5) + 127.5); + b = (int)(fabs(normal_r[i][2]) * (255.0)); + redptsout << xyz_r[i][0] << ' ' << xyz_r[i][1] << ' ' << xyz_r[i][2] + << ' ' << r << ' ' << g << ' ' << b << '\n'; } redptsout << std::flush; - } redptsout.close(); redptsout.clear(); @@ -955,8 +973,8 @@ int main(int argc, char **argv) for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { Scan* scan = *it; - p = scan->get_rPos(); - Point x(p[0], p[1], p[2]); + p = scan->get_rPos(); + Point x(p[0], p[1], p[2]); redptsout << x << endl; scan->saveFrames(); } @@ -967,9 +985,9 @@ int main(int argc, char **argv) cout << endl << endl; cout << "Normal program end." << endl - << (red < 0 && rand < 0 ? "(-> HINT: For a significant speedup, please use the '-r' or '-R' parameter <-)\n" - : "") - << endl; + << (red < 0 && rand < 0 ? "(-> HINT: For a significant speedup, please use the '-r' or '-R' parameter <-)\n" + : "") + << endl; // print metric information #ifdef WITH_METRICS diff --git a/src/thermo/CMakeLists.txt b/src/thermo/CMakeLists.txt index 51155b7..73080b2 100644 --- a/src/thermo/CMakeLists.txt +++ b/src/thermo/CMakeLists.txt @@ -1,5 +1,4 @@ IF (WITH_THERMO) - find_package(OpenCV REQUIRED) include_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob) include_directories(${CMAKE_SOURCE_DIR}/include/shapes/)