demote master to svn r761
Razvan Mihalyi
12 years ago
parent
1268ff8dc2
commit
12b8fe558d
@ -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;
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@ -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
|
||||
@ -1,10 +0,0 @@
|
||||
#ifndef __PAIRINGMODE_H__
|
||||
#define __PAIRINGMODE_H__
|
||||
|
||||
enum PairingMode {
|
||||
CLOSEST_POINT,
|
||||
CLOSEST_POINT_ALONG_NORMAL,
|
||||
CLOSEST_PLANE
|
||||
};
|
||||
|
||||
#endif // PAIRINGMODE_H
|
||||
@ -1,9 +1,7 @@
|
||||
IF(WITH_NORMALS)
|
||||
FIND_PACKAGE(OpenCV REQUIRED)
|
||||
add_executable(normals normals.cc)
|
||||
|
||||
add_library(normals normals.cc)
|
||||
target_link_libraries(normals newmat scan ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS})
|
||||
FIND_PACKAGE(OpenCV REQUIRED)
|
||||
|
||||
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;
|
||||
}
|
||||
Loading…
Reference in New Issue