demote master to svn r761
This commit is contained in:
Razvan Mihalyi
parent
1268ff8dc2
commit
12b8fe558d
40 changed files with 4140 additions and 1667 deletions
|
|
@ -0,0 +1,116 @@
|
|||
### 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)
|
||||
|
||||
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 icp6Dapx.cc icp6D.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})
|
||||
|
||||
target_link_libraries(scan scanclient scanio)
|
||||
|
||||
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)
|
||||
|
|
@ -0,0 +1,978 @@
|
|||
/*
|
||||
* 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 <getopt.h>
|
||||
#else
|
||||
#include "XGetopt.h"
|
||||
#endif
|
||||
|
||||
#include <csignal>
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#define strcasecmp _stricmp
|
||||
#define strncasecmp _strnicmp
|
||||
#else
|
||||
#include <strings.h>
|
||||
#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 <omp.h>
|
||||
#endif
|
||||
|
||||
#define WANT_STREAM ///< define the WANT stream :)
|
||||
|
||||
#include <string>
|
||||
using std::string;
|
||||
#include <iostream>
|
||||
using std::cout;
|
||||
using std::cerr;
|
||||
using std::endl;
|
||||
#include <fstream>
|
||||
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 <NR> '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 <NR> '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 <NR>" << endl
|
||||
<< endl
|
||||
<< bold << " -I" << normal << " NR, " << bold << "--iterSLAM=" << normal << "NR [default: 0]" << endl
|
||||
<< " sets the maximal number of iterations for SLAM to <NR>" << 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 <NR> 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=<NR>)" << 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=<NR>)" << endl
|
||||
<< endl
|
||||
<< bold << " -R" << normal << " NR, " << bold << "--random=" << normal << "NR" << endl
|
||||
<< " turns on randomized reduction, using about every <NR>-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"<<endl
|
||||
<< endl << endl;
|
||||
|
||||
cout << bold << "EXAMPLES " << normal << endl
|
||||
<< " " << prog << " dat" << endl
|
||||
<< " " << prog << " --max=500 -r 10.2 -i 20 dat" << endl
|
||||
<< " " << prog << " -s 2 -e 10 dat" << 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 the directory
|
||||
* @param red using point reduction?
|
||||
* @param rand use randomized point reduction?
|
||||
* @param mdm maximal distance match
|
||||
* @param mdml maximal distance match for SLAM
|
||||
* @param mni maximal number of iterations
|
||||
* @param start starting at scan number 'start'
|
||||
* @param end stopping at scan number 'end'
|
||||
* @param maxDist - maximal distance of points being loaded
|
||||
* @param minDist - minimal distance of points being loaded
|
||||
* @param quiet switches on/off the quiet mode
|
||||
* @param veryQuiet switches on/off the 'very quiet' mode
|
||||
* @param extrapolate_pose - i.e., extrapolating the odometry by the last transformation
|
||||
* (vs. taking the pose file as <b>exact</b>)
|
||||
* @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)
|
||||
{
|
||||
int c;
|
||||
// from unistd.h:
|
||||
extern char *optarg;
|
||||
extern int optind;
|
||||
|
||||
WriteOnce<IOType> w_type(type);
|
||||
WriteOnce<int> 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
|
||||
{ "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: <end> cannot be smaller than <start>.\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 ();
|
||||
}
|
||||
}
|
||||
|
||||
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 <Scan *> 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, 2006 - 2009" << 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;
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
scan->setReductionParameter(red, octree);
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
|
||||
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"));
|
||||
for(unsigned int i = 0; i < xyz_r.size(); ++i) {
|
||||
redptsout << xyz_r[i][0] << ' ' << xyz_r[i][1] << ' ' << xyz_r[i][2] << '\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
|
||||
}
|
||||
|
|
@ -0,0 +1,439 @@
|
|||
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 REQUIRED)
|
||||
ELSE(WIN32)
|
||||
find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time REQUIRED)
|
||||
ENDIF(WIN32)
|
||||
|
||||
if(Boost_FOUND)
|
||||
link_directories(${BOOST_LIBRARY_DIRS})
|
||||
include_directories(${Boost_INCLUDE_DIRS})
|
||||
add_definitions(${Boost_LIB_DIAGNOSTIC_DEFINITIONS})
|
||||
endif()
|
||||
|
||||
#################################################
|
||||
# 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)
|
||||
#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)
|
||||
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)
|
||||
|
||||
## 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 "")
|
||||
|
||||
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)
|
||||
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")
|
||||
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!")
|
||||
|
|
@ -0,0 +1,188 @@
|
|||
/*
|
||||
* show_animate implementation
|
||||
*
|
||||
* Copyright (C) Jan Elseberg, Dorit Borrmann
|
||||
*
|
||||
* Released under the GPL version 3.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <windows.h>
|
||||
#endif
|
||||
#ifdef __APPLE__
|
||||
#include <GLUT/glut.h>
|
||||
#else
|
||||
#include <GL/glut.h>
|
||||
#endif
|
||||
|
||||
#include "slam6d/point.h"
|
||||
#include "slam6d/scan.h"
|
||||
#include "show/colordisplay.h"
|
||||
#include "show/colormanager.h"
|
||||
#include "show/scancolormanager.h"
|
||||
#include <vector>
|
||||
#include <float.h>
|
||||
#include "slam6d/point_type.h"
|
||||
using std::vector;
|
||||
|
||||
ScanColorManager::ScanColorManager(unsigned int _buckets, PointType type, bool animation_color) : pointtype(type), animationColor(animation_color) {
|
||||
valid = false;
|
||||
inverted = false;
|
||||
buckets = _buckets;
|
||||
// types = _types;
|
||||
|
||||
pointtype = type;
|
||||
//pointdim = PointType::PointDim(types);
|
||||
//
|
||||
|
||||
|
||||
mins = new float[pointtype.getPointDim()];
|
||||
maxs = new float[pointtype.getPointDim()];
|
||||
for (unsigned int i = 0; i < pointtype.getPointDim(); i++) {
|
||||
mins[i] = FLT_MAX;
|
||||
maxs[i] = FLT_MIN;
|
||||
}
|
||||
|
||||
currenttype = PointType::USE_HEIGHT;
|
||||
currentdim = 0;
|
||||
}
|
||||
|
||||
void ScanColorManager::registerTree(colordisplay *b) { allScans.push_back(b); }
|
||||
|
||||
void ScanColorManager::setColorMap(ColorMap &cm) {
|
||||
makeValid();
|
||||
for (unsigned int i = 0; i < allManager.size(); i++) {
|
||||
allManager[i]->setColorMap(cm);
|
||||
}
|
||||
}
|
||||
|
||||
void ScanColorManager::setColorMap(ColorMap::CM &cm) {
|
||||
ColorMap cmap = ColorMap::getColorMap(cm);
|
||||
setColorMap(cmap);
|
||||
}
|
||||
|
||||
void ScanColorManager::setCurrentType(unsigned int type) {
|
||||
makeValid();
|
||||
if (type == PointType::USE_NONE) {
|
||||
for (unsigned int i = 0; i < allScans.size(); i++) {
|
||||
allScans[i]->setColorManager(0);
|
||||
}
|
||||
}
|
||||
currentdim = pointtype.getType(type);
|
||||
|
||||
if(type != PointType::USE_NONE) {
|
||||
for (unsigned int i = 0; i < allManager.size(); i++) {
|
||||
allManager[i]->setCurrentDim(currentdim);
|
||||
}
|
||||
currenttype = type;
|
||||
}
|
||||
}
|
||||
|
||||
void ScanColorManager::setMinMax(float min, float max) {
|
||||
makeValid();
|
||||
for (unsigned int i = 0; i < allManager.size(); i++) {
|
||||
allManager[i]->setMinMax(min, max);
|
||||
}
|
||||
}
|
||||
void ScanColorManager::setMode(const unsigned int &mode) {
|
||||
if (mode == ScanColorManager::MODE_STATIC) {
|
||||
for (unsigned int i = 0; i < allScans.size(); i++) {
|
||||
allScans[i]->setColorManager(staticManager[i]);
|
||||
}
|
||||
} else if (mode == ScanColorManager::MODE_COLOR_SCAN) {
|
||||
for (unsigned int i = 0; i < allScans.size(); i++) {
|
||||
allScans[i]->setColorManager(scanManager[i]);
|
||||
}
|
||||
} else if (mode == ScanColorManager::MODE_ANIMATION) {
|
||||
if (animationColor) {
|
||||
for (unsigned int i = 0; i < allScans.size(); i++) {
|
||||
allScans[i]->setColorManager(0);
|
||||
}
|
||||
}
|
||||
} else if (mode == ScanColorManager::MODE_POINT_COLOR) {
|
||||
for (unsigned int i = 0; i < allScans.size(); i++) {
|
||||
allScans[i]->setColorManager(colorsManager[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float ScanColorManager::getMin() { return mins[currentdim];};
|
||||
float ScanColorManager::getMax() { return maxs[currentdim];};
|
||||
float ScanColorManager::getMin(unsigned int dim) { return mins[dim];};
|
||||
float ScanColorManager::getMax(unsigned int dim) { return maxs[dim];};
|
||||
unsigned int ScanColorManager::getPointDim() { return pointtype.getPointDim(); };
|
||||
void ScanColorManager::makeValid() {
|
||||
if (!valid) {
|
||||
for (unsigned int i = 0; i < allScans.size(); i++) {
|
||||
colordisplay *scan = allScans[i];
|
||||
ColorManager *cm = new ColorManager(buckets, pointtype.getPointDim(), mins, maxs);
|
||||
cm->setCurrentDim(currentdim);
|
||||
scan->setColorManager(cm);
|
||||
staticManager.push_back(cm);
|
||||
|
||||
// new colormanager for scan index influenced colorscheme
|
||||
DiffMap m;
|
||||
// JetMap m;
|
||||
float c[3] = {0,0,0};
|
||||
m.calcColor(c, i, allScans.size() );
|
||||
ColorManager *cmc = new ColorManager(buckets, pointtype.getPointDim(), mins, maxs, c);
|
||||
scanManager.push_back(cmc);
|
||||
|
||||
// new colormanager for the color based on the color of the points
|
||||
CColorManager *ccm = new CColorManager(buckets, pointtype.getPointDim(), mins, maxs, pointtype.getType(PointType::USE_COLOR));
|
||||
colorsManager.push_back(ccm);
|
||||
|
||||
allManager.push_back(cm);
|
||||
allManager.push_back(cmc);
|
||||
allManager.push_back(ccm);
|
||||
|
||||
}
|
||||
valid = true;
|
||||
}
|
||||
}
|
||||
|
||||
void ScanColorManager::selectColors(Scan::AlgoType type) {
|
||||
switch(type) {
|
||||
case Scan::ICP:
|
||||
glColor4d(0.85, 0.30,0.023, 1.0);
|
||||
//glColor4d(1.0, 0.30,0.30, 1.0);
|
||||
//glColor4d(1.0, 0.00,0.00, 1.0);
|
||||
break;
|
||||
case Scan::ICPINACTIVE:
|
||||
glColor4d(0.78, 0.63,0.57, 1.0);
|
||||
//glColor4d(1.00, 1.00,1.00, 1.0);
|
||||
break;
|
||||
case Scan::LUM:
|
||||
glColor4d(1.0, 0.0,0.0, 1.0);
|
||||
break;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
const unsigned int ScanColorManager::MODE_STATIC = 0;
|
||||
const unsigned int ScanColorManager::MODE_COLOR_SCAN = 1;
|
||||
const unsigned int ScanColorManager::MODE_ANIMATION = 2;
|
||||
const unsigned int ScanColorManager::MODE_POINT_COLOR = 3;
|
||||
|
||||
|
||||
1069
.svn/pristine/de/de45ca3c310557ddb874b82215965e48e0490071.svn-base
Normal file
1069
.svn/pristine/de/de45ca3c310557ddb874b82215965e48e0490071.svn-base
Normal file
File diff suppressed because it is too large
Load diff
BIN
.svn/wc.db
BIN
.svn/wc.db
Binary file not shown.
|
|
@ -239,7 +239,7 @@ ENDIF(WITH_SEGMENTATION)
|
|||
|
||||
## Normals
|
||||
|
||||
OPT_DEP(WITH_NORMALS "Whether to build separate program for normal computation ON/OFF" OFF "WITH_FBR")
|
||||
OPT_DEP(WITH_NORMALS "Whether to build program for normal computation ON/OFF" OFF "WITH_FBR")
|
||||
|
||||
IF(WITH_NORMALS)
|
||||
MESSAGE(STATUS "With normals")
|
||||
|
|
|
|||
|
|
@ -1,134 +0,0 @@
|
|||
#ifndef NORMALS_H
|
||||
#define NORMALS_H
|
||||
|
||||
#include <vector>
|
||||
#include <slam6d/scan.h>
|
||||
#if (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2)
|
||||
#include <opencv/cv.h>
|
||||
#include <opencv/highgui.h>
|
||||
#else
|
||||
#include <opencv2/opencv.hpp>
|
||||
#endif
|
||||
|
||||
void calculateNormalsAKNN(std::vector<Point> &normals,vector<Point> &points, int k,
|
||||
const double _rPos[3] );
|
||||
void calculateNormalsAdaptiveAKNN(vector<Point> &normals,vector<Point> &points,
|
||||
int kmin, int kmax, const double _rPos[3]);
|
||||
void calculateNormalsPANORAMA(vector<Point> &normals,
|
||||
vector<Point> &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_<cv::Vec4f> it;
|
||||
|
||||
it = scan.begin<cv::Vec4f>();
|
||||
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_<float> it = source.begin<float>();
|
||||
it != source.end<float>(); ++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 <cutoff> values are larger than it
|
||||
vector<float> sorted(source.cols*source.rows);
|
||||
int i = 0;
|
||||
for (cv::MatIterator_<float> it = source.begin<float>();
|
||||
it != source.end<float>(); ++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_<float> src = source.begin<float>();
|
||||
cv::MatIterator_<uchar> dst = result.begin<uchar>();
|
||||
cv::MatIterator_<float> end = source.end<float>();
|
||||
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
|
||||
|
|
@ -42,7 +42,6 @@ public:
|
|||
protected:
|
||||
virtual void createSearchTreePrivate();
|
||||
virtual void calcReducedOnDemandPrivate();
|
||||
virtual void calcNormalsOnDemandPrivate();
|
||||
virtual void addFrame(AlgoType type);
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -217,7 +217,6 @@ private:
|
|||
typedef TripleArray<double> DataXYZ;
|
||||
typedef TripleArray<float> DataXYZFloat;
|
||||
typedef TripleArray<unsigned char> DataRGB;
|
||||
typedef TripleArray<double> DataNormal;
|
||||
typedef SingleArray<float> DataReflectance;
|
||||
typedef SingleArray<float> DataTemperature;
|
||||
typedef SingleArray<float> DataAmplitude;
|
||||
|
|
|
|||
|
|
@ -273,8 +273,8 @@ inline void M4identity( T *M )
|
|||
*/
|
||||
template <class T>
|
||||
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 <class T>
|
||||
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 <class T>
|
||||
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,63 +1090,63 @@ 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;
|
||||
// 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];
|
||||
}
|
||||
}
|
||||
|
||||
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];
|
||||
}
|
||||
}
|
||||
|
||||
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,18 +1301,6 @@ 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];
|
||||
|
|
@ -1320,28 +1308,6 @@ 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;
|
||||
|
|
|
|||
|
|
@ -15,7 +15,6 @@ using std::vector;
|
|||
|
||||
#include "slam6d/scan.h"
|
||||
#include "slam6d/icp6Dminimizer.h"
|
||||
#include "slam6d/pairingMode.h"
|
||||
|
||||
/**
|
||||
* @brief Representation of 3D scan matching with ICP.
|
||||
|
|
@ -39,15 +38,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 <Scan *> allScans, PairingMode pairing_mode = CLOSEST_POINT);
|
||||
virtual int match(Scan* PreviousScan, Scan* CurrentScan, PairingMode pairing_mode = CLOSEST_POINT);
|
||||
void doICP(vector <Scan *> allScans);
|
||||
virtual int match(Scan* PreviousScan, Scan* CurrentScan);
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -45,8 +45,6 @@ public:
|
|||
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;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -55,8 +55,6 @@ 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;
|
||||
|
|
|
|||
|
|
@ -62,8 +62,6 @@ 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;
|
||||
|
|
|
|||
|
|
@ -76,10 +76,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 +126,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 +178,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
|
||||
|
|
@ -214,8 +214,8 @@ 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);
|
||||
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;
|
||||
|
||||
|
|
@ -233,44 +233,6 @@ 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);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -32,11 +32,6 @@ public:
|
|||
*/
|
||||
double *p;
|
||||
|
||||
/**
|
||||
* pointer to direction vector, if we're using FindClosestAlongDir
|
||||
*/
|
||||
double *dir;
|
||||
|
||||
/**
|
||||
* expand to 128 bytes to avoid false-sharing, 16 bytes from above + 28*4 bytes = 128 bytes
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -43,7 +43,6 @@ public:
|
|||
protected:
|
||||
virtual void createSearchTreePrivate();
|
||||
virtual void calcReducedOnDemandPrivate();
|
||||
virtual void calcNormalsOnDemandPrivate() {};
|
||||
virtual void addFrame(AlgoType type);
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -34,7 +34,6 @@ public:
|
|||
protected:
|
||||
virtual void createSearchTreePrivate();
|
||||
virtual void calcReducedOnDemandPrivate() {}
|
||||
virtual void calcNormalsOnDemandPrivate() {}
|
||||
virtual void addFrame(AlgoType type) {}
|
||||
private:
|
||||
std::vector<Scan*> m_scans;
|
||||
|
|
|
|||
|
|
@ -1,10 +0,0 @@
|
|||
#ifndef __PAIRINGMODE_H__
|
||||
#define __PAIRINGMODE_H__
|
||||
|
||||
enum PairingMode {
|
||||
CLOSEST_POINT,
|
||||
CLOSEST_POINT_ALONG_NORMAL,
|
||||
CLOSEST_PLANE
|
||||
};
|
||||
|
||||
#endif // PAIRINGMODE_H
|
||||
|
|
@ -101,12 +101,6 @@ 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;
|
||||
|
|
|
|||
|
|
@ -30,7 +30,6 @@ 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;
|
||||
|
|
@ -45,7 +44,6 @@ public:
|
|||
PointType(unsigned int _types);
|
||||
|
||||
bool hasReflectance();
|
||||
bool hasNormal();
|
||||
bool hasTemperature();
|
||||
bool hasAmplitude();
|
||||
bool hasDeviation();
|
||||
|
|
@ -116,7 +114,6 @@ private:
|
|||
unsigned int getScanSize(Scan* scan);
|
||||
|
||||
DataXYZ* m_xyz;
|
||||
DataXYZ* m_normal;
|
||||
DataRGB* m_rgb;
|
||||
DataReflectance* m_reflectance;
|
||||
DataTemperature* m_temperature;
|
||||
|
|
@ -137,11 +134,6 @@ 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;
|
||||
}
|
||||
|
|
@ -179,11 +171,6 @@ 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++];
|
||||
}
|
||||
|
|
@ -219,10 +206,6 @@ 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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,7 +5,6 @@
|
|||
#include "data_types.h"
|
||||
#include "point_type.h"
|
||||
#include "ptpair.h"
|
||||
#include "pairingMode.h"
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
|
@ -98,7 +97,10 @@ Last, if program ends, clean up
|
|||
class Scan {
|
||||
//friend class SearchTree; // TODO: is this neccessary?
|
||||
public:
|
||||
enum AlgoType { INVALID, ICP, ICPINACTIVE, LUM, ELCH };
|
||||
enum AlgoType {
|
||||
INVALID, ICP, ICPINACTIVE, LUM, ELCH, LOOPTORO, LOOPHOGMAN, GRAPHTORO,
|
||||
GRAPHHOGMAN
|
||||
};
|
||||
|
||||
// delete copy-ctor and assignment, scans shouldn't be copied by basic class
|
||||
Scan(const Scan& other) = delete;
|
||||
|
|
@ -106,8 +108,7 @@ 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<Scan*> allScans;
|
||||
|
||||
/**
|
||||
|
|
@ -285,7 +286,7 @@ public:
|
|||
Scan* Source, Scan* Target,
|
||||
int thread_num,
|
||||
int rnd, double max_dist_match2, double &sum,
|
||||
double *centroid_m, double *centroid_d, PairingMode pairing_mode = CLOSEST_POINT);
|
||||
double *centroid_m, double *centroid_d);
|
||||
static void getNoPairsSimple(std::vector<double*> &diff,
|
||||
Scan* Source, Scan* Target,
|
||||
int thread_num,
|
||||
|
|
@ -301,8 +302,7 @@ public:
|
|||
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);
|
||||
double centroid_d[OPENMP_NUM_THREADS][3]);
|
||||
|
||||
protected:
|
||||
/**
|
||||
|
|
@ -351,9 +351,6 @@ 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;
|
||||
|
|
@ -375,31 +372,19 @@ protected:
|
|||
|
||||
/**
|
||||
* This function handles the reduction of points. It builds a lock for
|
||||
* multithread-safety and calls calcReducedOnDemandPrivate.
|
||||
* multithread-safety and calls caldReducedOnDemandPrivate.
|
||||
*
|
||||
* 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]);
|
||||
|
||||
|
|
@ -407,11 +392,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();
|
||||
|
||||
|
|
@ -425,7 +410,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, m_mutex_normals;
|
||||
boost::mutex m_mutex_reduction, m_mutex_create_tree;
|
||||
};
|
||||
|
||||
#include "scan.icc"
|
||||
|
|
|
|||
|
|
@ -12,7 +12,6 @@ using std::vector;
|
|||
|
||||
#include "ptpair.h"
|
||||
#include "data_types.h"
|
||||
#include "pairingMode.h"
|
||||
|
||||
/**
|
||||
* @brief The tree structure
|
||||
|
|
@ -39,7 +38,6 @@ class Scan;
|
|||
class SearchTree : public Tree {
|
||||
friend class Scan;
|
||||
public:
|
||||
|
||||
/**
|
||||
* Constructor (default)
|
||||
*/
|
||||
|
|
@ -79,9 +77,7 @@ 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 *FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const;
|
||||
virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const = 0 ;
|
||||
|
||||
virtual void getPtPairs(vector <PtPair> *pairs,
|
||||
double *source_alignxf,
|
||||
|
|
@ -92,10 +88,10 @@ public:
|
|||
|
||||
virtual void getPtPairs(vector <PtPair> *pairs,
|
||||
double *source_alignxf,
|
||||
const DataXYZ& xyz_r, const DataNormal& normal_r, unsigned int startindex, unsigned int endindex,
|
||||
const DataXYZ& xyz_r, unsigned int startindex, unsigned int endindex,
|
||||
int thread_num,
|
||||
int rnd, double max_dist_match2, double &sum,
|
||||
double *centroid_m, double *centroid_d, PairingMode pairing_mode = CLOSEST_POINT);
|
||||
double *centroid_m, double *centroid_d);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,9 +1,7 @@
|
|||
IF(WITH_NORMALS)
|
||||
add_executable(normals normals.cc)
|
||||
|
||||
FIND_PACKAGE(OpenCV REQUIRED)
|
||||
|
||||
add_library(normals normals.cc)
|
||||
target_link_libraries(normals newmat scan ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS})
|
||||
|
||||
add_executable(calc_normals calc_normals.cc)
|
||||
target_link_libraries(calc_normals normals ${Boost_LIBRARIES})
|
||||
target_link_libraries(normals newmat scan ANN fbr_cv_io fbr_panorama ${Boost_LIBRARIES} ${OpenCV_LIBS})
|
||||
ENDIF(WITH_NORMALS)
|
||||
|
|
|
|||
|
|
@ -1,275 +0,0 @@
|
|||
/**
|
||||
*
|
||||
* Copyright (C) Jacobs University Bremen
|
||||
*
|
||||
* @author Vaibhav Kumar Mehta
|
||||
* @file calc_normals.cc
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <fstream>
|
||||
#include <errno.h>
|
||||
|
||||
#include <boost/program_options.hpp>
|
||||
|
||||
#include <slam6d/io_types.h>
|
||||
#include <slam6d/globals.icc>
|
||||
#include <slam6d/scan.h>
|
||||
#include "slam6d/fbr/panorama.h"
|
||||
#include <scanserver/clientInterface.h>
|
||||
|
||||
#include <normals/normals.h>
|
||||
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#define strcasecmp _stricmp
|
||||
#define strncasecmp _strnicmp
|
||||
#else
|
||||
#include <strings.h>
|
||||
#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<std::string>& 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<std::string>& 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 <options> where options are (default values in brackets)");
|
||||
cmd_options.add_options()
|
||||
("help,?", "Display this help message")
|
||||
("start,s", po::value<int>(&start)->default_value(0), "Start at scan number <arg>")
|
||||
("end,e", po::value<int>(&end)->default_value(-1), "Stop at scan number <arg>")
|
||||
("scanserver,S", po::value<bool>(&scanserver)->default_value(false), "Use the scanserver as an input method")
|
||||
("format,f", po::value<IOType>(&iotype)->default_value(UOS),
|
||||
"using shared library <arg> 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<int>(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than <arg> 'units")
|
||||
("min,m", po::value<int>(&min_dist)->default_value(-1),"neglegt all data points with a distance smaller than <arg> 'units")
|
||||
("normal,g", po::value<normal_method>(&ntype)->default_value(AKNN), "normal calculation method "
|
||||
"(AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST)")
|
||||
("K1,k", po::value<int>(&k1)->default_value(20), "<arg> value of K value used in the nearest neighbor search of ANN or" "kmin for k-adaptation")
|
||||
("K2,K", po::value<int>(&k2)->default_value(20), "<arg> value of Kmax for k-adaptation")
|
||||
("width,w", po::value<int>(&width)->default_value(1280),"width of panorama image")
|
||||
("height,h", po::value<int>(&height)->default_value(960),"height of panorama image")
|
||||
;
|
||||
|
||||
po::options_description hidden("Hidden options");
|
||||
hidden.add_options()
|
||||
("input-dir", po::value<string>(&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/" <<endl;
|
||||
cout << endl << endl;
|
||||
cout << "SAMPLE COMMAND FOR VIEWING CALCULATING NORMALS IN RGB SPACE" << endl;
|
||||
cout << " bin/show -c -f UOS_RGB dat/normals/" << endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
// read scan path
|
||||
if (dir[dir.length()-1] != '/') dir = dir + "/";
|
||||
|
||||
}
|
||||
|
||||
/// 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<Point> &points, vector<Point> &normals, int scanNumber)
|
||||
{
|
||||
string ofilename = dir + "/scan" + to_string(scanNumber, 3) + ".3d";
|
||||
ofstream normptsout(ofilename.c_str());
|
||||
|
||||
for (size_t i=0; i<points.size(); ++i)
|
||||
{
|
||||
int r,g,b;
|
||||
r = (int)(normals[i].x * (127.5) + 127.5);
|
||||
g = (int)(normals[i].y * (127.5) + 127.5);
|
||||
b = (int)(fabs(normals[i].z) * (255.0));
|
||||
normptsout <<points[i].x<<" "<<points[i].y<<" "<<points[i].z<<" "<<r<<" "<<g<<" "<<b<<" "<<endl;
|
||||
}
|
||||
normptsout.clear();
|
||||
normptsout.close();
|
||||
}
|
||||
|
||||
/// =============================================
|
||||
/// Main
|
||||
/// =============================================
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
int start, end;
|
||||
bool scanserver;
|
||||
int max_dist, min_dist;
|
||||
string dir;
|
||||
IOType iotype;
|
||||
int k1, k2;
|
||||
normal_method ntype;
|
||||
int width, height;
|
||||
|
||||
parse_options(argc, argv, start, end, scanserver, max_dist, min_dist,
|
||||
dir, iotype, k1, k2, ntype, width, height);
|
||||
|
||||
/// ----------------------------------
|
||||
/// Prepare and read scans
|
||||
/// ----------------------------------
|
||||
if (scanserver) {
|
||||
try {
|
||||
ClientInterface::create();
|
||||
} catch(std::runtime_error& e) {
|
||||
cerr << "ClientInterface could not be created: " << e.what() << endl;
|
||||
cerr << "Start the scanserver first." << endl;
|
||||
exit(-1);
|
||||
}
|
||||
}
|
||||
|
||||
/// Make directory for saving the scan segments
|
||||
string normdir = dir + "normals";
|
||||
|
||||
#ifdef _MSC_VER
|
||||
int success = mkdir(normdir.c_str());
|
||||
#else
|
||||
int success = mkdir(normdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
|
||||
#endif
|
||||
if(success == 0) {
|
||||
cout << "Writing segments to " << normdir << endl;
|
||||
} else if(errno == EEXIST) {
|
||||
cout << "WARN: Directory " << normdir << " exists already. Contents will be overwriten" << endl;
|
||||
} else {
|
||||
cerr << "Creating directory " << normdir << " failed" << endl;
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/// Read the scans
|
||||
Scan::openDirectory(scanserver, dir, iotype, start, end);
|
||||
if(Scan::allScans.size() == 0) {
|
||||
cerr << "No scans found. Did you use the correct format?" << endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
cv::Mat img;
|
||||
|
||||
/// --------------------------------------------
|
||||
/// Initialize and perform segmentation
|
||||
/// --------------------------------------------
|
||||
std::vector<Scan*>::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<Point> points;
|
||||
points.reserve(xyz.size());
|
||||
vector<Point> 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();
|
||||
|
||||
Scan::closeDirectory();
|
||||
|
||||
cout << "Normal program end" << endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,29 +1,125 @@
|
|||
/**
|
||||
*
|
||||
*
|
||||
* Copyright (C) Jacobs University Bremen
|
||||
*
|
||||
* @author Vaibhav Kumar Mehta
|
||||
* @author Corneliu Claudiu Prodescu
|
||||
* @file normals.cc
|
||||
*/
|
||||
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <fstream>
|
||||
#include <errno.h>
|
||||
|
||||
#include <boost/program_options.hpp>
|
||||
|
||||
#include <ANN/ANN.h>
|
||||
#include <slam6d/io_types.h>
|
||||
#include <slam6d/globals.icc>
|
||||
#include <slam6d/scan.h>
|
||||
#include "slam6d/fbr/panorama.h"
|
||||
#include <scanserver/clientInterface.h>
|
||||
|
||||
#include <ANN/ANN.h>
|
||||
#include "newmat/newmat.h"
|
||||
#include "newmat/newmatap.h"
|
||||
|
||||
#include "normals/normals.h"
|
||||
|
||||
using namespace NEWMAT;
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#define strcasecmp _stricmp
|
||||
#define strncasecmp _strnicmp
|
||||
#else
|
||||
#include <strings.h>
|
||||
#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<std::string>& 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<std::string>& 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 <options> where options are (default values in brackets)");
|
||||
cmd_options.add_options()
|
||||
("help,?", "Display this help message")
|
||||
("start,s", po::value<int>(&start)->default_value(0), "Start at scan number <arg>")
|
||||
("end,e", po::value<int>(&end)->default_value(-1), "Stop at scan number <arg>")
|
||||
("scanserver,S", po::value<bool>(&scanserver)->default_value(false), "Use the scanserver as an input method")
|
||||
("format,f", po::value<IOType>(&iotype)->default_value(UOS),
|
||||
"using shared library <arg> 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<int>(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than <arg> 'units")
|
||||
("min,m", po::value<int>(&min_dist)->default_value(-1),"neglegt all data points with a distance smaller than <arg> 'units")
|
||||
("normal,g", po::value<normal_method>(&ntype)->default_value(AKNN), "normal calculation method "
|
||||
"(AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST)")
|
||||
("K1,k", po::value<int>(&k1)->default_value(20), "<arg> value of K value used in the nearest neighbor search of ANN or" "kmin for k-adaptation")
|
||||
("K2,K", po::value<int>(&k2)->default_value(20), "<arg> value of Kmax for k-adaptation")
|
||||
("width,w", po::value<int>(&width)->default_value(1280),"width of panorama image")
|
||||
("height,h", po::value<int>(&height)->default_value(960),"height of panorama image")
|
||||
;
|
||||
|
||||
po::options_description hidden("Hidden options");
|
||||
hidden.add_options()
|
||||
("input-dir", po::value<string>(&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/" <<endl;
|
||||
cout << endl << endl;
|
||||
cout << "SAMPLE COMMAND FOR VIEWING CALCULATING NORMALS IN RGB SPACE" << endl;
|
||||
cout << " bin/show -c -f UOS_RGB dat/normals/" << endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
// read scan path
|
||||
if (dir[dir.length()-1] != '/') dir = dir + "/";
|
||||
|
||||
}
|
||||
///////////////////////////////////////////////////////
|
||||
/////////////NORMALS USING AKNN METHOD ////////////////
|
||||
///////////////////////////////////////////////////////
|
||||
|
|
@ -35,69 +131,69 @@ void calculateNormalsAKNN(vector<Point> &normals,vector<Point> &points, int k, c
|
|||
ColumnVector rPos(3);
|
||||
for (int i = 0; i < 3; ++i)
|
||||
rPos(i+1) = _rPos[i];
|
||||
|
||||
|
||||
ANNpointArray pa = annAllocPts(points.size(), 3);
|
||||
for (size_t i=0; i<points.size(); ++i)
|
||||
{
|
||||
pa[i][0] = points[i].x;
|
||||
pa[i][1] = points[i].y;
|
||||
pa[i][2] = points[i].z;
|
||||
}
|
||||
ANNkd_tree t(pa, points.size(), 3);
|
||||
{
|
||||
pa[i][0] = points[i].x;
|
||||
pa[i][1] = points[i].y;
|
||||
pa[i][2] = points[i].z;
|
||||
}
|
||||
ANNkd_tree t(pa, points.size(), 3);
|
||||
ANNidxArray nidx = new ANNidx[nr_neighbors];
|
||||
ANNdistArray d = new ANNdist[nr_neighbors];
|
||||
ANNdistArray d = new ANNdist[nr_neighbors];
|
||||
|
||||
for (size_t i=0; i<points.size(); ++i)
|
||||
{
|
||||
ANNpoint p = pa[i];
|
||||
//ANN search for k nearest neighbors
|
||||
//indexes of the neighbors along with the query point
|
||||
//stored in the array n
|
||||
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
|
||||
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 += points[nidx[j]].x;
|
||||
mean.y += points[nidx[j]].y;
|
||||
mean.z += points[nidx[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) = points[nidx[i]].x - mean.x;
|
||||
X(i+1, 2) = points[nidx[i]].y - mean.y;
|
||||
X(i+1, 3) = points[nidx[i]].z - mean.z;
|
||||
}
|
||||
ANNpoint p = pa[i];
|
||||
//ANN search for k nearest neighbors
|
||||
//indexes of the neighbors along with the query point
|
||||
//stored in the array n
|
||||
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
|
||||
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 += points[nidx[j]].x;
|
||||
mean.y += points[nidx[j]].y;
|
||||
mean.z += points[nidx[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) = points[nidx[i]].x - mean.x;
|
||||
X(i+1, 2) = points[nidx[i]].y - mean.y;
|
||||
X(i+1, 3) = points[nidx[i]].z - mean.z;
|
||||
}
|
||||
|
||||
A << 1.0/nr_neighbors * X.t() * X;
|
||||
|
||||
A << 1.0/nr_neighbors * X.t() * X;
|
||||
EigenValues(A, D, U);
|
||||
|
||||
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;
|
||||
//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)));
|
||||
}
|
||||
n = n / n.NormFrobenius();
|
||||
normals.push_back(Point(n(1), n(2), n(3)));
|
||||
}
|
||||
|
||||
delete[] nidx;
|
||||
delete[] d;
|
||||
|
|
@ -107,97 +203,98 @@ void calculateNormalsAKNN(vector<Point> &normals,vector<Point> &points, int k, c
|
|||
/////////////NORMALS USING ADAPTIVE AKNN METHOD ////////////////
|
||||
////////////////////////////////////////////////////////////////
|
||||
void calculateNormalsAdaptiveAKNN(vector<Point> &normals,vector<Point> &points,
|
||||
int kmin, int kmax, const double _rPos[3])
|
||||
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: "<<points.size()<<endl;
|
||||
int nr_neighbors;
|
||||
int nr_neighbors;
|
||||
ANNpointArray pa = annAllocPts(points.size(), 3);
|
||||
for (size_t i=0; i<points.size(); ++i)
|
||||
{
|
||||
pa[i][0] = points[i].x;
|
||||
pa[i][1] = points[i].y;
|
||||
pa[i][2] = points[i].z;
|
||||
}
|
||||
ANNkd_tree t(pa, points.size(), 3);
|
||||
{
|
||||
pa[i][0] = points[i].x;
|
||||
pa[i][1] = points[i].y;
|
||||
pa[i][2] = points[i].z;
|
||||
}
|
||||
ANNkd_tree t(pa, points.size(), 3);
|
||||
|
||||
Point mean(0.0,0.0,0.0);
|
||||
double e1,e2,e3;
|
||||
|
||||
double temp_n[3],norm_n = 0.0;
|
||||
double e1,e2,e3;
|
||||
|
||||
for (size_t i=0; i<points.size(); ++i)
|
||||
{
|
||||
Matrix U(3,3);
|
||||
ANNpoint p = pa[i];
|
||||
for(int kidx = kmin; kidx < kmax; kidx++)
|
||||
{
|
||||
nr_neighbors=kidx+1;
|
||||
ANNidxArray nidx = new ANNidx[nr_neighbors];
|
||||
ANNdistArray d = new ANNdist[nr_neighbors];
|
||||
//ANN search for k nearest neighbors
|
||||
//indexes of the neighbors along with the query point
|
||||
//stored in the array n
|
||||
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
|
||||
mean.x=0,mean.y=0,mean.z=0;
|
||||
//calculate mean for all the points
|
||||
for (int j=0; j<nr_neighbors; ++j)
|
||||
{
|
||||
mean.x += points[nidx[j]].x;
|
||||
mean.y += points[nidx[j]].y;
|
||||
mean.z += points[nidx[j]].z;
|
||||
}
|
||||
mean.x /= nr_neighbors;
|
||||
mean.y /= nr_neighbors;
|
||||
mean.z /= nr_neighbors;
|
||||
Matrix U(3,3);
|
||||
ANNpoint p = pa[i];
|
||||
for(int kidx = kmin; kidx < kmax; kidx++)
|
||||
{
|
||||
nr_neighbors=kidx+1;
|
||||
ANNidxArray nidx = new ANNidx[nr_neighbors];
|
||||
ANNdistArray d = new ANNdist[nr_neighbors];
|
||||
//ANN search for k nearest neighbors
|
||||
//indexes of the neighbors along with the query point
|
||||
//stored in the array n
|
||||
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
|
||||
mean.x=0,mean.y=0,mean.z=0;
|
||||
//calculate mean for all the points
|
||||
for (int j=0; j<nr_neighbors; ++j)
|
||||
{
|
||||
mean.x += points[nidx[j]].x;
|
||||
mean.y += points[nidx[j]].y;
|
||||
mean.z += points[nidx[j]].z;
|
||||
}
|
||||
mean.x /= nr_neighbors;
|
||||
mean.y /= nr_neighbors;
|
||||
mean.z /= nr_neighbors;
|
||||
|
||||
Matrix X(nr_neighbors,3);
|
||||
SymmetricMatrix A(3);
|
||||
DiagonalMatrix D(3);
|
||||
Matrix X(nr_neighbors,3);
|
||||
SymmetricMatrix A(3);
|
||||
DiagonalMatrix D(3);
|
||||
|
||||
//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;
|
||||
}
|
||||
//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;
|
||||
}
|
||||
|
||||
A << 1.0/nr_neighbors * X.t() * X;
|
||||
|
||||
EigenValues(A, D, U);
|
||||
|
||||
A << 1.0/nr_neighbors * X.t() * X;
|
||||
|
||||
EigenValues(A, D, U);
|
||||
|
||||
e1 = D(1);
|
||||
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))
|
||||
break;
|
||||
e1 = D(1);
|
||||
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))
|
||||
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)));
|
||||
}
|
||||
|
||||
//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);
|
||||
}
|
||||
|
||||
|
|
@ -205,112 +302,113 @@ void calculateNormalsAdaptiveAKNN(vector<Point> &normals,vector<Point> &points,
|
|||
/////////////NORMALS USING IMAGE NEIGHBORS ////////////
|
||||
///////////////////////////////////////////////////////
|
||||
void calculateNormalsPANORAMA(vector<Point> &normals,
|
||||
vector<Point> &points,
|
||||
vector< vector< vector< cv::Vec3f > > > extendedMap,
|
||||
const double _rPos[3])
|
||||
vector<Point> &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: "<<points.size()<<endl;
|
||||
points.clear();
|
||||
int nr_neighbors = 0;
|
||||
cout << "height of Image: "<<extendedMap.size()<<endl;
|
||||
cout << "width of Image: "<<extendedMap[0].size()<<endl;
|
||||
|
||||
// as the nearest neighbors and then the same PCA method as done in AKNN
|
||||
//temporary dynamic array for all the neighbors of a given point
|
||||
|
||||
// as the nearest neighbors and then the same PCA method as done in AKNN
|
||||
//temporary dynamic array for all the neighbors of a given point
|
||||
vector<cv::Vec3f> neighbors;
|
||||
for (size_t i=0; i< extendedMap.size(); i++)
|
||||
{
|
||||
for (size_t j=0; j<extendedMap[i].size(); j++)
|
||||
{
|
||||
if (extendedMap[i][j].size() == 0) continue;
|
||||
neighbors.clear();
|
||||
Point mean(0.0,0.0,0.0);
|
||||
for (size_t j=0; j<extendedMap[i].size(); j++)
|
||||
{
|
||||
if (extendedMap[i][j].size() == 0) continue;
|
||||
neighbors.clear();
|
||||
Point mean(0.0,0.0,0.0);
|
||||
double temp_n[3],norm_n = 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;
|
||||
}
|
||||
|
||||
// Offset for neighbor computation
|
||||
int offset[2][5] = {{-1,0,1,0,0},{0,-1,0,1,0}};
|
||||
//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();
|
||||
|
||||
// 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)));
|
||||
}
|
||||
}
|
||||
}
|
||||
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 //////////////////////////
|
||||
|
|
@ -321,7 +419,7 @@ void calculateNormalsPANORAMA(vector<Point> &normals,
|
|||
cout<<"Total number of points: "<<points.size()<<endl;
|
||||
points.clear();
|
||||
int nr_points = 0;
|
||||
//int nr_neighbors = 0,nr_neighbors_center = 0;
|
||||
//int nr_neighbors = 0,nr_neighbors_center = 0;
|
||||
cout << "height of Image: "<<extendedMap.size()<<endl;
|
||||
cout << "width of Image: "<<extendedMap[0].size()<<endl;
|
||||
for (size_t i=0; i< extendedMap.size(); ++i)
|
||||
|
|
@ -334,57 +432,322 @@ void calculateNormalsPANORAMA(vector<Point> &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<uchar>(i-1,j);
|
||||
dRdPhi += 3 *img.at<uchar>(i-1,j-1);
|
||||
dRdPhi += 3 *img.at<uchar>(i-1,j+1);
|
||||
dRdPhi -= 10*img.at<uchar>(i+1,j);
|
||||
dRdPhi -= 3 *img.at<uchar>(i+1,j-1);
|
||||
dRdPhi -= 3 *img.at<uchar>(i+1,j+1);
|
||||
|
||||
|
||||
dRdTheta += 10*img.at<uchar>(i,j-1);
|
||||
dRdTheta += 3 *img.at<uchar>(i-1,j-1);
|
||||
dRdTheta += 3 *img.at<uchar>(i+1,j-1);
|
||||
dRdTheta -= 10*img.at<uchar>(i,j+1);
|
||||
dRdTheta -= 3 *img.at<uchar>(i-1,j+1);
|
||||
dRdTheta -= 3 *img.at<uchar>(i+1,j+1);
|
||||
|
||||
n[0] = cos(theta) * sin(phi) - sin(theta) * dRdTheta / rho / sin(phi) +
|
||||
dRdTheta -= 3 *img.at<uchar>(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_<cv::Vec4f> it;
|
||||
|
||||
it = scan.begin<cv::Vec4f>();
|
||||
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_<float> it = source.begin<float>();
|
||||
it != source.end<float>(); ++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 <cutoff> values are larger than it
|
||||
vector<float> sorted(source.cols*source.rows);
|
||||
int i = 0;
|
||||
for (cv::MatIterator_<float> it = source.begin<float>();
|
||||
it != source.end<float>(); ++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_<float> src = source.begin<float>();
|
||||
cv::MatIterator_<uchar> dst = result.begin<uchar>();
|
||||
cv::MatIterator_<float> end = source.end<float>();
|
||||
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<Point> &points, vector<Point> &normals, int scanNumber)
|
||||
{
|
||||
string ofilename = dir + "/scan" + to_string(scanNumber, 3) + ".3d";
|
||||
ofstream normptsout(ofilename.c_str());
|
||||
|
||||
for (size_t i=0; i<points.size(); ++i)
|
||||
{
|
||||
int r,g,b;
|
||||
r = (int)(normals[i].x * (127.5) + 127.5);
|
||||
g = (int)(normals[i].y * (127.5) + 127.5);
|
||||
b = (int)(fabs(normals[i].z) * (255.0));
|
||||
normptsout <<points[i].x<<" "<<points[i].y<<" "<<points[i].z<<" "<<r<<" "<<g<<" "<<b<<" "<<endl;
|
||||
}
|
||||
normptsout.clear();
|
||||
normptsout.close();
|
||||
}
|
||||
|
||||
/// =============================================
|
||||
/// Main
|
||||
/// =============================================
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
int start, end;
|
||||
bool scanserver;
|
||||
int max_dist, min_dist;
|
||||
string dir;
|
||||
IOType iotype;
|
||||
int k1, k2;
|
||||
normal_method ntype;
|
||||
int width, height;
|
||||
|
||||
parse_options(argc, argv, start, end, scanserver, max_dist, min_dist,
|
||||
dir, iotype, k1, k2, ntype, width, height);
|
||||
|
||||
/// ----------------------------------
|
||||
/// Prepare and read scans
|
||||
/// ----------------------------------
|
||||
if (scanserver) {
|
||||
try {
|
||||
ClientInterface::create();
|
||||
} catch(std::runtime_error& e) {
|
||||
cerr << "ClientInterface could not be created: " << e.what() << endl;
|
||||
cerr << "Start the scanserver first." << endl;
|
||||
exit(-1);
|
||||
}
|
||||
}
|
||||
|
||||
/// Make directory for saving the scan segments
|
||||
string normdir = dir + "normals";
|
||||
|
||||
#ifdef _MSC_VER
|
||||
int success = mkdir(normdir.c_str());
|
||||
#else
|
||||
int success = mkdir(normdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
|
||||
#endif
|
||||
if(success == 0) {
|
||||
cout << "Writing segments to " << normdir << endl;
|
||||
} else if(errno == EEXIST) {
|
||||
cout << "WARN: Directory " << normdir << " exists already. Contents will be overwriten" << endl;
|
||||
} else {
|
||||
cerr << "Creating directory " << normdir << " failed" << endl;
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/// Read the scans
|
||||
Scan::openDirectory(scanserver, dir, iotype, start, end);
|
||||
if(Scan::allScans.size() == 0) {
|
||||
cerr << "No scans found. Did you use the correct format?" << endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
cv::Mat img;
|
||||
|
||||
/// --------------------------------------------
|
||||
/// Initialize and perform segmentation
|
||||
/// --------------------------------------------
|
||||
std::vector<Scan*>::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<Point> points;
|
||||
points.reserve(xyz.size());
|
||||
vector<Point> 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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
SET(SHOW_LIBS glui scan ANN ${OPENGL_LIBRARIES} normals newmat)
|
||||
SET(SHOW_LIBS glui scan ANN ${OPENGL_LIBRARIES})
|
||||
IF(WIN32)
|
||||
IF( CMAKE_SIZEOF_VOID_P EQUAL 8 )
|
||||
SET(SHOW_LIBS ${SHOW_LIBS} ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/freeglut.lib XGetopt)
|
||||
|
|
|
|||
|
|
@ -160,6 +160,18 @@ 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;
|
||||
|
|
|
|||
|
|
@ -626,7 +626,9 @@ void setResetView(int origin) {
|
|||
// set origin to the center of mass of all scans
|
||||
for (size_t i = 0; i < octpts.size(); ++i) {
|
||||
vector <sfloat*> points;
|
||||
#ifndef USE_COMPACT_TREE
|
||||
#ifdef USE_COMPACT_TREE
|
||||
((compactTree*)octpts[i])->AllPoints( points );
|
||||
#else
|
||||
BOctTree<sfloat>* cur_tree = ((Show_BOctTree<sfloat>*)octpts[i])->getTree();
|
||||
cur_tree->AllPoints( points );
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -61,7 +61,7 @@ ENDIF(WITH_TOOLS)
|
|||
|
||||
SET(SCANLIB_SRCS
|
||||
kd.cc kdManaged.cc kdMeta.cc graphSlam6D.cc
|
||||
graph.cc icp6D.cc icp6Dapx.cc icp6Dsvd.cc
|
||||
graph.cc icp6Dapx.cc icp6D.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,9 +77,7 @@ endif(WITH_METRICS)
|
|||
|
||||
add_library(scan STATIC ${SCANLIB_SRCS})
|
||||
|
||||
FIND_PACKAGE(OpenCV REQUIRED)
|
||||
|
||||
target_link_libraries(scan scanclient scanio normals)
|
||||
target_link_libraries(scan scanclient scanio)
|
||||
|
||||
IF(UNIX)
|
||||
target_link_libraries(scan dl)
|
||||
|
|
|
|||
|
|
@ -242,8 +242,6 @@ 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
|
||||
|
|
@ -328,20 +326,12 @@ 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
|
||||
|
|
|
|||
|
|
@ -92,8 +92,7 @@ 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,
|
||||
PairingMode pairing_mode)
|
||||
int icp6D::match(Scan* PreviousScan, Scan* CurrentScan)
|
||||
{
|
||||
// If ICP shall not be applied, then just write
|
||||
// the identity matrix
|
||||
|
|
@ -149,7 +148,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, pairing_mode);
|
||||
sum, centroid_m, centroid_d);
|
||||
|
||||
n[thread_num] = (unsigned int)pairs[thread_num].size();
|
||||
|
||||
|
|
@ -215,7 +214,7 @@ int icp6D::match(Scan* PreviousScan, Scan* CurrentScan,
|
|||
vector<PtPair> pairs;
|
||||
|
||||
Scan::getPtPairs(&pairs, PreviousScan, CurrentScan, 0, rnd,
|
||||
max_dist_match2, ret, centroid_m, centroid_d, pairing_mode);
|
||||
max_dist_match2, ret, centroid_m, centroid_d);
|
||||
|
||||
// do we have enough point pairs?
|
||||
if (pairs.size() > 3) {
|
||||
|
|
@ -282,7 +281,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, CLOSEST_POINT);
|
||||
sum, centroid_m, centroid_d);
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -300,8 +299,7 @@ double icp6D::Point_Point_Error(Scan* PreviousScan, Scan* CurrentScan, double ma
|
|||
double centroid_d[3] = {0.0, 0.0, 0.0};
|
||||
vector<PtPair> pairs;
|
||||
|
||||
Scan::getPtPairs(&pairs, PreviousScan, CurrentScan, 0, rnd, sqr(max_dist_match),
|
||||
error, centroid_m, centroid_d, CLOSEST_POINT);
|
||||
Scan::getPtPairs(&pairs, PreviousScan, CurrentScan, 0, rnd, sqr(max_dist_match),error, centroid_m, centroid_d);
|
||||
|
||||
// getPtPairs computes error as sum of squared distances
|
||||
error = 0;
|
||||
|
|
@ -324,7 +322,7 @@ double icp6D::Point_Point_Error(Scan* PreviousScan, Scan* CurrentScan, double ma
|
|||
*
|
||||
* @param allScans Contains all necessary scans.
|
||||
*/
|
||||
void icp6D::doICP(vector <Scan *> allScans, PairingMode pairing_mode)
|
||||
void icp6D::doICP(vector <Scan *> allScans)
|
||||
{
|
||||
double id[16];
|
||||
M4identity(id);
|
||||
|
|
@ -348,12 +346,12 @@ void icp6D::doICP(vector <Scan *> allScans, PairingMode pairing_mode)
|
|||
|
||||
if (i > 0) {
|
||||
if (meta) {
|
||||
match(my_MetaScan, CurrentScan, pairing_mode);
|
||||
match(my_MetaScan, CurrentScan);
|
||||
} else
|
||||
if (cad_matching) {
|
||||
match(allScans[0], CurrentScan, pairing_mode);
|
||||
match(allScans[0], CurrentScan);
|
||||
} else {
|
||||
match(PreviousScan, CurrentScan, pairing_mode);
|
||||
match(PreviousScan, CurrentScan);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -67,13 +67,3 @@ 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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -60,16 +60,6 @@ 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<boost::mutex> lock(m_mutex_locking);
|
||||
|
|
|
|||
|
|
@ -97,16 +97,6 @@ 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<boost::mutex> lock(m_mutex_locking);
|
||||
|
|
|
|||
|
|
@ -46,25 +46,18 @@ PointType::PointType(unsigned int _types) : types(_types) {
|
|||
|
||||
pointdim = 3;
|
||||
if (types & PointType::USE_REFLECTANCE) dimensionmap[1] = 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++;
|
||||
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++;
|
||||
}
|
||||
|
||||
bool PointType::hasReflectance() {
|
||||
return hasType(USE_REFLECTANCE);
|
||||
}
|
||||
bool PointType::hasNormal() {
|
||||
return hasType(USE_NORMAL);
|
||||
}
|
||||
bool PointType::hasTemperature() {
|
||||
return hasType(USE_TEMPERATURE);
|
||||
}
|
||||
|
|
@ -123,20 +116,18 @@ unsigned int PointType::getType(unsigned int type) {
|
|||
return dimensionmap[0];
|
||||
} else if (type == USE_REFLECTANCE) {
|
||||
return dimensionmap[1];
|
||||
} else if (type == USE_NORMAL) {
|
||||
return dimensionmap[2];
|
||||
} else if (type == USE_TEMPERATURE) {
|
||||
return dimensionmap[3];
|
||||
return dimensionmap[2];
|
||||
} else if (type == USE_AMPLITUDE) {
|
||||
return dimensionmap[4];
|
||||
return dimensionmap[3];
|
||||
} else if (type == USE_DEVIATION) {
|
||||
return dimensionmap[5];
|
||||
return dimensionmap[4];
|
||||
} else if (type == USE_TYPE) {
|
||||
return dimensionmap[6];
|
||||
return dimensionmap[5];
|
||||
} else if (type == USE_COLOR) {
|
||||
return dimensionmap[7];
|
||||
return dimensionmap[6];
|
||||
} else if (type == USE_TIME) {
|
||||
return dimensionmap[8];
|
||||
return dimensionmap[7];
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -164,15 +155,14 @@ 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_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;
|
||||
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;
|
||||
|
||||
|
||||
void PointType::useScan(Scan* scan)
|
||||
|
|
|
|||
|
|
@ -17,8 +17,6 @@
|
|||
#include "slam6d/Boctree.h"
|
||||
#include "slam6d/globals.icc"
|
||||
|
||||
#include "normals/normals.h"
|
||||
|
||||
#ifdef WITH_METRICS
|
||||
#include "slam6d/metrics.h"
|
||||
#endif
|
||||
|
|
@ -39,7 +37,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)
|
||||
|
|
@ -147,9 +145,7 @@ 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<boost::mutex> lock(m_mutex_create_tree);
|
||||
if(kd != 0) return;
|
||||
|
||||
|
|
@ -169,9 +165,7 @@ 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<boost::mutex> lock(m_mutex_reduction);
|
||||
if(m_has_reduced) return;
|
||||
|
||||
|
|
@ -188,17 +182,6 @@ 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<boost::mutex> lock(m_mutex_normals);
|
||||
if(m_has_normals) return;
|
||||
calcNormalsOnDemandPrivate();
|
||||
m_has_normals = true;
|
||||
}
|
||||
|
||||
void Scan::copyReducedToOriginal()
|
||||
{
|
||||
#ifdef WITH_METRICS
|
||||
|
|
@ -240,33 +223,7 @@ 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<Point> points;
|
||||
points.reserve(xyz.size());
|
||||
vector<Point> 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;
|
||||
calculateNormalsAKNN(normals, points, K_NEIGHBOURS, get_rPos());
|
||||
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.
|
||||
|
|
@ -279,110 +236,137 @@ void Scan::calcReducedPoints()
|
|||
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
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<double> *oct = new BOctTree<double>(PointerArray<double>(xyz).get(),
|
||||
xyz.size(), reduction_voxelSize, reduction_pointtype);
|
||||
|
||||
vector<double*> 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];
|
||||
}
|
||||
|
||||
#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
|
||||
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.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];
|
||||
}
|
||||
}
|
||||
}
|
||||
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<double> *oct = new BOctTree<double>(xyz_in,
|
||||
xyz.size(),
|
||||
reduction_voxelSize,
|
||||
reduction_pointtype);
|
||||
BOctTree<double> *oct = new BOctTree<double>(xyz_reflectance, xyz.size(), reduction_voxelSize, reduction_pointtype);
|
||||
vector<double*> reduced;
|
||||
reduced.clear();
|
||||
|
||||
vector<double*> center;
|
||||
center.clear();
|
||||
if (reduction_nrpts > 0) {
|
||||
if (reduction_nrpts == 1) {
|
||||
oct->GetOctTreeRandom(center);
|
||||
} else {
|
||||
oct->GetOctTreeRandom(center, reduction_nrpts);
|
||||
}
|
||||
if (reduction_nrpts == 1) {
|
||||
oct->GetOctTreeRandom(reduced);
|
||||
} else {
|
||||
oct->GetOctTreeRandom(reduced, reduction_nrpts);
|
||||
}
|
||||
} else {
|
||||
oct->GetOctTreeCenter(center);
|
||||
oct->GetOctTreeCenter(reduced);
|
||||
}
|
||||
|
||||
// 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++];
|
||||
}
|
||||
}
|
||||
|
||||
// storing it as reduced scan
|
||||
unsigned int size = reduced.size();
|
||||
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];
|
||||
}
|
||||
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
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -415,7 +399,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]);
|
||||
}
|
||||
|
|
@ -431,17 +415,11 @@ 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
|
||||
|
|
@ -460,7 +438,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
|
||||
|
|
@ -502,7 +480,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
|
||||
|
|
@ -594,7 +572,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);
|
||||
|
|
@ -677,9 +655,9 @@ void Scan::transformToQuat(double rP[3], double rPQ[4], const AlgoType type, int
|
|||
*/
|
||||
|
||||
void Scan::getNoPairsSimple(vector <double*> &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<double>(Target->get("xyz reduced")).get(), Target->size<DataXYZ>("xyz reduced"));
|
||||
|
|
@ -716,10 +694,10 @@ void Scan::getNoPairsSimple(vector <double*> &diff,
|
|||
* @param max_dist_match2 maximal allowed distance for matching
|
||||
*/
|
||||
void Scan::getPtPairsSimple(vector <PtPair> *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<double>(Source->get("xyz reduced")).get(), Source->size<DataXYZ>("xyz reduced"));
|
||||
DataXYZ xyz_reduced(Target->get("xyz reduced"));
|
||||
|
|
@ -773,10 +751,10 @@ void Scan::getPtPairsSimple(vector <PtPair> *pairs,
|
|||
* @return a set of corresponding point pairs
|
||||
*/
|
||||
void Scan::getPtPairs(vector <PtPair> *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)
|
||||
Scan* Source, Scan* Target,
|
||||
int thread_num,
|
||||
int rnd, double max_dist_match2, double &sum,
|
||||
double *centroid_m, double *centroid_d)
|
||||
{
|
||||
// initialize centroids
|
||||
for(unsigned int i = 0; i < 3; ++i) {
|
||||
|
|
@ -786,12 +764,10 @@ void Scan::getPtPairs(vector <PtPair> *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, normal_reduced, 0, xyz_reduced.size(),
|
||||
thread_num,
|
||||
rnd, max_dist_match2, sum, centroid_m, centroid_d,
|
||||
pairing_mode);
|
||||
xyz_reduced, 0, xyz_reduced.size(),
|
||||
thread_num,
|
||||
rnd, max_dist_match2, sum, centroid_m, centroid_d);
|
||||
|
||||
// normalize centroids
|
||||
unsigned int size = pairs->size();
|
||||
|
|
@ -825,14 +801,11 @@ void Scan::getPtPairs(vector <PtPair> *pairs,
|
|||
* by Langis / Greenspan / Godin, IEEE 3DIM 2001
|
||||
*
|
||||
*/
|
||||
void Scan::getPtPairsParallel(vector <PtPair> *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)
|
||||
void Scan::getPtPairsParallel(vector <PtPair> *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])
|
||||
{
|
||||
// initialize centroids
|
||||
for(unsigned int i = 0; i < 3; ++i) {
|
||||
|
|
@ -849,26 +822,22 @@ void Scan::getPtPairsParallel(vector <PtPair> *pairs,
|
|||
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);
|
||||
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]);
|
||||
}
|
||||
} 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);
|
||||
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]);
|
||||
}
|
||||
|
||||
// normalize centroids
|
||||
|
|
|
|||
|
|
@ -18,23 +18,16 @@
|
|||
#include "slam6d/scan.h"
|
||||
#include "slam6d/globals.icc"
|
||||
|
||||
#include <stdexcept>
|
||||
|
||||
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 <PtPair> *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);
|
||||
|
||||
|
|
@ -70,7 +63,7 @@ void SearchTree::getPtPairs(vector <PtPair> *pairs,
|
|||
sum += Len2(p12);
|
||||
|
||||
pairs->push_back(myPair);
|
||||
/*cout << "PTPAIR" << i << " "
|
||||
/*cout << "PTPAIR" << i << " "
|
||||
<< p[0] << " "
|
||||
<< p[1] << " "
|
||||
<< p[2] << " - "
|
||||
|
|
@ -88,11 +81,11 @@ void SearchTree::getPtPairs(vector <PtPair> *pairs,
|
|||
}
|
||||
|
||||
void SearchTree::getPtPairs(vector <PtPair> *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)
|
||||
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)
|
||||
{
|
||||
// prepare this tree for resource access in FindClosest
|
||||
lock();
|
||||
|
|
@ -102,53 +95,20 @@ void SearchTree::getPtPairs(vector <PtPair> *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], normal[3];
|
||||
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] = 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);
|
||||
}
|
||||
|
||||
|
||||
double *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];
|
||||
|
|
@ -165,7 +125,7 @@ void SearchTree::getPtPairs(vector <PtPair> *pairs,
|
|||
sum += Len2(p12);
|
||||
|
||||
pairs->push_back(myPair);
|
||||
/*cout << "PTPAIR" << i << " "
|
||||
/*cout << "PTPAIR" << i << " "
|
||||
<< p[0] << " "
|
||||
<< p[1] << " "
|
||||
<< p[2] << " - "
|
||||
|
|
|
|||
|
|
@ -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,144 +128,145 @@ 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 <NR> '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 <NR> '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 <NR>" << endl
|
||||
<< endl
|
||||
<< bold << " -I" << normal << " NR, " << bold << "--iterSLAM=" << normal << "NR [default: 0]" << endl
|
||||
<< " sets the maximal number of iterations for SLAM to <NR>" << 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 <NR> 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=<NR>)" << 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=<NR>)" << endl
|
||||
<< endl
|
||||
<< bold << " -R" << normal << " NR, " << bold << "--random=" << normal << "NR" << endl
|
||||
<< " turns on randomized reduction, using about every <NR>-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"<<endl
|
||||
<< endl << 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 <NR> '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 <NR> '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 <NR>" << endl
|
||||
<< endl
|
||||
<< bold << " -I" << normal << " NR, " << bold << "--iterSLAM=" << normal << "NR [default: 0]" << endl
|
||||
<< " sets the maximal number of iterations for SLAM to <NR>" << 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 <NR> 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=<NR>)" << 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=<NR>)" << endl
|
||||
<< endl
|
||||
<< bold << " -R" << normal << " NR, " << bold << "--random=" << normal << "NR" << endl
|
||||
<< " turns on randomized reduction, using about every <NR>-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"<<endl
|
||||
<< endl << endl;
|
||||
|
||||
cout << bold << "EXAMPLES " << normal << endl
|
||||
<< " " << prog << " dat" << endl
|
||||
<< " " << prog << " --max=500 -r 10.2 -i 20 dat" << endl
|
||||
<< " " << prog << " -s 2 -e 10 dat" << endl << endl;
|
||||
<< " " << prog << " dat" << endl
|
||||
<< " " << prog << " --max=500 -r 10.2 -i 20 dat" << endl
|
||||
<< " " << prog << " -s 2 -e 10 dat" << endl << endl;
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
|
@ -299,13 +300,13 @@ void usage(char* prog)
|
|||
* @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)
|
||||
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)
|
||||
{
|
||||
int c;
|
||||
// from unistd.h:
|
||||
|
|
@ -346,8 +347,6 @@ int parseArgs(int argc, char **argv, string &dir, double &red, int &rand,
|
|||
{ "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
|
||||
|
|
@ -360,147 +359,141 @@ int parseArgs(int argc, char **argv, string &dir, double &red, int &rand,
|
|||
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: <end> cannot be smaller than <start>.\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 ();
|
||||
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: <end> cannot be smaller than <start>.\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 ();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -540,10 +533,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 <Scan *> 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);
|
||||
|
||||
|
|
@ -587,18 +580,18 @@ void matchGraph6Dautomatic(double cldist, int loopsize, vector <Scan *> 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 {
|
||||
|
|
@ -700,7 +693,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, 2006 - 2009" << endl << endl;
|
||||
|
||||
if (argc <= 1) {
|
||||
usage(argv[0]);
|
||||
|
|
@ -738,13 +731,12 @@ 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, pairing_mode);
|
||||
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);
|
||||
|
||||
cout << "slam6D will proceed with the following parameters:" << endl;
|
||||
//@@@ to do :-)
|
||||
|
|
@ -760,44 +752,39 @@ 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);
|
||||
unsigned int types;
|
||||
if ((pairing_mode == CLOSEST_POINT_ALONG_NORMAL) ||
|
||||
(pairing_mode == CLOSEST_PLANE)) {
|
||||
types = PointType::USE_NORMAL;
|
||||
}
|
||||
scan->setReductionParameter(red, octree, PointType(types));
|
||||
scan->setReductionParameter(red, octree);
|
||||
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
|
||||
|
|
@ -812,13 +799,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
|
||||
|
|
@ -827,7 +814,7 @@ int main(int argc, char **argv)
|
|||
my_icp->set_cad_matching (true);
|
||||
}
|
||||
|
||||
if (my_icp) my_icp->doICP(Scan::allScans, pairing_mode);
|
||||
if (my_icp) my_icp->doICP(Scan::allScans);
|
||||
delete my_icp;
|
||||
} else if (clpairs > -1) {
|
||||
//!!!!!!!!!!!!!!!!!!!!!!!!
|
||||
|
|
@ -835,38 +822,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, pairing_mode);
|
||||
my_icp->doICP(Scan::allScans);
|
||||
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") {
|
||||
|
|
@ -874,15 +861,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, pairing_mode);
|
||||
my_icp->doICP(Scan::allScans);
|
||||
|
||||
Graph* structure;
|
||||
structure = new Graph(net);
|
||||
|
|
@ -898,38 +885,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;
|
||||
|
|
@ -953,16 +940,11 @@ 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) {
|
||||
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 << xyz_r[i][0] << ' ' << xyz_r[i][1] << ' ' << xyz_r[i][2] << '\n';
|
||||
}
|
||||
redptsout << std::flush;
|
||||
|
||||
}
|
||||
redptsout.close();
|
||||
redptsout.clear();
|
||||
|
|
@ -973,8 +955,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();
|
||||
}
|
||||
|
|
@ -985,9 +967,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
|
||||
|
|
|
|||
Loading…
Reference in a new issue