528 lines
18 KiB
Text
528 lines
18 KiB
Text
/*
|
|
* scan_red implementation
|
|
*
|
|
* Copyright (C) Dorit Borrmann, Razvan-George Mihalyi, Remus Dumitru
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
|
|
/**
|
|
* @file
|
|
* @brief Main program for reducing 3D scans.
|
|
*
|
|
* Program to reduce scans for use with slam6d
|
|
* Usage: bin/scan_red -r <NR> 'dir',
|
|
* Use -r for octree based reduction (voxel size=<NR>)
|
|
* and 'dir' the directory of a set of scans
|
|
* Reduced scans will be written to 'dir/reduced'
|
|
*
|
|
* @author Dorit Borrmann. Automation Group, Jacobs University Bremen gGmbH, Germany.
|
|
*/
|
|
#ifdef _MSC_VER
|
|
#if !defined _OPENMP && defined OPENMP
|
|
#define _OPENMP
|
|
#endif
|
|
#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::ofstream;
|
|
#include <errno.h>
|
|
|
|
#include "slam6d/metaScan.h"
|
|
#include "slam6d/io_utils.h"
|
|
#include "slam6d/scan.h"
|
|
#include "slam6d/Boctree.h"
|
|
#include "slam6d/fbr/fbr_global.h"
|
|
#include "slam6d/fbr/panorama.h"
|
|
#include "slam6d/fbr/scan_cv.h"
|
|
|
|
#include "scanserver/clientInterface.h"
|
|
|
|
#include "slam6d/globals.icc"
|
|
|
|
#ifdef _OPENMP
|
|
#include <omp.h>
|
|
#endif
|
|
|
|
|
|
#ifndef _MSC_VER
|
|
#include <getopt.h>
|
|
#else
|
|
#include "XGetopt.h"
|
|
#endif
|
|
|
|
#ifdef _MSC_VER
|
|
#define strcasecmp _stricmp
|
|
#define strncasecmp _strnicmp
|
|
#include <windows.h>
|
|
#include <direct.h>
|
|
#else
|
|
#include <sys/stat.h>
|
|
#include <sys/types.h>
|
|
#include <strings.h>
|
|
#include <dlfcn.h>
|
|
#endif
|
|
|
|
using namespace fbr;
|
|
|
|
#include <boost/program_options.hpp>
|
|
namespace po = boost::program_options;
|
|
|
|
enum reduction_method {OCTREE, RANGE, INTERPOLATE};
|
|
|
|
/* Function used to check that 'opt1' and 'opt2' are not specified
|
|
at the same time. */
|
|
void conflicting_options(const po::variables_map & vm,
|
|
const char *opt1, const char *opt2)
|
|
{
|
|
if (vm.count(opt1) && !vm[opt1].defaulted()
|
|
&& vm.count(opt2) && !vm[opt2].defaulted())
|
|
throw std::logic_error(string("Conflicting options '")
|
|
+ opt1 + "' and '" + opt2 + "'.");
|
|
}
|
|
|
|
/* Function used to check that if 'for_what' is specified, then
|
|
'required_option' is specified too. */
|
|
void option_dependency(const po::variables_map & vm,
|
|
const char *for_what, const char *required_option)
|
|
{
|
|
if (vm.count(for_what) && !vm[for_what].defaulted())
|
|
if (vm.count(required_option) == 0
|
|
|| vm[required_option].defaulted())
|
|
throw std::logic_error(string("Option '") + for_what +
|
|
"' requires option '" +
|
|
required_option + "'.");
|
|
}
|
|
|
|
/*
|
|
* validates panorama method specification
|
|
*/
|
|
namespace fbr {
|
|
void validate(boost::any& v, const std::vector<std::string>& values,
|
|
projection_method*, int) {
|
|
if (values.size() == 0)
|
|
throw std::runtime_error("Invalid model specification");
|
|
string arg = values.at(0);
|
|
if(strcasecmp(arg.c_str(), "EQUIRECTANGULAR") == 0) v = EQUIRECTANGULAR;
|
|
else if(strcasecmp(arg.c_str(), "CYLINDRICAL") == 0) v = CYLINDRICAL;
|
|
else if(strcasecmp(arg.c_str(), "MERCATOR") == 0) v = MERCATOR;
|
|
else if(strcasecmp(arg.c_str(), "CONIC") == 0) v = CONIC;
|
|
else throw std::runtime_error(std::string("projection method ") + arg + std::string(" is unknown"));
|
|
}
|
|
}
|
|
|
|
/*
|
|
* validates input type specification
|
|
*/
|
|
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.");
|
|
}
|
|
}
|
|
|
|
void reduction_option_dependency(const po::variables_map & vm, reduction_method stype, const char *option)
|
|
{
|
|
if (vm.count("reduction") && vm["reduction"].as<reduction_method>() == stype) {
|
|
if (!vm.count(option)) {
|
|
throw std::logic_error (string("this reduction option needs ")+option+" to be set");
|
|
}
|
|
}
|
|
}
|
|
|
|
void reduction_option_conflict(const po::variables_map & vm, reduction_method stype, const char *option)
|
|
{
|
|
if (vm.count("reduction") && vm["reduction"].as<reduction_method>() == stype) {
|
|
if (vm.count(option)) {
|
|
throw std::logic_error (string("this reduction option is incompatible with ")+option);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
* validates reduction method specification
|
|
*/
|
|
void validate(boost::any& v, const std::vector<std::string>& values,
|
|
reduction_method*, int) {
|
|
if (values.size() == 0)
|
|
throw std::runtime_error("Invalid model specification");
|
|
string arg = values.at(0);
|
|
if(strcasecmp(arg.c_str(), "OCTREE") == 0) v = OCTREE;
|
|
else if(strcasecmp(arg.c_str(), "RANGE") == 0) v = RANGE;
|
|
else if(strcasecmp(arg.c_str(), "INTERPOLATE") == 0) v = INTERPOLATE;
|
|
else throw std::runtime_error(std::string("reduction method ") + arg + std::string(" is unknown"));
|
|
}
|
|
|
|
void parse_options(int argc, char **argv, int &start, int &end,
|
|
bool &scanserver, int &width, int &height,
|
|
fbr::projection_method &ptype, string &dir, IOType &iotype,
|
|
int &maxDist, int &minDist, reduction_method &rtype, double &scale,
|
|
double &voxel, int &octree, bool &use_reflectance)
|
|
{
|
|
po::options_description generic("Generic options");
|
|
generic.add_options()
|
|
("help,h", "output this help message");
|
|
|
|
po::options_description input("Input options");
|
|
input.add_options()
|
|
("start,s", po::value<int>(&start)->default_value(0),
|
|
"start at scan <arg> (i.e., neglects the first <arg> scans) "
|
|
"[ATTENTION: counting naturally starts with 0]")
|
|
("end,e", po::value<int>(&end)->default_value(-1),
|
|
"end after scan <arg>")
|
|
("format,f", po::value<IOType>(&iotype)->default_value(UOS),
|
|
"using shared library <arg> for input. (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})")
|
|
("max,M", po::value<int>(&maxDist)->default_value(-1),
|
|
"neglegt all data points with a distance larger than <arg> 'units")
|
|
("min,m", po::value<int>(&minDist)->default_value(-1),
|
|
"neglegt all data points with a distance smaller than <arg> 'units")
|
|
("scanserver,S", po::bool_switch(&scanserver),
|
|
"Use the scanserver as an input method and handling of scan data");
|
|
|
|
po::options_description reduction("Reduction options");
|
|
reduction.add_options()
|
|
("reduction,r", po::value<reduction_method>(&rtype)->required(),
|
|
"choose reduction method (OCTREE, RANGE, INTERPOLATE)")
|
|
("scale,S", po::value<double>(&scale),
|
|
"scaling factor")
|
|
("voxel,v", po::value<double>(&voxel),
|
|
"voxel size")
|
|
("projection,P", po::value<fbr::projection_method>(&ptype),
|
|
"projection method or panorama image")
|
|
("octree,O", po::value<int>(&octree),
|
|
"0 -> center\n1 -> random\nN>1 -> random N")
|
|
("width,w", po::value<int>(&width),
|
|
"width of panorama")
|
|
("height,h", po::value<int>(&height),
|
|
"height of panorama");
|
|
|
|
po::options_description output("Output options");
|
|
output.add_options()
|
|
("reflectance,R", po::bool_switch(&use_reflectance),
|
|
"Use reflectance when reducing points and save scan files in UOSR format");
|
|
|
|
po::options_description hidden("Hidden options");
|
|
hidden.add_options()
|
|
("input-dir", po::value<string>(&dir), "input dir");
|
|
|
|
// all options
|
|
po::options_description all;
|
|
all.add(generic).add(input).add(reduction).add(output).add(hidden);
|
|
|
|
// options visible with --help
|
|
po::options_description cmdline_options;
|
|
cmdline_options.add(generic).add(input).add(reduction).add(output);
|
|
|
|
// positional argument
|
|
po::positional_options_description pd;
|
|
pd.add("input-dir", 1);
|
|
|
|
// process options
|
|
po::variables_map vm;
|
|
po::store(po::command_line_parser(argc, argv).
|
|
options(all).positional(pd).run(), vm);
|
|
|
|
// display help
|
|
if (vm.count("help")) {
|
|
cout << cmdline_options;
|
|
cout << endl
|
|
<< "Example usage:" << endl
|
|
<< "\t./bin/scan_red -s 0 -e 0 -f uos --reduction OCTREE --voxel 10 --octree 0 dat" << endl
|
|
<< "\t./bin/scan_red -s 0 -e 0 -f uos --reduction RANGE --scale 0.5 --projection EQUIRECTANGULAR --width 3600 --height 1000 dat" << endl
|
|
<< "\t./bin/scan_red -s 0 -e 0 -f uos --reduction INTERPOLATE --scale 0.2 --projection EQUIRECTANGULAR --width 3600 --height 1000 dat" << endl;
|
|
exit(0);
|
|
}
|
|
|
|
po::notify(vm);
|
|
|
|
reduction_option_dependency(vm, OCTREE, "voxel");
|
|
reduction_option_dependency(vm, OCTREE, "octree");
|
|
reduction_option_conflict(vm, OCTREE, "scale");
|
|
reduction_option_conflict(vm, OCTREE, "projection");
|
|
reduction_option_conflict(vm, OCTREE, "width");
|
|
reduction_option_conflict(vm, OCTREE, "height");
|
|
|
|
reduction_option_conflict(vm, RANGE, "voxel");
|
|
reduction_option_conflict(vm, RANGE, "octree");
|
|
reduction_option_dependency(vm, RANGE, "scale");
|
|
reduction_option_dependency(vm, RANGE, "projection");
|
|
reduction_option_dependency(vm, RANGE, "width");
|
|
reduction_option_dependency(vm, RANGE, "height");
|
|
|
|
reduction_option_conflict(vm, INTERPOLATE, "voxel");
|
|
reduction_option_conflict(vm, INTERPOLATE, "octree");
|
|
reduction_option_dependency(vm, INTERPOLATE, "scale");
|
|
reduction_option_dependency(vm, INTERPOLATE, "projection");
|
|
reduction_option_dependency(vm, INTERPOLATE, "width");
|
|
reduction_option_dependency(vm, INTERPOLATE, "height");
|
|
|
|
#ifndef _MSC_VER
|
|
if (dir[dir.length()-1] != '/') dir = dir + "/";
|
|
#else
|
|
if (dir[dir.length()-1] != '\\') dir = dir + "\\";
|
|
#endif
|
|
}
|
|
|
|
void createdirectory(string dir)
|
|
{
|
|
int success = mkdir(dir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
|
|
|
|
if (success == 0 || errno == EEXIST) {
|
|
cout << "Writing to " << dir << endl;
|
|
} else {
|
|
cerr << "Creating directory " << dir << " failed" << endl;
|
|
exit(1);
|
|
}
|
|
}
|
|
|
|
void scan2mat(Scan *source, cv::Mat &mat)
|
|
{
|
|
DataXYZ xyz = source->get("xyz");
|
|
DataReflectance xyz_reflectance = (((DataReflectance)source->get("reflectance")).size() == 0) ?
|
|
source->create("reflectance", sizeof(float)*xyz.size())
|
|
: source->get("reflectance");
|
|
if(((DataReflectance)source->get("reflectance")).size() == 0){
|
|
for(unsigned int i = 0; i < xyz.size(); i++)
|
|
xyz_reflectance[i] = 255;
|
|
}
|
|
unsigned int nPoints = xyz.size();
|
|
mat.create(nPoints,1,CV_32FC(4));
|
|
mat = cv::Scalar::all(0);
|
|
cv::MatIterator_<cv::Vec4f> it = mat.begin<cv::Vec4f>();
|
|
for(unsigned int i = 0; i < nPoints; i++){
|
|
float 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] = xyz[i][0];
|
|
(*it)[1] = xyz[i][1];
|
|
(*it)[2] = xyz[i][2];
|
|
(*it)[3] = reflectance;
|
|
++it;
|
|
}
|
|
}
|
|
|
|
void reduce_octree(Scan *scan, vector<cv::Vec4f> &reduced_points, int octree,
|
|
int red, bool use_reflectance)
|
|
{
|
|
if (use_reflectance) {
|
|
unsigned int types = PointType::USE_REFLECTANCE;
|
|
PointType pointtype(types);
|
|
scan->setReductionParameter(red, octree, pointtype);
|
|
scan->calcReducedPoints();
|
|
|
|
DataXYZ xyz_reduced(scan->get("xyz reduced"));
|
|
DataReflectance reflectance_reduced(scan->get("reflectance reduced"));
|
|
|
|
if (xyz_reduced.size() != reflectance_reduced.size()) {
|
|
cerr << "xyz_reduced size different than reflectance_reduced size" << endl;
|
|
return;
|
|
}
|
|
|
|
for(unsigned int j = 0; j < xyz_reduced.size(); j++) {
|
|
reduced_points.push_back(cv::Vec4f(xyz_reduced[j][0], xyz_reduced[j][1], xyz_reduced[j][2], reflectance_reduced[j]));
|
|
}
|
|
}
|
|
else {
|
|
scan->setReductionParameter(red, octree);
|
|
scan->calcReducedPoints();
|
|
|
|
DataXYZ xyz_reduced(scan->get("xyz reduced"));
|
|
for(unsigned int j = 0; j < xyz_reduced.size(); j++) {
|
|
reduced_points.push_back(cv::Vec4f(xyz_reduced[j][0], xyz_reduced[j][1], xyz_reduced[j][2], 0.0));
|
|
}
|
|
}
|
|
}
|
|
|
|
void reduce_range(Scan *scan, vector<cv::Vec4f> &reduced_points, int width,
|
|
int height, fbr::projection_method ptype, double scale,
|
|
bool use_reflectance)
|
|
{
|
|
panorama image(width, height, ptype);
|
|
cv::Mat mat;
|
|
scan2mat(scan, mat);
|
|
image.createPanorama(mat);
|
|
image.getDescription();
|
|
|
|
cv::Mat range_image_resized;
|
|
cv::Mat reflectance_image_resized;
|
|
resize(image.getRangeImage(), range_image_resized, cv::Size(),
|
|
scale, scale, cv::INTER_NEAREST);
|
|
if (use_reflectance) {
|
|
resize(image.getReflectanceImage(), reflectance_image_resized,
|
|
cv::Size(), scale, scale, cv::INTER_NEAREST);
|
|
} else {
|
|
reflectance_image_resized.create(range_image_resized.size(), CV_8U);
|
|
reflectance_image_resized = cv::Scalar::all(0);
|
|
}
|
|
image.recoverPointCloud(range_image_resized, reflectance_image_resized, reduced_points);
|
|
}
|
|
|
|
void reduce_interpolation(Scan *scan, vector<cv::Vec4f> &reduced_points,
|
|
int width, int height, fbr::projection_method ptype, double scale,
|
|
bool use_reflectance)
|
|
{
|
|
panorama image(width, height, ptype);
|
|
cv::Mat mat;
|
|
scan2mat(scan, mat);
|
|
image.createPanorama(mat);
|
|
image.getDescription();
|
|
|
|
cv::Mat range_image_resized;
|
|
cv::Mat reflectance_image_resized;
|
|
resize(image.getMap(), range_image_resized, cv::Size(),
|
|
scale, scale, cv::INTER_NEAREST);
|
|
if (use_reflectance) {
|
|
resize(image.getReflectanceImage(), reflectance_image_resized,
|
|
cv::Size(), scale, scale, cv::INTER_NEAREST);
|
|
}
|
|
for(int i = 0; i < range_image_resized.rows; i++) {
|
|
for(int j = 0; j < range_image_resized.cols; j++) {
|
|
cv::Vec3f vec = range_image_resized.at<cv::Vec3f>(i, j);
|
|
if (use_reflectance) {
|
|
reduced_points.push_back(cv::Vec4f(
|
|
vec[0], vec[1], vec[2],
|
|
reflectance_image_resized.at<uchar>(i, j)/255.0));
|
|
} else {
|
|
reduced_points.push_back(cv::Vec4f(vec[0], vec[1], vec[2], 0.0));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
* given a vector of 3d points, write them out as uos files
|
|
*/
|
|
void write_uos(vector<cv::Vec4f> &points, string &dir, string id)
|
|
{
|
|
ofstream outfile(dir + "/scan" + id + ".3d");
|
|
|
|
outfile << "# header is ignored" << endl;
|
|
|
|
for (vector<cv::Vec4f>::iterator it=points.begin(); it < points.end(); it++) {
|
|
outfile << (*it)[0] << " " << (*it)[1] << " " << (*it)[2] << endl;
|
|
}
|
|
|
|
outfile.close();
|
|
}
|
|
|
|
/*
|
|
* given a vector of 3d points, write them out as uosr files
|
|
*/
|
|
void write_uosr(vector<cv::Vec4f> &points, string &dir, string id)
|
|
{
|
|
ofstream outfile(dir + "/scan" + id + ".3d");
|
|
|
|
outfile << "# header is ignored" << endl;
|
|
|
|
for (vector<cv::Vec4f>::iterator it=points.begin(); it < points.end(); it++) {
|
|
outfile << (*it)[0] << " " << (*it)[1] << " " << (*it)[2] << " " << (*it)[3] << endl;
|
|
}
|
|
|
|
outfile.close();
|
|
}
|
|
|
|
// write .pose files
|
|
// .frames files can later be generated from them using ./bin/pose2frames
|
|
void writeposefile(string &dir, const double* rPos, const double* rPosTheta, string id)
|
|
{
|
|
ofstream posefile(dir + "/scan" + id + ".pose");
|
|
posefile << rPos[0] << " " << rPos[1] << " " << rPos[2] << endl;
|
|
posefile << deg(rPosTheta[0]) << " "
|
|
<< deg(rPosTheta[1]) << " "
|
|
<< deg(rPosTheta[2]) << endl;
|
|
posefile.close();
|
|
}
|
|
|
|
/**
|
|
* Main program for reducing scans.
|
|
* Usage: bin/scan_red -r <NR> 'dir',
|
|
* Use -r for octree based reduction (voxel size=<NR>)
|
|
* and 'dir' the directory of a set of scans
|
|
* Reduced scans will be written to 'dir/reduced'
|
|
*
|
|
*/
|
|
int main(int argc, char **argv)
|
|
{
|
|
int start, end;
|
|
bool scanserver;
|
|
int width, height;
|
|
int maxDist, minDist;
|
|
fbr::projection_method ptype;
|
|
string dir;
|
|
IOType iotype;
|
|
reduction_method rtype;
|
|
double scale, voxel;
|
|
int octree;
|
|
bool use_reflectance;
|
|
|
|
parse_options(argc, argv, start, end, scanserver, width, height, ptype,
|
|
dir, iotype, maxDist, minDist, rtype, scale, voxel, octree,
|
|
use_reflectance);
|
|
|
|
for (int iter = start; iter <= end; iter++) {
|
|
|
|
Scan::openDirectory(scanserver, dir, iotype, iter, iter);
|
|
|
|
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);
|
|
|
|
vector<cv::Vec4f> reduced_points;
|
|
|
|
string reddir = dir + "reduced";
|
|
createdirectory(reddir);
|
|
|
|
switch (rtype) {
|
|
case OCTREE:
|
|
reduce_octree(scan, reduced_points, octree, voxel, use_reflectance);
|
|
break;
|
|
case RANGE:
|
|
reduce_range(scan, reduced_points, width, height, ptype, scale, use_reflectance);
|
|
break;
|
|
case INTERPOLATE:
|
|
reduce_interpolation(scan, reduced_points, width, height, ptype, scale, use_reflectance);
|
|
break;
|
|
default:
|
|
cerr << "unknown method" << endl;
|
|
return 1;
|
|
break;
|
|
}
|
|
|
|
if (use_reflectance)
|
|
write_uosr(reduced_points, reddir, scan->getIdentifier());
|
|
else
|
|
write_uos(reduced_points, reddir, scan->getIdentifier());
|
|
writeposefile(reddir, scan->get_rPos(), scan->get_rPosTheta(), scan->getIdentifier());
|
|
}
|
|
Scan::closeDirectory();
|
|
}
|
|
}
|