Update to svn revision 712

.svn/pristine/26/2638410539b1ae55fb27e0ce177aeb5f4ff37aec.svn-base

@@ -0,0 +1,109 @@
+/*
+ * scan_cv implementation
+ *
+ * Copyright (C) HamidReza Houshiar
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+#include "slam6d/fbr/scan_cv.h"
+
+using namespace std;
+
+namespace fbr{
+
+  scan_cv::scan_cv(string dir, unsigned int number, IOType format){
+    sDir = dir;
+    sNumber = number;
+    sFormat = format;
+    zMax = numeric_limits<double>::min(); 
+    zMin = numeric_limits<double>::max();
+    nPoints = 0;
+  } 
+  
+  void scan_cv::convertScanToMat(){
+    bool scanserver = false;
+    Scan::openDirectory(scanserver, sDir, sFormat, sNumber, sNumber);
+    if(Scan::allScans.size() == 0){
+      cerr << "No scans found. Did you use the correct format?" <<endl;
+      exit(-1);
+    }
+    cout<<"loading "<<sDir<<" with scan number " <<sNumber<<"."<<endl;
+    Scan * source = * Scan::allScans.begin();
+    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;
+    }
+    nPoints = xyz.size();
+    scan.create(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];
+      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;
+      (*it)[3] = reflectance;
+      
+      //finding min and max of z                                      
+      if (z  > zMax) zMax = z;
+      if (z  < zMin) zMin = z;
+      
+      ++it;
+    }
+    Scan::closeDirectory();
+  }
+
+  string scan_cv::getScanDir(){
+    return sDir;
+  }
+  
+  unsigned int scan_cv::getScanNumber(){
+    return sNumber;
+  }
+  
+  unsigned int scan_cv::getNumberOfPoints(){
+    return nPoints;
+  }
+
+  double scan_cv::getZMin(){
+    return zMin;
+  }
+
+  double scan_cv::getZMax(){
+    return zMax;
+  }
+
+  IOType scan_cv::getScanFormat(){
+    return sFormat;
+  }
+  
+  cv::Mat scan_cv::getMatScan()
+  {
+    return scan;
+  }
+  
+  void scan_cv::getDescription(){
+    cout<<"load "<<sDir<<", with scan number: " <<sNumber<<", with "<<nPoints<<" points, sFormat: "<<scanFormatToString(sFormat)<<"."<<endl;
+    cout<<endl;
+  }
+}

.svn/pristine/28/2807818bc2c0156cfb5fc7e14ead3224e265628f.svn-base

@@ -0,0 +1,68 @@
+#ifndef BASIC_SCAN_H
+#define BASIC_SCAN_H
+
+#include "scan.h"
+#include "frame.h"
+
+#include <vector>
+#include <map>
+
+
+class BasicScan : public Scan {
+public:
+  static void openDirectory(const std::string& path, IOType type, int start, int end);
+  static void closeDirectory();
+/*
+  Scan(const double *euler, int maxDist = -1);
+  Scan(const double rPos[3], const double rPosTheta[3], int maxDist = -1);
+  Scan(const double _rPos[3], const double _rPosTheta[3], vector<double *> &pts);
+*/
+  virtual ~BasicScan();
+
+  virtual void setRangeFilter(double max, double min);
+  virtual void setHeightFilter(double top, double bottom);
+  virtual void setRangeMutation(double range);
+
+  virtual const char* getIdentifier() const { return m_identifier.c_str(); }
+
+  virtual DataPointer get(const std::string& identifier);
+  virtual void get(unsigned int types);
+  virtual DataPointer create(const std::string& identifier, unsigned int size);
+  virtual void clear(const std::string& identifier);
+
+  virtual unsigned int readFrames();
+  virtual void saveFrames();
+  virtual unsigned int getFrameCount();
+  virtual void getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type);
+
+protected:
+  virtual void createSearchTreePrivate();
+  virtual void calcReducedOnDemandPrivate();
+  virtual void addFrame(AlgoType type);
+
+private:
+  //! Path and identifier of where the scan is located
+  std::string m_path, m_identifier;
+
+  IOType m_type;
+
+  double m_filter_max, m_filter_min, m_filter_top, m_filter_bottom, m_range_mutation;
+  bool m_filter_range_set, m_filter_height_set, m_range_mutation_set;
+
+  std::map<std::string, std::pair<unsigned char*, unsigned int>> m_data;
+
+  std::vector<Frame> m_frames;
+
+
+  //! Constructor for openDirectory
+  BasicScan(const std::string& path, const std::string& identifier, IOType type);
+
+  //! Initialization function
+  void init();
+
+  void createANNTree();
+
+  void createOcttree();
+};
+
+#endif //BASIC_SCAN_H

.svn/pristine/28/2878eac1a186f2e71956fd631c8f6434a75ca8da.svn-base

@@ -0,0 +1,101 @@
+/**
+ * @file fbr_global.h
+ * @brief Globally used headers, functions, structures
+ * @author HamidReza Houshiar. Jacobs University Bremen gGmbH, Germany.
+ * @date 2012/05/9 14:00
+ */
+
+#ifndef FBR_GLOBAL_H_
+#define FBR_GLOBAL_H_
+
+#include <iostream>
+#include <vector>
+#include <fstream>
+#include <opencv2/opencv.hpp>
+#include <math.h>
+#include <string>
+#include "slam6d/io_types.h"
+#include "slam6d/globals.icc"
+
+using namespace std;
+
+namespace fbr{
+  
+  //Vertical angle of view of scanner
+#define MAX_ANGLE 60.0
+#define MIN_ANGLE -40.0
+  
+  /**
+   * @enum projection_method
+   */
+  enum projection_method{
+    EQUIRECTANGULAR,
+    CYLINDRICAL,
+    MERCATOR,
+    RECTILINEAR,
+    PANNINI,
+    STEREOGRAPHIC,
+    ZAXIS,
+    CONIC,
+  };
+  /**
+   * @enum panorama_map_method
+   */
+  enum panorama_map_method{
+    FARTHEST,
+    EXTENDED,
+  };
+  /**
+   * @enum feature_method
+   */
+  enum feature_detector_method{
+    SIFT_DET,
+    SURF_DET,
+    ORB_DET,
+    FAST_DET,
+    STAR_DET,
+  };
+  enum feature_descriptor_method{
+    SIFT_DES,
+    SURF_DES,
+    ORB_DES,
+  };
+  /**
+   * @enum matching_method
+   */
+  enum matcher_method{
+    BRUTEFORCE,
+    FLANN,
+    KNN,
+    RADIUS,
+    RATIO,
+  };
+  /**
+   * @enum registration_method
+   */
+  enum registration_method{
+    ALL,
+    RANSAC,
+    DISABLE,
+  };
+  //RANSAC iteration       
+#define RANSACITR 20000
+  //Inlier influence                                                     
+#define iInfluence 0.5
+
+  string scanFormatToString(IOType format);
+  IOType stringToScanFormat(string format);
+  string projectionMethodToString(projection_method method);
+  projection_method stringToProjectionMethod(string method);
+  string panoramaMapMethodToString(panorama_map_method method);
+  panorama_map_method stringToPanoramaMapMethod(string method);
+  string featureDetectorMethodToString(feature_detector_method method);
+  feature_detector_method stringToFeatureDetectorMethod(string method);
+  string featureDescriptorMethodToString(feature_descriptor_method method);
+  feature_descriptor_method stringToFeatureDescriptorMethod(string method);
+  string matcherMethodToString(matcher_method method);
+  matcher_method stringToMatcherMethod(string method);
+  string registrationMethodToString(registration_method method);
+  registration_method stringToRegistrationMethod(string method);
+}
+#endif /* FBR_GLOBAL_H_ */

.svn/pristine/45/45a8678ada1c9c0d4a7d5ec515a6aa4d87f50ac2.svn-base

@@ -0,0 +1,373 @@
+/*
+ * managedScan implementation
+ *
+ * Copyright (C) Kai Lingemann, Thomas Escher
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+#include "slam6d/managedScan.h"
+
+#include "scanserver/clientInterface.h"
+#include "slam6d/Boctree.h"
+#include "slam6d/kdManaged.h"
+
+#ifdef WITH_METRICS
+#include "slam6d/metrics.h"
+#endif //WITH_METRICS
+
+#include <sstream>
+using std::stringstream;
+
+#include <boost/filesystem/operations.hpp>
+using namespace boost::filesystem;
+
+
+
+SharedScanVector* ManagedScan::shared_scans = 0;
+
+
+
+void ManagedScan::openDirectory(const std::string& path, IOType type, int start, int end)
+{
+  // start the client first
+  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);
+  }
+
+#ifdef WITH_METRICS
+  Timer t = ClientMetric::read_scan_time.start();
+#endif //WITH_METRICS
+
+  ClientInterface* client = ClientInterface::getInstance();
+  shared_scans = client->readDirectory(path.c_str(), type, start, end);
+
+  for(SharedScanVector::iterator it = shared_scans->begin(); it != shared_scans->end(); ++it) {
+    // add a scan with reference on the shared scan
+    SharedScan* shared = it->get();
+    ManagedScan* scan = new ManagedScan(shared);
+    Scan::allScans.push_back(scan);
+  }
+
+#ifdef WITH_METRICS
+  ClientMetric::read_scan_time.end(t);
+#endif //WITH_METRICS
+}
+
+void ManagedScan::closeDirectory()
+{
+  // clean up the scan vector
+  for(std::vector<Scan*>::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it)
+    delete *it;
+  allScans.clear();
+  // remove the shared scan vector
+  ClientInterface* client = ClientInterface::getInstance();
+#ifdef WITH_METRICS
+  ClientInterface::getInstance()->printMetrics();
+#endif //WITH_METRICS
+  client->closeDirectory(shared_scans);
+}
+
+std::size_t ManagedScan::getMemorySize()
+{
+  ClientInterface* client = ClientInterface::getInstance();
+  return client->getCacheSize();
+}
+
+
+
+ManagedScan::ManagedScan(SharedScan* shared_scan) :
+  m_shared_scan(shared_scan),
+  m_reduced_ready(false),
+  m_reset_frames_on_write(true)
+{
+  // request pose from the shared scan
+  double* euler = m_shared_scan->getPose();
+  rPos[0] = euler[0];
+  rPos[1] = euler[1];
+  rPos[2] = euler[2];
+  rPosTheta[0] = euler[3];
+  rPosTheta[1] = euler[4];
+  rPosTheta[2] = euler[5];
+
+  // write original pose matrix
+  EulerToMatrix4(euler, &euler[3], transMatOrg);
+
+  // initialize transform matrices from the original one, could just copy transMatOrg to transMat instead
+  transformMatrix(transMatOrg);
+
+  // reset the delta align matrix to represent only the transformations after local-to-global (transMatOrg) one
+  M4identity(dalignxf);
+}
+
+ManagedScan::~ManagedScan()
+{
+  // TODO: something to do?
+}
+
+void ManagedScan::setRangeFilter(double max, double min)
+{
+  m_shared_scan->setRangeParameters(max, min);
+}
+
+void ManagedScan::setHeightFilter(double top, double bottom)
+{
+  m_shared_scan->setHeightParameters(top, bottom);
+}
+
+void ManagedScan::setRangeMutation(double range)
+{
+  m_shared_scan->setRangeMutationParameters(range);
+}
+
+
+void ManagedScan::setReductionParameter(double voxelSize, int nrpts, PointType pointtype)
+{
+  Scan::setReductionParameter(voxelSize, nrpts, pointtype);
+
+  // set parameters to invalidate old cache data
+  stringstream s;
+  s << voxelSize << " " << nrpts << " " << transMatOrg;
+  m_shared_scan->setReductionParameters(s.str().c_str());
+}
+
+void ManagedScan::setShowReductionParameter(double voxelSize, int nrpts, PointType pointtype)
+{
+  show_reduction_voxelSize = voxelSize;
+  show_reduction_nrpts = nrpts;
+  show_reduction_pointtype = pointtype;
+
+  // set parameters to invalidate old cache data
+  stringstream s;
+  s << voxelSize << " " << nrpts;
+  m_shared_scan->setShowReductionParameters(s.str().c_str());
+}
+
+void ManagedScan::setOcttreeParameter(double reduction_voxelSize, double octtree_voxelSize, PointType pointtype, bool loadOct, bool saveOct)
+{
+  Scan::setOcttreeParameter(reduction_voxelSize, octtree_voxelSize, pointtype, loadOct, saveOct);
+
+  // set octtree parameters to invalidate cached ones with other parameters (changing range/height is already handled)
+  stringstream s;
+  s << reduction_voxelSize << " " << octtree_voxelSize << " " << pointtype.toFlags();
+  m_shared_scan->setOcttreeParameters(s.str().c_str());
+}
+
+DataPointer ManagedScan::get(const std::string& identifier)
+{
+  if(identifier == "xyz") {
+    return m_shared_scan->getXYZ();
+  } else
+  if(identifier == "rgb") {
+    return m_shared_scan->getRGB();
+  } else
+  if(identifier == "reflectance") {
+    return m_shared_scan->getReflectance();
+  } else
+  if(identifier == "amplitude") {
+    return m_shared_scan->getAmplitude();
+  } else
+  if(identifier == "type") {
+    return m_shared_scan->getType();
+  } else
+  if(identifier == "deviation") {
+    return m_shared_scan->getDeviation();
+  } else
+  if(identifier == "xyz reduced") {
+    // if this is a fresh run, initialize reduced properly via original or creating it anew
+    if(!m_reduced_ready) {
+      calcReducedOnDemand();
+    }
+    return m_shared_scan->getXYZReduced();
+  } else
+  if(identifier == "xyz reduced original") {
+    // if reduction has completed, original will exist (either from last run or created in this run)
+    if(!m_reduced_ready) {
+      calcReducedOnDemand();
+    }
+    return m_shared_scan->getXYZReducedOriginal();
+  } else
+  if(identifier == "xyz reduced show") {
+    if(m_shared_scan->getXYZReducedShow().valid())
+      return m_shared_scan->getXYZReducedShow();
+    calcReducedShow();
+    return m_shared_scan->getXYZReducedShow();
+  } else
+  if(identifier == "octtree") {
+    if(m_shared_scan->getOcttree().valid())
+      return m_shared_scan->getOcttree();
+    createOcttree();
+    return m_shared_scan->getOcttree();
+  }
+  {
+    throw runtime_error(string("Identifier '") + identifier + "' not compatible with ManagedScan::get. Upgrade SharedScan for this data field.");
+  }
+}
+
+void ManagedScan::get(unsigned int types)
+{
+  m_shared_scan->prefetch(types);
+}
+
+DataPointer ManagedScan::create(const std::string& identifier, unsigned int size)
+{
+  // map identifiers to functions in SharedScan and scale back size from bytes to number of points
+  if(identifier == "xyz reduced") {
+    return m_shared_scan->createXYZReduced(size / (3*sizeof(double)));
+  } else
+  if(identifier == "xyz reduced original") {
+    return m_shared_scan->createXYZReducedOriginal(size / (3*sizeof(double)));
+  } else
+  {
+    throw runtime_error(string("Identifier '") + identifier + "' not compatible with ManagedScan::create. Upgrade SharedScan for this data field.");
+  }
+}
+
+void ManagedScan::clear(const std::string& identifier)
+{
+  // nothing to do here
+  // TODO: mark CacheObjects with a low priority for faster removal by the manager
+}
+
+void ManagedScan::createSearchTreePrivate()
+{
+  switch(searchtree_nnstype)
+  {
+    case simpleKD:
+      kd = new KDtreeManaged(this);
+      break;
+    case BOCTree:
+      kd = new BOctTree<double>(PointerArray<double>(get("xyz reduced original")).get(), size<DataXYZ>("xyz reduced original"), 10.0, PointType(), true);
+      break;
+    case -1:
+      throw runtime_error("Cannot create a SearchTree without setting a type.");
+    default:
+      throw runtime_error("SearchTree type not implemented for ManagedScan");
+  }
+
+  // TODO: look into CUDA compability
+}
+
+void ManagedScan::calcReducedOnDemandPrivate()
+{
+  // either copy from original or create them like BasicScan
+  DataXYZ xyz_orig(m_shared_scan->getXYZReducedOriginal());
+  if(xyz_orig.valid()) {
+    // set true to inform further get("xyz reduced original") calls to get the data instead of looping calcReducedOnDemand
+    m_reduced_ready = true;
+    copyOriginalToReduced();
+  } else {
+    // create reduced points and transform to initial position, save a copy of this for SearchTree
+    calcReducedPoints();
+    // set true to inform further get("xyz reduced") calls to get the data instead of looping calcReducedOnDemand
+    m_reduced_ready = true;
+    transformReduced(transMatOrg);
+    copyReducedToOriginal();
+  }
+}
+
+void ManagedScan::calcReducedShow()
+{
+  // create an octtree reduction from full points
+  DataXYZ xyz(get("xyz"));
+  BOctTree<double>* oct = new BOctTree<double>(PointerArray<double>(xyz).get(), xyz.size(), show_reduction_voxelSize);
+
+  vector<double*> center;
+  center.clear();
+
+  if(show_reduction_nrpts > 0) {
+    if(show_reduction_nrpts == 1) {
+      oct->GetOctTreeRandom(center);
+    } else {
+      oct->GetOctTreeRandom(center, show_reduction_nrpts);
+    }
+  } else {
+    oct->GetOctTreeCenter(center);
+  }
+
+  unsigned int size = center.size();
+  TripleArray<float> xyz_r(m_shared_scan->createXYZReducedShow(size));
+  for(unsigned int i = 0; i < size; ++i) {
+    for(unsigned int j = 0; j < 3; ++j) {
+      xyz_r[i][j] = center[i][j];
+    }
+  }
+
+  delete oct;
+}
+
+void ManagedScan::createOcttree()
+{
+  string scanFileName = string(m_shared_scan->getDirPath()) + "scan" + getIdentifier() + ".oct";
+  BOctTree<float>* btree = 0;
+
+  // if loadOct is given, load the octtree under blind assumption that parameters match
+  if(octtree_loadOct && exists(scanFileName)) {
+    btree = new BOctTree<float>(scanFileName);
+  } else {
+    if(octtree_reduction_voxelSize > 0) { // with reduction, only xyz points
+      TripleArray<float> xyz_r(get("xyz reduced show"));
+      btree = new BOctTree<float>(PointerArray<float>(xyz_r).get(), xyz_r.size(), octtree_voxelSize, octtree_pointtype, true);
+    } else { // without reduction, xyz + attribute points
+      float** pts = octtree_pointtype.createPointArray<float>(this);
+      unsigned int nrpts = size<DataXYZ>("xyz");
+      btree = new BOctTree<float>(pts, nrpts, octtree_voxelSize, octtree_pointtype, true);
+      for(unsigned int i = 0; i < nrpts; ++i) delete[] pts[i]; delete[] pts;
+    }
+    // save created octtree
+    if(octtree_saveOct) {
+      cout << "Saving octree " << scanFileName << endl;
+      btree->serialize(scanFileName);
+    }
+  }
+
+  // copy tree into cache
+  try {
+    unsigned int size = btree->getMemorySize();
+    unsigned char* mem_ptr = m_shared_scan->createOcttree(size).get_raw_pointer();
+    new(mem_ptr) BOctTree<float>(*btree, mem_ptr, size);
+    delete btree; btree = 0;
+  } catch(runtime_error& e) {
+    // delete tree if copy to cache failed
+    delete btree;
+    throw e;
+  }
+}
+
+
+unsigned int ManagedScan::readFrames()
+{
+  // automatically read on getFrames
+  return m_shared_scan->getFrames().size();
+}
+
+void ManagedScan::saveFrames()
+{
+  m_shared_scan->saveFrames();
+}
+
+unsigned int ManagedScan::getFrameCount()
+{
+  return m_shared_scan->getFrames().size();
+}
+
+void ManagedScan::getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type)
+{
+  const Frame& frame(m_shared_scan->getFrames().at(i));
+  pose_matrix = frame.transformation;
+  type = static_cast<AlgoType>(frame.type);
+}
+
+void ManagedScan::addFrame(AlgoType type)
+{
+  if(m_reset_frames_on_write) {
+    m_shared_scan->clearFrames();
+    m_reset_frames_on_write = false;
+  }
+  m_shared_scan->addFrame(transMat, static_cast<unsigned int>(type));
+}

.svn/pristine/51/510ce7ed25ff56a932fc7c5485d244c0abd8dd80.svn-base

@@ -0,0 +1,398 @@
+/*
+ * basicScan implementation
+ *
+ * Copyright (C) Thomas Escher, Kai Lingemann
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+#include "slam6d/basicScan.h"
+
+#include "scanio/scan_io.h"
+#include "slam6d/kd.h"
+#include "slam6d/Boctree.h"
+#include "slam6d/ann_kd.h"
+
+#ifdef WITH_METRICS
+#include "slam6d/metrics.h"
+#endif //WITH_METRICS
+
+#include <list>
+#include <utility>
+#include <fstream>
+using std::ifstream;
+using std::ofstream;
+using std::flush;
+using std::string;
+using std::map;
+using std::pair;
+using std::vector;
+
+#include <boost/filesystem/operations.hpp>
+using namespace boost::filesystem;
+
+
+
+void BasicScan::openDirectory(const std::string& path, IOType type, int start, int end)
+{
+#ifdef WITH_METRICS
+  Timer t = ClientMetric::read_scan_time.start();
+#endif //WITH_METRICS
+
+  // create an instance of ScanIO
+  ScanIO* sio = ScanIO::getScanIO(type);
+
+  // query available scans in the directory from the ScanIO
+  std::list<std::string> identifiers(sio->readDirectory(path.c_str(), start, end));
+
+  Scan::allScans.reserve(identifiers.size());
+
+  // for each identifier, create a scan
+  for(std::list<std::string>::iterator it = identifiers.begin(); it != identifiers.end(); ++it) {
+    Scan::allScans.push_back(new BasicScan(path, *it, type));
+  }
+
+#ifdef WITH_METRICS
+  ClientMetric::read_scan_time.end(t);
+#endif //WITH_METRICS
+}
+
+void BasicScan::closeDirectory()
+{
+  // clean up the scan vector
+  for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it)
+    delete *it;
+  Scan::allScans.clear();
+}
+
+BasicScan::BasicScan(const std::string& path, const std::string& identifier, IOType type) :
+  m_path(path), m_identifier(identifier), m_type(type)
+{
+  init();
+
+  // request pose from file
+  double euler[6];
+  ScanIO* sio = ScanIO::getScanIO(m_type);
+  sio->readPose(m_path.c_str(), m_identifier.c_str(), euler);
+  rPos[0] = euler[0];
+  rPos[1] = euler[1];
+  rPos[2] = euler[2];
+  rPosTheta[0] = euler[3];
+  rPosTheta[1] = euler[4];
+  rPosTheta[2] = euler[5];
+
+  // write original pose matrix
+  EulerToMatrix4(euler, &euler[3], transMatOrg);
+
+  // initialize transform matrices from the original one, could just copy transMatOrg to transMat instead
+  transformMatrix(transMatOrg);
+
+  // reset the delta align matrix to represent only the transformations after local-to-global (transMatOrg) one
+  M4identity(dalignxf);
+}
+
+BasicScan::~BasicScan()
+{
+  // TODO: clean m_data up
+}
+
+void BasicScan::init()
+{
+  m_filter_max = 0.0;
+  m_filter_min = 0.0;
+  m_filter_top = 0.0;
+  m_filter_bottom = 0.0;
+  m_range_mutation = 0.0;
+  m_filter_range_set = false;
+  m_filter_height_set = false;
+  m_range_mutation_set = false;
+}
+
+
+void BasicScan::setRangeFilter(double max, double min)
+{
+  m_filter_max = max;
+  m_filter_min = min;
+  m_filter_range_set = true;
+}
+
+void BasicScan::setHeightFilter(double top, double bottom)
+{
+  m_filter_top = top;
+  m_filter_bottom = bottom;
+  m_filter_height_set = true;
+}
+
+void BasicScan::setRangeMutation(double range)
+{
+  m_range_mutation_set = true;
+  m_range_mutation = range;
+}
+
+void BasicScan::get(unsigned int types)
+{
+  ScanIO* sio = ScanIO::getScanIO(m_type);
+
+  vector<double> xyz;
+  vector<unsigned char> rgb;
+  vector<float> reflectance;
+  vector<float> amplitude;
+  vector<int> type;
+  vector<float> deviation;
+
+  PointFilter filter;
+  if(m_filter_range_set)
+    filter.setRange(m_filter_max, m_filter_min);
+  if(m_filter_height_set)
+    filter.setHeight(m_filter_top, m_filter_bottom);
+  if(m_range_mutation_set)
+    filter.setRangeMutator(m_range_mutation);
+
+  sio->readScan(m_path.c_str(),
+                m_identifier.c_str(),
+                filter,
+                &xyz,
+                &rgb,
+                &reflectance,
+                &amplitude,
+                &type,
+                &deviation);
+
+  // for each requested and filled data vector, allocate and write contents to their new data fields
+  if(types & DATA_XYZ && !xyz.empty()) {
+    double* data = reinterpret_cast<double*>(create("xyz", sizeof(double) * xyz.size()).get_raw_pointer());
+    for(unsigned int i = 0; i < xyz.size(); ++i) data[i] = xyz[i];
+  }
+  if(types & DATA_RGB && !rgb.empty()) {
+    unsigned char* data = reinterpret_cast<unsigned char*>(create("rgb", sizeof(unsigned char) * rgb.size()).get_raw_pointer());
+    for(unsigned int i = 0; i < rgb.size(); ++i) data[i] = rgb[i];
+  }
+  if(types & DATA_REFLECTANCE && !reflectance.empty()) {
+    float* data = reinterpret_cast<float*>(create("reflectance", sizeof(float) * reflectance.size()).get_raw_pointer());
+    for(unsigned int i = 0; i < reflectance.size(); ++i) data[i] = reflectance[i];
+  }
+  if(types & DATA_AMPLITUDE && !amplitude.empty()) {
+    int* data = reinterpret_cast<int*>(create("amplitude", sizeof(int) * amplitude.size()).get_raw_pointer());
+    for(unsigned int i = 0; i < amplitude.size(); ++i) data[i] = amplitude[i];
+  }
+  if(types & DATA_TYPE && !type.empty()) {
+    float* data = reinterpret_cast<float*>(create("type", sizeof(double) * type.size()).get_raw_pointer());
+    for(unsigned int i = 0; i < type.size(); ++i) data[i] = type[i];
+  }
+  if(types & DATA_DEVIATION && !deviation.empty()) {
+    float* data = reinterpret_cast<float*>(create("deviation", sizeof(float) * deviation.size()).get_raw_pointer());
+    for(unsigned int i = 0; i < deviation.size(); ++i) data[i] = deviation[i];
+  }
+}
+
+DataPointer BasicScan::get(const std::string& identifier)
+{
+  // try to get data
+  map<string, pair<unsigned char*, unsigned int>>::iterator it = m_data.find(identifier);
+
+  // create data fields
+  if(it == m_data.end()) {
+    // load from file
+    if(identifier == "xyz") get(DATA_XYZ); else
+    if(identifier == "rgb") get(DATA_RGB); else
+    if(identifier == "reflectance") get(DATA_REFLECTANCE); else
+    if(identifier == "amplitude") get(DATA_AMPLITUDE); else
+    if(identifier == "type") get(DATA_TYPE); else
+    if(identifier == "deviation") get(DATA_DEVIATION); else
+    // reduce on demand
+    if(identifier == "xyz reduced") calcReducedOnDemand(); else
+    if(identifier == "xyz reduced original") calcReducedOnDemand(); else
+    // show requests reduced points, manipulate in showing the same entry
+    if(identifier == "xyz reduced show") {
+      calcReducedOnDemand();
+      m_data["xyz reduced show"] = m_data["xyz reduced"];
+    } else
+    if(identifier == "octtree") {
+      createOcttree();
+    }
+
+    it = m_data.find(identifier);
+  }
+
+    // if nothing can be loaded, return an empty pointer
+  if(it == m_data.end())
+    return DataPointer(0, 0);
+  else
+    return DataPointer(it->second.first, it->second.second);
+}
+
+DataPointer BasicScan::create(const std::string& identifier, unsigned int size)
+{
+  map<string, pair<unsigned char*, unsigned int>>::iterator it = m_data.find(identifier);
+  if(it != m_data.end()) {
+    // try to reuse, otherwise reallocate
+    if(it->second.second != size) {
+      delete it->second.first;
+      it->second.first = new unsigned char[size];
+      it->second.second = size;
+    }
+  } else {
+    // create a new block of data
+    it = m_data.insert(
+      std::make_pair(
+        identifier,
+        std::make_pair(
+          new unsigned char[size],
+          size
+        )
+      )
+    ).first;
+  }
+  return DataPointer(it->second.first, it->second.second);
+}
+
+void BasicScan::clear(const std::string& identifier)
+{
+  map<string, pair<unsigned char*, unsigned int>>::iterator it = m_data.find(identifier);
+  if(it != m_data.end()) {
+    delete it->second.first;
+    m_data.erase(it);
+  }
+}
+
+
+void BasicScan::createSearchTreePrivate()
+{
+  DataXYZ xyz_orig(get("xyz reduced original"));
+  PointerArray<double> ar(xyz_orig);
+  switch(searchtree_nnstype)
+  {
+    case simpleKD:
+      kd = new KDtree(ar.get(), xyz_orig.size());
+      break;
+    case ANNTree:
+      kd = new ANNtree(ar.get(), xyz_orig.size());
+      break;
+    case BOCTree:
+      kd = new BOctTree<double>(ar.get(), xyz_orig.size(), 10.0, PointType(), true);
+      break;
+    case -1:
+      throw runtime_error("Cannot create a SearchTree without setting a type.");
+    default:
+      throw runtime_error("SearchTree type not implemented");
+  }
+
+  // TODO: make the switch cases above work with CUDA
+  if (searchtree_cuda_enabled) createANNTree();
+}
+
+void BasicScan::calcReducedOnDemandPrivate()
+{
+  // create reduced points and transform to initial position, save a copy of this for SearchTree
+  calcReducedPoints();
+  transformReduced(transMatOrg);
+  copyReducedToOriginal();
+}
+
+void BasicScan::createANNTree()
+{
+  // TODO: metrics
+#ifdef WITH_CUDA
+  if(!ann_kd_tree) {
+    DataXYZ xyz_orig(get("xyz reduced original"));
+    ann_kd_tree = new ANNkd_tree(PointArray<double>(xyz_orig).get(), xyz_orig.size(), 3, 1, ANN_KD_STD);
+    cout << "Cuda tree was generated with " << xyz_orig.size() << " points" << endl;
+  } else {
+    cout << "Cuda tree exists. No need for another creation" << endl;
+  }
+#endif
+}
+
+void BasicScan::createOcttree()
+{
+  string scanFileName = m_path + "scan" + m_identifier + ".oct";
+  BOctTree<float>* btree = 0;
+
+  // try to load from file, if successful return
+  if(octtree_loadOct && exists(scanFileName)) {
+    btree = new BOctTree<float>(scanFileName);
+    m_data.insert(
+      std::make_pair(
+        "octtree",
+        std::make_pair(
+          reinterpret_cast<unsigned char*>(btree),
+          0 // or memorySize()?
+        )
+      )
+    );
+    return;
+  }
+
+  // create octtree from scan
+  if(octtree_reduction_voxelSize > 0) { // with reduction, only xyz points
+    DataXYZ xyz_r(get("xyz reduced show"));
+    btree = new BOctTree<float>(PointerArray<double>(xyz_r).get(), xyz_r.size(), octtree_voxelSize, octtree_pointtype, true);
+  } else { // without reduction, xyz + attribute points
+    float** pts = octtree_pointtype.createPointArray<float>(this);
+    unsigned int nrpts = size<DataXYZ>("xyz");
+    btree = new BOctTree<float>(pts, nrpts, octtree_voxelSize, octtree_pointtype, true);
+    for(unsigned int i = 0; i < nrpts; ++i) delete[] pts[i]; delete[] pts;
+  }
+
+  // save created octtree
+  if(octtree_saveOct) {
+    cout << "Saving octree " << scanFileName << endl;
+    btree->serialize(scanFileName);
+  }
+
+  m_data.insert(
+    std::make_pair(
+      "octtree",
+      std::make_pair(
+        reinterpret_cast<unsigned char*>(btree),
+        0 // or memorySize()?
+      )
+    )
+  );
+}
+
+unsigned int BasicScan::readFrames()
+{
+  string filename = m_path + "scan" + m_identifier + ".frames";
+  ifstream file(filename.c_str());
+  file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit);
+  try {
+    double transformation[16];
+    unsigned int type;
+    do {
+      file >> transformation >> type;
+      m_frames.push_back(Frame(transformation, type));
+    } while(file.good());
+  } catch(...) {}
+
+  return m_frames.size();
+}
+
+void BasicScan::saveFrames()
+{
+  string filename = m_path + "scan" + m_identifier + ".frames";
+  ofstream file(filename.c_str());
+  for(vector<Frame>::iterator it = m_frames.begin(); it != m_frames.end(); ++it) {
+    file << it->transformation << it->type << '\n';
+  }
+  file << flush;
+  file.close();
+}
+
+unsigned int BasicScan::getFrameCount()
+{
+  return m_frames.size();
+}
+
+void BasicScan::getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type)
+{
+  const Frame& frame(m_frames.at(i));
+  pose_matrix = frame.transformation;
+  type = static_cast<AlgoType>(frame.type);
+}
+
+void BasicScan::addFrame(AlgoType type)
+{
+  m_frames.push_back(Frame(transMat, type));
+}

.svn/pristine/52/5236522fa5d1061da94980ae6be0cd497574e062.svn-base

@@ -0,0 +1,602 @@
+/*
+ * panorama implementation
+ *
+ * Copyright (C) HamidReza Houshiar
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+#include "slam6d/fbr/panorama.h"
+
+using namespace std;
+
+namespace fbr{
+  
+  void panorama::init(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param, panorama_map_method mMethod){
+    iWidth = width;
+    iHeight = height;
+    pMethod = method;
+    nImages = numberOfImages;
+    pParam = param;
+    if(mMethod == FARTHEST){
+      iMap.create(iHeight, iWidth, CV_32FC(3));
+      iMap = cv::Scalar::all(0);
+    }
+    else if(mMethod == EXTENDED){
+      extendedIMap.resize(iHeight);
+      for (unsigned int i = 0; i < iHeight; i++)
+	extendedIMap[i].resize(iWidth);
+    }
+    iReflectance.create(iHeight, iWidth, CV_8U);
+    iReflectance = cv::Scalar::all(0);
+    iRange.create(iHeight, iWidth, CV_32FC(1));
+    iRange = cv::Scalar::all(0);
+    mapMethod = mMethod;
+  }
+
+  panorama::panorama(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param, panorama_map_method mMethod){ 
+    init(width, height, method, numberOfImages, param, mMethod);
+  }
+  
+  panorama::panorama(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param){ 
+    init(width, height, method, numberOfImages, param, FARTHEST);
+  }
+  
+  panorama::panorama(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages){
+    double param = 0;
+    if(method == PANNINI)
+      param = 1;
+    else if (method == STEREOGRAPHIC)
+      param = 2;
+   
+    init(width, height, method, numberOfImages, param, FARTHEST);
+  }  
+
+  panorama::panorama(unsigned int width, unsigned int height, projection_method method){ 
+    double param = 0;
+    unsigned int numberOfImages = 1;
+    if(method == RECTILINEAR)
+      numberOfImages = 3;
+    else if(method == PANNINI){
+      numberOfImages = 3;
+      param = 1;
+    } else if (method == STEREOGRAPHIC){
+      numberOfImages = 3;
+      param = 2;
+    }
+
+    init(width, height, method, numberOfImages, param, FARTHEST);
+  }
+  
+  void panorama::map(int x, int y, cv::MatIterator_<cv::Vec4f> it, double range){
+    iReflectance.at<uchar>(y,x) = (*it)[3]*255;//reflectance
+    iRange.at<float>(y,x) = range;//range
+    if(mapMethod == FARTHEST){
+      //adding the point with max distance
+      if( iRange.at<float>(y,x) < range ){
+        iMap.at<cv::Vec3f>(y,x)[0] = (*it)[0];//x
+        iMap.at<cv::Vec3f>(y,x)[1] = (*it)[1];//y
+        iMap.at<cv::Vec3f>(y,x)[2] = (*it)[2];//z
+      }
+    }else if(mapMethod == EXTENDED){
+      //adding all the points
+      cv::Vec3f point;
+      point[0] = (*it)[0];//x
+      point[1] = (*it)[1];//y
+      point[2] = (*it)[2];//z
+      extendedIMap[y][x].push_back(point);
+    }
+  }
+
+  void panorama::createPanorama(cv::Mat scan){
+
+    //EQUIRECTANGULAR projection
+    if(pMethod == EQUIRECTANGULAR){
+      //adding the longitude to x axis and latitude to y axis
+      double xFactor = (double) iWidth / 2 / M_PI;
+      int widthMax = iWidth - 1;
+      double yFactor = (double) iHeight / ((MAX_ANGLE - MIN_ANGLE) / 360 * 2 * M_PI);
+      //shift all the valuse to positive points on image 
+      double heightLow =(0 - MIN_ANGLE) / 360 * 2 * M_PI;
+      int heightMax = iHeight - 1;
+      
+      cv::MatIterator_<cv::Vec4f> it, end; 
+      
+      for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); it != end; ++it){
+	double kart[3], polar[3], phi, theta, range;
+	kart[0] = (*it)[2]/100;
+	kart[1] = (*it)[0]/-100;
+	kart[2] = (*it)[1]/100;
+	toPolar(kart, polar);
+	//theta == polar[0] == scan [4]
+	//phi == polar[1] == scan [5]
+	//range == polar[2] == scan [3]
+	theta = polar[0] * 180 / M_PI;
+	phi = polar[1] * 180 / M_PI;
+	range = polar[2];
+	//horizantal angle of view of [0:360] and vertical of [-40:60]
+	phi = 360.0 - phi;
+	phi = phi * 2.0 * M_PI / 360.0;
+	theta -= 90;
+	theta *= -1;
+	theta *= 2.0 * M_PI / 360.0;
+	int x = (int) ( xFactor * phi);
+	if (x < 0) x = 0;
+	if (x > widthMax) x = widthMax;
+	int y = (int) ( yFactor * (theta + heightLow) );
+	y = heightMax - y;
+	if (y < 0) y = 0;
+	if (y > heightMax) y = heightMax;
+	
+	//create the iReflectance iRange and map
+	map(x, y, it, range);
+      }
+    }
+
+    //CONIC projection
+    if(pMethod == CONIC){
+      // set up maximum latitude and longitude angles of the robot 
+			double MIN_VERT_ANGLE = MIN_ANGLE * M_PI / 180.0, MAX_VERT_ANGLE = MAX_ANGLE * M_PI / 180.0,
+             MIN_HORIZ_ANGLE = -M_PI, MAX_HORIZ_ANGLE = M_PI;
+      // set up initial parameters according to MathWorld: http://mathworld.wolfram.com/AlbersEqual-AreaConicProjection.html
+      double Lat0 = 0., Long0 = 0.;
+      double Phi1 = -40. * M_PI / 180.0, Phi2 = 60 * M_PI / 180.0;
+      double n = (sin(Phi1) + sin(Phi2)) / 2.;
+      double C = sqr(cos(Phi1)) + 2 * n * sin(Phi1);
+      double Rho0 = sqrt(C - 2 * n * sin(Lat0)) / n;
+			// set up max values for x and y and add the longitude to x axis and latitude to y axis
+			double xmax = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MAX_HORIZ_ANGLE - Long0));
+			double xmin = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MIN_HORIZ_ANGLE - Long0));
+      double xFactor = (double) iWidth / ( xmax - xmin );
+      int widthMax = iWidth - 1;
+			double ymin = Rho0 - (1./n * sqrt(C - 2*n*sin(MIN_VERT_ANGLE)) ) * cos(n * ( 0. - Long0 ));
+			double ymax = Rho0 - (1./n * sqrt(C - 2*n*sin(MAX_VERT_ANGLE)) ) * cos(n * (MAX_HORIZ_ANGLE - Long0 ));
+      double yFactor = (double) iHeight / ( ymax - ymin );
+      //shift all the values to positive points on image 
+      int heightMax = iHeight - 1;
+      cv::MatIterator_<cv::Vec4f> it, end; 
+
+      for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); it != end; ++it){
+        double kart[3], polar[3], phi, theta, range;
+        kart[0] = (*it)[2]/100;
+        kart[1] = (*it)[0]/-100;
+        kart[2] = (*it)[1]/100;
+        toPolar(kart, polar);
+        //theta == polar[0] == scan [4]
+        //phi == polar[1] == scan [5]
+        //range == polar[2] == scan [3]
+        theta = polar[0] * 180 / M_PI;
+        phi = polar[1] * 180 / M_PI;
+        range = polar[2];
+        //phi == longitude == horizantal angle of view of [0:360] 
+        phi = 180.0 - phi;
+        phi *= M_PI / 180.0;
+        //theta == latitude == vertical angle of view of [-40:60]
+        theta -= 90;
+        theta *= -1;
+        theta *= M_PI / 180.0;
+
+				// add minimum x position as an offset
+        int x = (int) ( xFactor * (sqrt(C - 2 * n * sin( theta) ) / n * sin(n * (phi - Long0)) + fabs(xmin) ) );
+        if (x < 0) x = 0;
+        if (x > widthMax) x = widthMax;
+        
+        // add minimum y position as an offset
+        int y = (int) ( yFactor * (Rho0 - (1/n * sqrt(C - 2 * n * sin( theta) ) ) * cos(n * (phi - Long0)) + fabs( ymin ) ) );
+        y = heightMax - y;
+        if (y < 0) y = 0;
+        if (y > heightMax) y = heightMax;
+        //create the iReflectance iRange and map
+        map(x, y, it, range);
+      }
+    }
+    
+    //CYLINDRICAL projection
+    if(pMethod == CYLINDRICAL){
+      //adding the longitude to x and tan(latitude) to y
+      //find the x and y range
+      double xFactor = (double) iWidth / 2 / M_PI;
+      int widthMax = iWidth - 1;
+      double yFactor = (double) iHeight / (tan(MAX_ANGLE / 360 * 2 * M_PI) - tan(MIN_ANGLE / 360 * 2 * M_PI));
+      double heightLow = (MIN_ANGLE) / 360 * 2 * M_PI;
+      int heightMax = iHeight - 1;
+      
+      cv::MatIterator_<cv::Vec4f> it, end; 
+      
+      for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); it != end; ++it){
+	double kart[3], polar[3], phi, theta, range;
+	kart[0] = (*it)[2]/100;
+	kart[1] = (*it)[0]/-100;
+	kart[2] = (*it)[1]/100;
+	toPolar(kart, polar);
+	//theta == polar[0] == scan [4]
+	//phi == polar[1] == scan [5]
+	//range == polar[2] == scan [3]
+	theta = polar[0] * 180 / M_PI;
+	phi = polar[1] * 180 / M_PI;
+	range = polar[2];
+	//horizantal angle of view of [0:360] and vertical of [-40:60]
+	phi = 360.0 - phi;
+	phi = phi * 2.0 * M_PI / 360.0;
+	theta -= 90;
+	theta *= -1;
+	theta *= 2.0 * M_PI / 360.0;
+	int x = (int) ( xFactor * phi);
+	if (x < 0) x = 0;
+	if (x > widthMax) x = widthMax;
+	int y = (int) ((double) yFactor * (tan(theta) - tan(heightLow)));
+	y = heightMax - y;
+	if (y < 0) y = 0;
+	if (y > heightMax) y = heightMax;
+	
+	//create the iReflectance iRange and map
+	map(x, y, it, range);
+      }
+    }
+    
+    //Mercator Projection
+    if( pMethod == MERCATOR){
+      //find the x and y range
+      double xFactor = (double) iWidth / 2 / M_PI;
+      int widthMax = iWidth - 1;
+      double yFactor = (double) iHeight / ( log( tan( MAX_ANGLE / 360 * 2 * M_PI ) + ( 1 / cos( MAX_ANGLE / 360 * 2 * M_PI ) ) ) - log ( tan( MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI) ) ) );
+      double heightLow = log(tan(MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI)));
+      int heightMax = iHeight - 1;
+      
+      cv::MatIterator_<cv::Vec4f> it, end; 
+      
+      for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); it != end; ++it){
+	double kart[3], polar[3], phi, theta, range;
+	kart[0] = (*it)[2]/100;
+	kart[1] = (*it)[0]/-100;
+	kart[2] = (*it)[1]/100;
+	toPolar(kart, polar);
+	//theta == polar[0] == scan [4]
+	//phi == polar[1] == scan [5]
+	//range == polar[2] == scan [3]
+	theta = polar[0] * 180 / M_PI;
+	phi = polar[1] * 180 / M_PI;
+	range = polar[2];
+	//horizantal angle of view of [0:360] and vertical of [-40:60]
+	phi = 360.0 - phi;
+	phi = phi * 2.0 * M_PI / 360.0;
+	theta -= 90;
+	theta *= -1;
+	theta *= 2.0 * M_PI / 360.0;
+	int x = (int) ( xFactor * phi);
+	if (x < 0) x = 0;
+	if (x > widthMax) x = widthMax;
+	int y = (int) ( yFactor * (log(tan(theta) + (1/cos(theta))) - heightLow) );
+	y = heightMax - y;
+	if (y < 0) y = 0;
+	if (y > heightMax) y = heightMax;
+	
+	//create the iReflectance iRange and map
+	map(x, y, it, range);
+      }
+    }
+    
+    //RECTILINEAR projection
+    if(pMethod == RECTILINEAR){
+      //default value for nImages
+      if(nImages == 0) nImages = 3;
+      cout<<"Number of images per scan is: "<<nImages<<endl;
+      double l0, p1, iMinx, iMaxx, iMiny, iMaxy, interval;
+      interval = 2 * M_PI / nImages;
+      iMiny = -M_PI/9;
+      iMaxy = 2*M_PI/9;
+      //latitude of projection center
+      p1 = 0;
+      
+      //go through all points 
+      cv::MatIterator_<cv::Vec4f> it, end; 
+      
+      for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); it != end; ++it){
+	double kart[3], polar[3], phi, theta, range;
+	kart[0] = (*it)[2]/100;
+	kart[1] = (*it)[0]/-100;
+	kart[2] = (*it)[1]/100;
+	toPolar(kart, polar);
+	//theta == polar[0] == scan [4]
+	//phi == polar[1] == scan [5]
+	//range == polar[2] == scan [3]
+	theta = polar[0] * 180 / M_PI;
+	phi = polar[1] * 180 / M_PI;
+	range = polar[2];
+	//horizantal angle of view of [0:360] and vertical of [-40:60]
+	phi = 360.0 - phi;
+	phi = phi * 2.0 * M_PI / 360.0;
+	theta -= 90;
+	theta *= -1;
+	theta *= 2.0 * M_PI / 360.0;
+	for(unsigned int j = 0 ; j < nImages ; j++){
+	  iMinx = j * interval;
+	  iMaxx = (j + 1) * interval;
+	  //check for point in interval
+	  if(phi < iMaxx && phi > iMinx){
+	    double max, min, coscRectilinear;
+	    //the longitude of projection center
+	    l0 = iMinx + interval / 2;
+	    //finding the min and max of the x direction
+	    coscRectilinear = sin(p1) * sin(iMaxy) + cos(p1) * cos(iMaxy) * cos(iMaxx - l0);
+	    max = (cos(iMaxy) * sin(iMaxx - l0) / coscRectilinear);
+	    coscRectilinear = sin(p1) * sin(iMiny) + cos(p1) * cos(iMiny) * cos(iMinx - l0);
+	    min = (cos(iMiny) * sin(iMinx - l0) / coscRectilinear);
+	    double xFactor = (double) (iWidth / nImages) / (max - min);
+	    double xlow = min;
+	    int widthMax = (iWidth / nImages) - 1;
+	    //finding the min and max of y direction
+	    coscRectilinear = sin(p1) * sin(iMaxy) + cos(p1) * cos(iMaxy) * cos(iMaxx - l0);
+	    max = ( (cos(p1) * sin(iMaxy) - sin(p1) * cos(iMaxy) * cos(iMaxx - l0) )/ coscRectilinear);
+	    coscRectilinear = sin(p1) * sin(iMiny) + cos(p1) * cos(iMiny) * cos(iMinx - l0);
+	    min = ( (cos(p1) * sin(iMiny) - sin(p1) * cos(iMiny) * cos(iMinx - l0) )/ coscRectilinear);
+	    double yFactor = (double) iHeight / (max - min);
+	    double heightLow = min;
+	    int heightMax = iHeight - 1;
+	    //project the points and add them to image
+	    coscRectilinear = sin(p1) * sin(theta) + cos(p1) * cos(theta) * cos(phi - l0);
+	    int x = (int)(xFactor) * ((cos(theta) * sin(phi - l0) / coscRectilinear) - xlow);
+	    if (x < 0) x = 0;
+	    if (x > widthMax) x = widthMax;
+	    x = x + (j * iWidth / nImages);
+	    int y = (int) (yFactor) * (( (cos(p1) * sin(theta) - sin(p1) * cos(theta) * cos(phi - l0)) / coscRectilinear) - heightLow);
+	    y = heightMax - y;
+	    if (y < 0) y = 0;
+	    if (y > heightMax) y = heightMax;
+	    
+	    //create the iReflectance iRange and map
+	    map(x, y, it, range);
+	  }
+	}
+      }
+    }
+    
+    //PANNINI projection
+    if(pMethod == PANNINI){
+      //default values for nImages and dPannini==pParam
+      if(pParam == 0) pParam = 1;
+      if(nImages == 0) nImages = 3;
+      cout << "Parameter d is:" << pParam <<", Number of images per scan is:" << nImages << endl;
+      double l0, p1, iMinx, iMaxx, iMiny, iMaxy, interval;
+      interval = 2 * M_PI / nImages;
+      iMiny = -M_PI/9;
+      iMaxy = 2*M_PI/9;
+      //latitude of projection center
+      p1 = 0;
+      
+      cv::MatIterator_<cv::Vec4f> it, end; 
+         
+      for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); it != end; ++it){
+	double kart[3], polar[3], phi, theta, range;
+	kart[0] = (*it)[2]/100;
+	kart[1] = (*it)[0]/-100;
+	kart[2] = (*it)[1]/100;
+	toPolar(kart, polar);
+	//theta == polar[0] == scan [4]
+	//phi == polar[1] == scan [5]
+	//range == polar[2] == scan [3]
+	theta = polar[0] * 180 / M_PI;
+	phi = polar[1] * 180 / M_PI;
+	range = polar[2];
+	//horizantal angle of view of [0:360] and vertical of [-40:60]
+	phi = 360.0 - phi;
+	phi = phi * 2.0 * M_PI / 360.0;
+	theta -= 90;
+	theta *= -1;
+	theta *= 2.0 * M_PI / 360.0;
+	for(unsigned int j = 0 ; j < nImages ; j++){
+	  iMinx = j * interval;
+	  iMaxx = (j + 1) * interval;
+	  //check for point in interval
+	  if(phi < (iMaxx) && phi > (iMinx)){
+	    double max, min, sPannini;
+	    //the longitude of projection center
+	    l0 = iMinx + interval / 2;
+	    //use the S variable of pannini projection mentioned in the thesis
+	    //finding the min and max of the x direction
+	    sPannini = (pParam + 1) / (pParam + sin(p1) * tan(iMaxy) + cos(p1) * cos(iMaxx - l0));
+	    max = sPannini * (sin(iMaxx - l0));
+	    sPannini = (pParam + 1) / (pParam + sin(p1) * tan(iMiny) + cos(p1) * cos(iMinx - l0));
+	    min = sPannini * (sin(iMinx - l0));
+	    double xFactor = (double) (iWidth / nImages) / (max - min);
+	    double xlow = min;
+	    int widthMax = (iWidth / nImages) - 1;
+	    //finding the min and max of y direction
+	    sPannini = (pParam + 1) / (pParam + sin(p1) * tan(iMaxy) + cos(p1) * cos(iMaxx - l0));
+	    max = sPannini * (tan(iMaxy) * (cos(p1) - sin(p1) * 1/tan(iMaxy) * cos(iMaxx - l0)));
+	    sPannini = (pParam + 1) / (pParam + sin(p1) * tan(iMiny) + cos(p1) * cos(iMinx - l0));
+	    min = sPannini * (tan(iMiny) * (cos(p1) - sin(p1) * 1/tan(iMiny) * cos(iMinx - l0)));
+	    double yFactor = (double) iHeight / (max - min);
+	    double heightLow = min;
+	    int heightMax = iHeight - 1;
+	    //project the points and add them to image
+	    sPannini = (pParam + 1) / (pParam + sin(p1) * tan(theta) + cos(p1) * cos(phi - l0));
+	    int x = (int)(xFactor) * (sPannini * sin(phi - l0) - xlow);
+	    if (x < 0) x = 0;
+	    if (x > widthMax) x = widthMax;
+	    x = x + (j * iWidth / nImages);
+	    int y = (int) (yFactor) * ( (sPannini * tan(theta) * (cos(p1) - sin(p1) * (1/tan(theta)) * cos(phi - l0) ) ) - heightLow );
+	    y = heightMax - y;
+	    if (y < 0) y = 0;
+	    if (y > heightMax) y = heightMax;
+		    
+	    //create the iReflectance iRange and map
+	    map(x, y, it, range);
+	  }
+	}
+      }
+    }
+    
+    //STEREOGRAPHIC projection
+    if(pMethod == STEREOGRAPHIC){
+      //default values for nImages and rStereographic==pParam
+      if(pParam == 0) pParam = 2;
+      if(nImages == 0) nImages = 3;
+      cout << "Paremeter R is:" << pParam << ", Number of images per scan is:" << nImages << endl;
+      // l0 and p1 are the center of projection iminx, imaxx, iminy, imaxy are the bounderis of intervals
+      double l0, p1, iMinx, iMaxx, iMiny, iMaxy, interval;
+      interval = 2 * M_PI / nImages;
+      iMiny = -M_PI/9;
+      iMaxy = 2*M_PI/9;
+      //latitude of projection center
+      p1 = 0;
+      
+      //go through all points
+      cv::MatIterator_<cv::Vec4f> it, end; 
+         
+      for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); it != end; ++it){
+	double kart[3], polar[3], phi, theta, range;
+	kart[0] = (*it)[2]/100;
+	kart[1] = (*it)[0]/-100;
+	kart[2] = (*it)[1]/100;
+	toPolar(kart, polar);
+	//theta == polar[0] == scan [4]
+	//phi == polar[1] == scan [5]
+	//range == polar[2] == scan [3]
+	theta = polar[0] * 180 / M_PI;
+	phi = polar[1] * 180 / M_PI;
+	range = polar[2];
+	//horizantal angle of view of [0:360] and vertical of [-40:60]
+	phi = 360.0 - phi;
+	phi = phi * 2.0 * M_PI / 360.0;
+	theta -= 90;
+	theta *= -1;
+	theta *= 2.0 * M_PI / 360.0;
+	for (unsigned int j = 0 ; j < nImages ; j++){
+	  iMinx = j * interval;
+	  iMaxx = (j + 1) * interval;
+	  //check for point in intervals
+	  if(phi < (iMaxx) && phi > (iMinx)){
+	    double max, min, k;
+	    //longitude of projection center
+	    l0 = iMinx + interval / 2;
+	    //use the R variable of stereographic projection mentioned in the thesis
+	    //finding the min and max of x direction
+	    k = (2 * pParam) / (1 + sin(p1) * sin(p1) + cos(p1) * cos(p1) * cos(iMaxx - l0));
+	    max = k * cos(p1) * sin (iMaxx - l0);
+	    k = (2 * pParam) / (1 + sin (p1) * sin(p1) + cos(p1) * cos(p1) * cos(iMinx -l0));
+	    min = k * cos(p1) * sin (iMinx -l0);
+	    double xFactor = (double) (iWidth / nImages) / (max - min);
+	    double xlow = min;
+	    int widthMax = (iWidth / nImages) - 1;
+	    //finding the min and max of y direction
+	    k = (2 * pParam) / (1 + sin(p1) * sin(iMaxy) + cos(p1) * cos(iMaxy) * cos(iMaxx - l0));
+	    max = k * (cos(p1) * sin(iMaxy) - sin(p1) * cos(iMaxy) * cos(iMaxx - l0));
+	    k = (2 * pParam) / (1 + sin(p1) * sin(iMiny) + cos(p1) * cos(iMiny) * cos(iMinx - l0));
+	    min = k * (cos(p1) * sin(iMiny) - sin(p1) * cos(iMiny) * cos(iMinx - l0));
+	    double yFactor = (double) iHeight / (max - min);
+	    double heightLow = min;
+	    int heightMax = iHeight - 1;
+	    //project the points and add them to image
+	    k = (2 * pParam) / (1 + sin(p1) * sin(theta) + cos(p1) * cos(theta) * cos(phi - l0));
+	    int x = (int) (xFactor) * (k * cos(theta) * sin(phi - l0) - xlow);
+	    if (x < 0) x = 0;
+	    if (x > widthMax) x = widthMax;
+	    x = x + (j * iWidth / nImages);
+	    int y = (int) (yFactor) * (k * ( cos(p1) * sin(theta) - sin(p1) * cos(theta) * cos(phi - l0) ) - heightLow);
+	    y = heightMax - y;
+	    if (y < 0) y = 0;
+	    if (y > heightMax) y = heightMax;
+	    
+	    //create the iReflectance iRange and map
+	    map(x, y, it, range);
+	  }
+	}
+      }
+    }
+    
+    //ZAXIS projection
+    if(pMethod == ZAXIS){
+      double zmin = -200;
+      double zmax = 4000;
+      //adding the longitude to x axis and latitude to y axis
+      double xFactor = (double) iWidth / 2 / M_PI;
+      int widthMax = iWidth - 1;
+      cout << "ZMAX= " << zmax << " ZMIN= "<< zmin << endl;
+      double yFactor = (double) iHeight / (zmax - zmin);
+      //shift all the valuse to positive points on image 
+      double heightLow = zmin;
+      int heightMax = iHeight - 1;
+      
+      cv::MatIterator_<cv::Vec4f> it, end; 
+         
+      for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); it != end; ++it){
+	double kart[3], polar[3], phi, theta, range;
+	kart[0] = (*it)[2]/100;
+	kart[1] = (*it)[0]/-100;
+	kart[2] = (*it)[1]/100;
+	toPolar(kart, polar);
+	//theta == polar[0] == scan [4]
+	//phi == polar[1] == scan [5]
+	//range == polar[2] == scan [3]
+	theta = polar[0] * 180 / M_PI;
+	phi = polar[1] * 180 / M_PI;
+	range = polar[2];
+	//horizantal angle of view of [0:360] and vertical of [-40:60]
+	phi = 360.0 - phi;
+	phi = phi * 2.0 * M_PI / 360.0;
+	theta -= 90;
+	theta *= -1;
+	theta *= 2.0 * M_PI / 360.0;
+	int x = (int) ( xFactor * phi);
+	if (x < 0) x = 0;
+	if (x > widthMax) x = widthMax;
+	///////////////////check this
+	int y = (int) ( yFactor * ((*it)[1] - heightLow) );
+	y = heightMax - y;
+	if (y < 0) y = 0;
+	if (y > heightMax) y = heightMax;
+	
+	//create the iReflectance iRange and map
+	map(x, y, it, range);
+      }
+    }
+  }
+
+  unsigned int panorama::getImageWidth(){
+    return iWidth;
+  }
+
+  unsigned int panorama::getImageHeight(){
+    return iHeight;
+  }
+
+  projection_method panorama::getProjectionMethod(){
+    return pMethod;
+  }
+
+  unsigned int panorama::getNumberOfImages(){
+    return nImages;
+  }
+  
+  double panorama::getProjectionParam(){
+    return pParam;
+  }
+  
+  cv::Mat panorama::getReflectanceImage(){
+    return iReflectance;
+  }
+
+  cv::Mat panorama::getMap(){
+    return iMap;
+  }
+
+  cv::Mat panorama::getRangeImage(){
+    return iRange;
+  }
+  
+  vector<vector<vector<cv::Vec3f> > > panorama::getExtendedMap(){
+    return extendedIMap;
+  }
+
+  panorama_map_method panorama::getMapMethod(){
+    return mapMethod;
+  }
+
+  void panorama::getDescription(){
+    cout<<"panorama created with width: "<<iWidth<<", and height: "<<iHeight<<", and projection method: "<<projectionMethodToString(pMethod)<<", number of images: "<<nImages<<", projection param: "<<pParam<<"."<<endl;
+    cout<<endl;
+  }
+}
+

.svn/pristine/53/535f20e64cd69a844e713f686931353c70b324a4.svn-base

@@ -0,0 +1,278 @@
+/*
+ * sharedScan implementation
+ *
+ * Copyright (C) Thomas Escher, Kai Lingemann
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+#include "scanserver/sharedScan.h"
+
+#include <iostream>
+using std::cout;
+using std::cerr;
+using std::endl;
+#include <string>
+
+#include "scanserver/clientInterface.h"
+
+
+
+SharedScan::SharedScan(const ip::allocator<void, SegmentManager> & allocator,
+  const SharedStringSharedPtr& dir_path_ptr, const char *io_identifier,
+  IOType iotype) :
+  m_dir_path_ptr(dir_path_ptr),
+  m_io_identifier(io_identifier, allocator),
+  m_iotype(iotype),
+  m_prefetch(0),
+  m_max_dist(0.0), m_min_dist(0.0),
+  m_height_top(0.0), m_height_bottom(0.0),
+  m_range_mutator_param(0.0),
+  m_range_mutator_param_set(false),
+  m_range_param_set(false), m_height_param_set(false),
+  m_reduction_parameters(allocator),
+  m_show_parameters(allocator),
+  m_octtree_parameters(allocator),
+  m_load_frames_file(true),
+  m_frames(allocator)
+{
+  // until boost-1.47 ipc-strings can do garbage with g++-4.4 -O2 and higher (optimizations and versions)
+  std::string bugfix(io_identifier);
+  m_io_identifier[bugfix.length()] = '\0';
+
+  // COs are allocated in the ServerScan-ctor
+}
+
+SharedScan::~SharedScan()
+{
+  // TODO
+  // get segment manager and deconstruct all available data fields
+  // TODO: Delete COs if notzero and notify CM?
+}
+
+bool SharedScan::operator==(const SharedScan& r) const
+{
+  return (m_io_identifier == r.m_io_identifier) && (*m_dir_path_ptr == *r.m_dir_path_ptr) && m_iotype == r.m_iotype;
+}
+
+void SharedScan::setRangeParameters(double max_dist, double min_dist)
+{
+  // if a non-first set differs from the previous ones, invalidate all COs
+  if(m_range_param_set) {
+    if(max_dist != m_max_dist || min_dist != m_min_dist) {
+      invalidateFull();
+      invalidateReduced();
+      invalidateShow();
+    }
+  }
+  m_max_dist = max_dist;
+  m_min_dist = min_dist;
+  m_range_param_set = true;
+}
+
+void SharedScan::setHeightParameters(double top, double bottom)
+{
+  // if a non-first set differs from the previous ones, invalidate all COs
+  if(m_height_param_set) {
+    if(top != m_height_top || bottom != m_height_bottom) {
+      invalidateFull();
+      invalidateReduced();
+      invalidateShow();
+    }
+  }
+  m_height_top = top;
+  m_height_bottom = bottom;
+  m_height_param_set = true;
+}
+
+void SharedScan::setRangeMutationParameters(double range)
+{
+  // if a non-first set differs from the previous ones, invalidate all COs
+  if(m_range_mutator_param_set) {
+    if(range != m_range_mutator_param) {
+      invalidateFull();
+      invalidateReduced();
+      invalidateShow();
+    }
+  }
+  m_range_mutator_param = range;
+  m_range_mutator_param_set = true;
+}
+
+void SharedScan::setReductionParameters(const char* params)
+{
+  // if a non-first set differs from the previous ones, invalidate reduced COs
+  if(!m_reduction_parameters.empty() && m_reduction_parameters != params) {
+    invalidateReduced();
+  }
+  m_reduction_parameters = params;
+}
+
+void SharedScan::setShowReductionParameters(const char* params)
+{
+  // if a non-first set differs from the previous ones, invalidate reduced COs
+  if(!m_show_parameters.empty() && m_show_parameters != params) {
+    invalidateShow();
+  }
+  m_show_parameters = params;
+}
+
+void SharedScan::setOcttreeParameters(const char* params)
+{
+  // if a non-first set differs from the previous ones, invalidate reduced COs
+  if(!m_octtree_parameters.empty() && m_octtree_parameters != params) {
+    m_octtree->invalidate<SharedScan::onInvalidation>();
+  }
+  m_octtree_parameters = params;
+}
+
+PointFilter SharedScan::getPointFilter() const
+{
+  PointFilter r;
+  if(m_range_param_set)
+    r.setRange(m_max_dist, m_min_dist);
+  if(m_height_param_set)
+    r.setHeight(m_height_top, m_height_bottom);
+  if(m_range_mutator_param_set)
+    r.setRangeMutator(m_range_mutator_param);
+  
+  return r;
+}
+
+void SharedScan::invalidateFull()
+{
+  m_xyz->invalidate<SharedScan::onInvalidation>();
+  m_rgb->invalidate<SharedScan::onInvalidation>();
+}
+
+void SharedScan::invalidateReduced()
+{
+  m_xyz_reduced->invalidate<SharedScan::onInvalidation>();
+  m_xyz_reduced_original->invalidate<SharedScan::onInvalidation>();
+}
+
+void SharedScan::invalidateShow()
+{
+  m_show_reduced->invalidate<SharedScan::onInvalidation>();
+  m_octtree->invalidate<SharedScan::onInvalidation>();
+}
+
+void SharedScan::clearFrames()
+{
+  ClientInterface* client = ClientInterface::getInstance();
+  client->clearFrames(this);
+  // don't try to load again from the still existing files
+  m_load_frames_file = false;
+}
+
+void SharedScan::addFrame(double* transformation, unsigned int type)
+{
+  ClientInterface* client = ClientInterface::getInstance();
+  client->addFrame(this, transformation, type);
+}
+
+const FrameVector& SharedScan::getFrames()
+{
+  // on a restart with existing frame files try to load these
+  if(m_frames.empty() && m_load_frames_file == true) {
+    ClientInterface* client = ClientInterface::getInstance();
+    client->loadFramesFile(this);
+    // don't try to load again if frames are still empty
+    m_load_frames_file = false;
+  }
+  return m_frames;
+}
+
+void SharedScan::saveFrames()
+{
+  ClientInterface* client = ClientInterface::getInstance();
+  client->saveFramesFile(this);
+  // we just saved the file, no need to read it
+  m_load_frames_file = false;
+}
+
+double* SharedScan::getPose()
+{
+  if(m_pose == 0) {
+    ClientInterface* client = ClientInterface::getInstance();
+    client->getPose(this);
+  }
+  return m_pose.get();
+}
+
+DataXYZ SharedScan::getXYZ() {
+  return m_xyz.get()->getCacheData<SharedScan::onCacheMiss>();
+}
+
+DataRGB SharedScan::getRGB() {
+  return m_rgb->getCacheData<SharedScan::onCacheMiss>();
+}
+
+DataReflectance SharedScan::getReflectance() {
+  return m_reflectance->getCacheData<SharedScan::onCacheMiss>();
+}
+
+DataAmplitude SharedScan::getAmplitude() {
+  return m_amplitude->getCacheData<SharedScan::onCacheMiss>();
+}
+
+DataType SharedScan::getType() {
+  return m_type->getCacheData<SharedScan::onCacheMiss>();
+}
+
+DataDeviation SharedScan::getDeviation() {
+  return m_deviation->getCacheData<SharedScan::onCacheMiss>();
+}
+
+DataXYZ SharedScan::getXYZReduced() {
+  return m_xyz_reduced->getCacheData<SharedScan::onCacheMiss>();
+}
+
+DataXYZ SharedScan::createXYZReduced(unsigned int size) {
+  // size is in units of double[3], scale to bytes
+  return m_xyz_reduced->createCacheData<SharedScan::onAllocation>(size*3*sizeof(double));
+}
+
+DataXYZ SharedScan::getXYZReducedOriginal() {
+  return m_xyz_reduced_original->getCacheData<SharedScan::onCacheMiss>();
+}
+
+DataXYZ SharedScan::createXYZReducedOriginal(unsigned int size) {
+  // size is in units of double[3], scale to bytes
+  return m_xyz_reduced_original->createCacheData<SharedScan::onAllocation>(size*3*sizeof(double));
+}
+
+TripleArray<float> SharedScan::getXYZReducedShow() {
+  return m_show_reduced->getCacheData<SharedScan::onCacheMiss>();
+}
+
+TripleArray<float> SharedScan::createXYZReducedShow(unsigned int size) {
+  return m_show_reduced->createCacheData<SharedScan::onAllocation>(size*3*sizeof(float));
+}
+
+DataPointer SharedScan::getOcttree() {
+  return m_octtree->getCacheData<SharedScan::onCacheMiss>();
+}
+
+DataPointer SharedScan::createOcttree(unsigned int size) {
+  return m_octtree->createCacheData<SharedScan::onAllocation>(size);
+}
+
+void SharedScan::onCacheMiss(CacheObject* obj)
+{
+  ClientInterface* client = ClientInterface::getInstance();
+  client->loadCacheObject(obj);
+}
+
+void SharedScan::onAllocation(CacheObject* obj, unsigned int size)
+{
+  ClientInterface* client = ClientInterface::getInstance();
+  client->allocateCacheObject(obj, size);
+}
+
+void SharedScan::onInvalidation(CacheObject* obj)
+{
+  ClientInterface* client = ClientInterface::getInstance();
+  client->invalidateCacheObject(obj);
+}

.svn/pristine/78/78173974d7adc42895bd5ed6c64cf3d66c8813a7.svn-base

@@ -0,0 +1,416 @@
+/*
+ * feature_based_registration implementation
+ *
+ * Copyright (C) HamidReza Houshiar
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+#include <stdio.h>
+#include <fstream>
+#include "slam6d/fbr/fbr_global.h"
+#include "slam6d/fbr/scan_cv.h"
+#include "slam6d/fbr/panorama.h"
+#include "slam6d/fbr/feature.h"
+#include "slam6d/fbr/feature_matcher.h"
+#include "slam6d/fbr/registration.h"
+
+using namespace std;
+using namespace fbr;
+
+struct information{
+  string local_time;
+  string dir, outDir;
+  int iWidth, iHeight, nImages, minDistance, minError, minInlier, fScanNumber, sScanNumber, verbose;
+  double pParam, mParam;
+  IOType sFormat;
+  projection_method pMethod;
+  feature_detector_method fMethod;
+  feature_descriptor_method dMethod;
+  matcher_method mMethod;
+  registration_method rMethod;
+  
+  int fSPoints, sSPoints, fFNum, sFNum, mNum, filteredMNum;
+  double fSTime, sSTime, fPTime, sPTime, fFTime, sFTime, fDTime, sDTime, mTime, rTime; 
+} info;
+
+/**
+ * usage : explains how to use the program CMD
+ */
+void usage(int argc, char** argv){
+  printf("\n");
+  printf("USAGE: %s dir -s firstScanNumber -e secondScanNumber \n", argv[0]);
+  printf("\n");
+  printf("\n");
+  printf("\tOptions:\n");
+  printf("\t\t-f scanFormat\t\t input scan file format [RIEGL_TXT|RXP|ALL SLAM6D SCAN_IO]\n");
+  printf("\t\t-W iWidth\t\t panorama image width\n");
+  printf("\t\t-H iHeight\t\t panorama image height\n");
+  printf("\t\t-p pMethod\t\t projection method [EQUIRECTANGULAR|CONIC|CYLINDRICAL|MERCATOR|RECTILINEAR|PANNINI|STEREOGRAPHIC|ZAXIS]\n");
+  printf("\t\t-N nImage\t\t number of images used for some projections\n");
+  printf("\t\t-P pParam\t\t special projection parameter (d for Pannini and r for stereographic)\n");
+  printf("\t\t-F fMethod\t\t feature detection method [SURF|SIFT|ORB|FAST|STAR]\n");
+  printf("\t\t-d dMethod\t\t feature description method [SURF|SIFT|ORB]\n");
+  printf("\t\t-m mMethod\t\t feature matching method [BRUTEFORCE|FLANN|KNN|RADIUS|RATIO]\n");
+  printf("\t\t-D minDistance \t\t threshold for min distance in registration process\n");
+  printf("\t\t-E minError \t\t threshold for min error in registration process\n");
+  printf("\t\t-I minInlier \t\t threshold for min number of inliers in registration process\n");
+  printf("\t\t-M mParam \t\t special matching paameter (knn for KNN and r for radius)\n");
+  printf("\t\t-r registration \t registration method [ALL|ransac]\n");
+  printf("\t\t-V verbose \t\t level of verboseness\n");
+  printf("\t\t-O outDir \t\t output directory if not stated same as input\n");
+  printf("\n");
+  printf("\tExamples:\n");
+  printf("\tUsing Bremen City dataset:\n");
+  printf("\tLoading scan000.txt and scan001.txt:\n");
+  printf("\t\t %s ~/dir/to/bremen_city -s 0 -e 1\n", argv[0]);
+  printf("\tLoading scan005.txt and scan006.txt and output panorma images and feature images and match images in ~/dir/to/bremen_city/out dir:\n");
+  printf("\t\t %s -V 1 -O ~/dir/to/bremen_city/out/ ~/dir/to/bremen_city -s 5 -e 6 \n", argv[0]);
+  printf("\tLoading scan010.txt and scan011.txt using Mercator projection and SURF feature detector and SIFT descriptor:\n");
+  printf("\t\t %s -p MERCATOR -F SURF -d SIFT -O ~/dir/to/bremen_city/out/ ~/dir/to/bremen_city -s 10 -e 11 \n", argv[0]);
+  printf("\n");
+  exit(1);
+}
+
+void parssArgs(int argc, char** argv, information& info){
+  time_t rawtime;
+  struct tm *timeinfo;
+  time(&rawtime);
+  char time[50];
+  timeinfo = localtime (&rawtime);
+  sprintf(time, "%d-%d-%d-%d:%d:%d", timeinfo->tm_year + 1900, timeinfo->tm_mon + 1, timeinfo->tm_mday, timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec);
+  info.local_time = time;
+  
+  //default values
+  info.iWidth = 3600;
+  info.iHeight = 1000;
+  info.nImages = 1;
+  info.minDistance = 50;
+  info.minError = 50;
+  info.minInlier = 5;
+  info.verbose = 0;
+  //depend on the projection method
+  info.pParam = 0;
+  info.mParam = 0;
+  //===============================
+  info.sFormat = RIEGL_TXT;
+  info.pMethod = EQUIRECTANGULAR;
+  info.fMethod = SIFT_DET;
+  info.dMethod = SIFT_DES;
+  info.mMethod = RATIO;
+  info.rMethod = RANSAC;
+  info.outDir = "";
+
+  int c;
+  opterr = 0;
+  //reade the command line and get the options
+  while ((c = getopt (argc, argv, "F:W:H:p:N:P:f:d:m:D:E:I:M:r:V:O:s:e:")) != -1)
+    switch (c)
+      {
+      case 's':
+	info.fScanNumber = atoi(optarg);
+	break;
+      case 'e':
+	info.sScanNumber = atoi(optarg);
+	break;
+      case 'f':
+	info.sFormat = stringToScanFormat(optarg);
+	break;
+      case 'W':
+	info.iWidth = atoi(optarg);
+	break;
+      case 'H':
+        info.iHeight = atoi(optarg);
+	break;
+      case 'p':
+	info.pMethod = stringToProjectionMethod(optarg);
+	break;
+      case 'N':
+	info.nImages = atoi(optarg);
+	break;
+      case 'P':
+	info.pParam = atoi(optarg);
+	break;
+      case 'F':
+	info.fMethod = stringToFeatureDetectorMethod(optarg);
+	break;
+      case 'd':
+	info.dMethod = stringToFeatureDescriptorMethod(optarg);
+	break;
+      case 'm':
+	info.mMethod = stringToMatcherMethod(optarg);
+	break;
+      case 'D':
+	info.minDistance = atoi(optarg);
+	break;
+      case 'E':
+	info.minError = atoi(optarg);
+	break;
+      case 'I':
+	info.minInlier = atoi(optarg);
+	break;
+      case 'M':
+	info.mParam = atoi(optarg);
+	break;
+      case 'r':
+	info.rMethod = stringToRegistrationMethod(optarg);
+	break;
+      case 'V':
+	info.verbose = atoi(optarg);
+	break;
+      case 'O':
+	info.outDir = optarg;
+	break;
+      case '?':
+	cout<<"Unknown option character "<<optopt<<endl;
+	usage(argc, argv);
+	break;
+      default:
+	usage(argc, argv);
+      }
+  if(info.pMethod == PANNINI && info.pParam == 0){
+    info.pParam = 1;
+    if(info.nImages < 3) info.nImages = 3;
+  }
+  if(info.pMethod == STEREOGRAPHIC && info.pParam == 0){
+    info.pParam = 2;
+    if(info.nImages < 3) info.nImages = 3;
+  }
+  if(info.pMethod == RECTILINEAR && info.nImages < 3)
+    info.nImages = 3;
+  if(info.mMethod == KNN && info.mParam == 0)
+    info.mParam = 3;
+  if(info.mMethod == RADIUS && info.mParam == 0)
+    info.mParam = 100;
+  if(info.dMethod == ORB_DES && info.fMethod == SIFT_DET){
+    cout<<"Error: SIFT feature doesn't work with ORB descriptor."<<endl;
+    usage(argc, argv);
+  }
+  if(info.mMethod == FLANN && info.dMethod == ORB_DES){
+    cout<<"Error: ORB descriptoronly works with BRUTEFORCE matcher."<<endl;
+    usage(argc, argv);
+  }
+
+  if (optind > argc - 1)
+    {
+      cout<<"Too few input arguments. At least dir and two scan numbers are required."<<endl;
+      usage(argc, argv);
+    }
+  
+  info.dir = argv[optind];
+  //info.fScanNumber = atoi(argv[optind+1]);
+  //info.sScanNumber = atoi(argv[optind+2]);
+  if(info.outDir.empty()) info.outDir = info.dir;
+  else if(info.outDir.compare(info.outDir.size()-1, 1, "/") != 0) info.outDir += "/";
+}
+
+void informationDescription(information info){
+  cout<<"program parameters are:"<<endl;
+  cout<<endl;
+  cout<<"local time: "<<info.local_time<<endl;
+  cout<<"input dir: "<<info.dir<<endl;
+  cout<<"output dir: "<<info.outDir<<endl;
+  cout<<"first scan number: "<<info.fScanNumber<<endl;
+  cout<<"second scan number: "<<info.sScanNumber<<endl;
+  cout<<"scan format: "<<scanFormatToString(info.sFormat)<<endl;
+  cout<<endl;
+  cout<<"image width: "<<info.iWidth<<endl;
+  cout<<"image height: "<<info.iHeight<<endl;
+  cout<<"number of images: "<<info.nImages<<endl;
+  cout<<"projection parameter: "<<info.pParam<<endl;
+  cout<<"projection method: "<<projectionMethodToString(info.pMethod)<<endl;
+  cout<<endl;
+  cout<<"feature detector method: "<<featureDetectorMethodToString(info.fMethod)<<endl;
+  cout<<"feature descriptor method: "<<featureDescriptorMethodToString(info.dMethod)<<endl;
+  cout<<endl;
+  cout<<"matcher parameter: "<<info.mParam<<endl;
+  cout<<"matcher method: "<<matcherMethodToString(info.mMethod)<<endl;
+  cout<<endl;
+  cout<<"min distacne: "<<info.minDistance<<endl;
+  cout<<"min error: "<<info.minError<<endl;
+  cout<<"min inlier: "<<info.minInlier<<endl;
+  cout<<"registration method: "<<registrationMethodToString(info.rMethod)<<endl;
+  cout<<endl;
+}
+
+void info_yml(information info, double bError, double bErrorIdx, double* bAlign){
+  cv::Mat align(16, 1, CV_32FC(1), cv::Scalar::all(0));
+  for(int i = 0 ; i < 16 ; i++)
+    align.at<float>(i,0) = bAlign[i];
+  
+  string yml;
+  yml = info.outDir+"fbr-yml.yml";
+  cv::FileStorage fs(yml.c_str(), cv::FileStorage::APPEND);
+  fs << "feature_bas_registration" << "{";
+  
+  fs << "pair" << "{" << "scan" << to_string(info.fScanNumber, 3);
+  fs << "scan" << to_string(info.sScanNumber, 3) << "}";
+  
+  fs << "time" << "{" << "local_time" << info.local_time << "}";
+  
+  fs << "param" << "{";
+  fs << "DIR" << info.dir;
+  fs << "sFormat" << scanFormatToString(info.sFormat);
+  fs << "pMethod" << projectionMethodToString(info.pMethod);
+  fs << "nImage" << info.nImages;
+  fs << "pParam" << info.pParam;
+  fs << "iWidth" << info.iWidth;
+  fs << "iHeight" << info.iHeight;
+  fs << "fMethod" << featureDetectorMethodToString(info.fMethod);
+  fs << "dMethod" << featureDescriptorMethodToString(info.dMethod);
+  fs << "mMethod" << matcherMethodToString(info.mMethod);
+  fs << "mParam" << info.mParam;
+  fs << "rMethod" << registrationMethodToString(info.rMethod);
+  fs << "minDistance" << info.minDistance;
+  fs << "minInlier" << info.minInlier;
+  fs << "minError" << info.minError;
+  fs << "}";
+
+  fs << "input" << "{";
+  fs << "first_input" << "{";
+  fs << "name" << "{" << "scan" << to_string(info.fScanNumber, 3) << "}";
+  fs << "point" << "{" << "amount" << info.fSPoints << "time" << info.fSTime << "}";
+  fs << "projection" << "{" << "time" << info.fPTime << "}";
+  fs << "feature" << "{" << "amount" << info.fFNum << "fTime" << info.fFTime << "dTime" << info.fDTime << "}";
+  fs << "}";
+  fs << "second_input" << "{";
+  fs << "name" << "{" << "scan" << to_string(info.sScanNumber, 3) << "}";
+  fs << "point" << "{" << "amount" << info.sSPoints << "time" << info.sSTime << "}";
+  fs << "projection" << "{" << "time" << info.sPTime << "}";
+  fs << "feature" << "{" << "amount" << info.sFNum << "fTime" << info.sFTime << "dTime" << info.sDTime << "}";
+  fs << "}";
+  fs << "}";
+  
+  fs << "matches" << "{";
+  fs << "amount" << info.mNum << "filteration" << info.filteredMNum << "time" << info.mTime << "}";
+  
+  fs << "reg" << "{";
+  fs << "bestError" << bError << "bestErrorIdx" << bErrorIdx << "time" << info.rTime << "bAlign" << align << "}";
+
+  fs << "}";
+}
+
+int main(int argc, char** argv){
+  string out;
+  cv::Mat outImage;
+  parssArgs(argc, argv, info);
+  if(info.verbose >= 1) informationDescription(info);
+
+  scan_cv fScan (info.dir, info.fScanNumber, info.sFormat);
+  if(info.verbose >= 4) info.fSTime = (double)cv::getTickCount();
+  fScan.convertScanToMat();
+  if(info.verbose >= 4) info.fSTime = ((double)cv::getTickCount() - info.fSTime)/cv::getTickFrequency();
+  if(info.verbose >= 2) fScan.getDescription();
+  panorama fPanorama (info.iWidth, info.iHeight, info.pMethod, info.nImages, info.pParam);
+  if(info.verbose >= 4) info.fPTime = (double)cv::getTickCount();
+  fPanorama.createPanorama(fScan.getMatScan());
+  if(info.verbose >= 4) info.fPTime = ((double)cv::getTickCount() - info.fPTime)/cv::getTickFrequency();
+  if(info.verbose >= 2) fPanorama.getDescription();
+  //write panorama to image
+  if(info.verbose >= 1){
+    out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+".jpg";
+    imwrite(out, fPanorama.getReflectanceImage());
+  }
+  feature fFeature;
+  if(info.verbose >= 4) info.fFTime = (double)cv::getTickCount();
+  fFeature.featureDetection(fPanorama.getReflectanceImage(), info.fMethod);
+  if(info.verbose >= 4) info.fFTime = ((double)cv::getTickCount() - info.fFTime)/cv::getTickFrequency();
+  //write panorama with keypoints to image
+  if(info.verbose >= 1){
+    cv::drawKeypoints(fPanorama.getReflectanceImage(), fFeature.getFeatures(), outImage, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS );
+    out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+".jpg";
+    imwrite(out, outImage);
+    outImage.release();
+  }
+  if(info.verbose >= 4) info.fDTime = (double)cv::getTickCount();
+  fFeature.featureDescription(fPanorama.getReflectanceImage(), info.dMethod);
+  if(info.verbose >= 4) info.fDTime = ((double)cv::getTickCount() - info.fDTime)/cv::getTickFrequency();
+  if(info.verbose >= 2) fFeature.getDescription();
+  
+  scan_cv sScan (info.dir, info.sScanNumber, info.sFormat);
+  if(info.verbose >= 4) info.sSTime = (double)cv::getTickCount();
+  sScan.convertScanToMat();
+  if(info.verbose >= 4) info.sSTime = ((double)cv::getTickCount() - info.sSTime)/cv::getTickFrequency();
+  if(info.verbose >= 2) sScan.getDescription();
+  panorama sPanorama (info.iWidth, info.iHeight, info.pMethod, info.nImages, info.pParam);
+  if(info.verbose >= 4) info.sPTime = (double)cv::getTickCount();
+  sPanorama.createPanorama(sScan.getMatScan());
+  if(info.verbose >= 4) info.sPTime = ((double)cv::getTickCount() - info.sPTime)/cv::getTickFrequency();
+  if(info.verbose >= 2) sPanorama.getDescription();
+  //write panorama to image
+  if(info.verbose >= 1){
+    out = info.outDir+info.local_time+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+".jpg";
+    imwrite(out, sPanorama.getReflectanceImage());
+  }
+  feature sFeature;
+  if(info.verbose >= 4) info.sFTime = (double)cv::getTickCount();
+  sFeature.featureDetection(sPanorama.getReflectanceImage(), info.fMethod);
+  if(info.verbose >= 4) info.sFTime = ((double)cv::getTickCount() - info.sFTime)/cv::getTickFrequency();
+  //write panorama with keypoints to image
+  if(info.verbose >= 1){
+    cv::drawKeypoints(sPanorama.getReflectanceImage(), sFeature.getFeatures(), outImage, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS );
+    out = info.outDir+info.local_time+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+".jpg";
+    imwrite(out, outImage);
+    outImage.release();
+  }
+  if(info.verbose >= 4) info.sDTime = (double)cv::getTickCount();
+  sFeature.featureDescription(sPanorama.getReflectanceImage(), info.dMethod);
+  if(info.verbose >= 4) info.sDTime = ((double)cv::getTickCount() - info.sDTime)/cv::getTickFrequency();
+  if(info.verbose >= 2) sFeature.getDescription();
+ 
+  feature_matcher matcher (info.mMethod, info.mParam);
+  if(info.verbose >= 4) info.mTime = (double)cv::getTickCount();
+  matcher.match(fFeature, sFeature);
+  if(info.verbose >= 4) info.mTime = ((double)cv::getTickCount() - info.mTime)/cv::getTickFrequency();
+  if(info.verbose >= 2) matcher.getDescription();
+  //write matcheed feature to image
+  if(info.verbose >= 1){
+    cv::drawMatches(fPanorama.getReflectanceImage(), fFeature.getFeatures(), sPanorama.getReflectanceImage(), sFeature.getFeatures(), matcher.getMatches(), outImage, cv::Scalar::all(-1), cv::Scalar::all(-1), vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
+    out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+"_"+featureDescriptorMethodToString(info.dMethod)+"_"+matcherMethodToString(info.mMethod)+".jpg";
+    imwrite(out, outImage);
+    outImage.release();
+  }
+
+  registration reg (info.minDistance, info.minError, info.minInlier, info.rMethod);
+  if(info.verbose >= 4) info.rTime = (double)cv::getTickCount();
+  reg.findRegistration(fPanorama.getMap(), fFeature.getFeatures(), sPanorama.getMap(), sFeature.getFeatures(), matcher.getMatches());
+  if(info.verbose >= 4) info.rTime = ((double)cv::getTickCount() - info.rTime)/cv::getTickFrequency();
+  if(info.verbose >= 2) reg.getDescription();
+
+  //write .dat and .frames files
+  if(info.verbose >= 0){
+    double *bAlign = reg.getBestAlign();
+    
+    out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+"_"+featureDescriptorMethodToString(info.dMethod)+"_"+matcherMethodToString(info.mMethod)+"_"+registrationMethodToString(info.rMethod)+".dat";
+    ofstream dat(out.c_str());
+    dat << bAlign[0] << " " << bAlign[4] << " " << bAlign[8] << " " << bAlign[12] <<endl;
+    dat << bAlign[1] << " " << bAlign[5] << " " << bAlign[9] << " " << bAlign[13] <<endl;
+    dat << bAlign[2] << " " << bAlign[6] << " " << bAlign[10] << " " << bAlign[14] <<endl;
+    dat << bAlign[3] << " " << bAlign[7] << " " << bAlign[11] << " " << bAlign[15] <<endl;
+    dat.close();
+    
+    out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+"_"+featureDescriptorMethodToString(info.dMethod)+"_"+matcherMethodToString(info.mMethod)+"_"+registrationMethodToString(info.rMethod)+".frames";
+    ofstream frames(out.c_str());
+    for (int n = 0 ; n < 2 ; n++)
+      {
+	for(int i = 0; i < 16; i++)
+	  frames << bAlign[i] <<" ";
+	frames << "2" << endl;
+      }
+    frames.close();
+  }
+  
+  if(info.verbose >= 3){
+    info.fSPoints = fScan.getNumberOfPoints();
+    info.sSPoints = sScan.getNumberOfPoints();
+    info.fFNum = fFeature.getNumberOfFeatures();
+    info.sFNum = sFeature.getNumberOfFeatures();
+    info.mNum = matcher.getNumberOfMatches();
+    info.filteredMNum = matcher.getNumberOfFilteredMatches();
+    
+    info_yml(info, reg.getBestError(), reg.getBestErrorIndex(), reg.getBestAlign());
+  }
+  
+  return 0;
+}

.svn/pristine/84/84ddea41be6d4de3085e901211548fc9180665b1.svn-base

@@ -1,13 +1,13 @@
 /*
  * pointfilter implementation
  *
- * Copyright (C) Thomas Escher, Dorit Borrmann, Kai Lingemann
+ * Copyright (C) Dorit Borrmann, Jan Elseberg
  *
  * Released under the GPL version 3.
  *
  */
 
-#include "scanserver/pointfilter.h"
+#include "slam6d/pointfilter.h"
 
 using std::string;
 using std::map;
@@ -20,9 +20,11 @@ using std::runtime_error;
 using std::cout;
 using std::endl;
 
+#include <cmath>
 
 
-map<string, Checker* (*)(const string&)> PointFilter::factory;
+
+map<string, Checker* (*)(const string&)>* PointFilter::factory = new map<string, Checker* (*)(const string&)>;
 
 
 
@@ -57,7 +59,7 @@ PointFilter::~PointFilter()
     delete m_checker;
 }
 
-PointFilter& PointFilter::setRange(float maxDist, float minDist)
+PointFilter& PointFilter::setRange(double maxDist, double minDist)
 {
   m_changed = true;
   stringstream s_max; s_max << maxDist;
@@ -77,6 +79,14 @@ PointFilter& PointFilter::setHeight(double top, double bottom)
   return *this;
 }
 
+PointFilter& PointFilter::setRangeMutator(double range)
+{
+  m_changed = true;
+  stringstream s_range; s_range << range;
+  m_params["rangemutation"] = s_range.str();
+  return *this;
+}
+
 std::string PointFilter::getParams()
 {
   stringstream s;
@@ -97,7 +107,7 @@ void PointFilter::createCheckers()
   // create new ones
   Checker** current = &m_checker;
   for(map<string, string>::iterator it = m_params.begin(); it != m_params.end(); ++it) {
-    *current = factory[it->first](it->second);
+    *current = (*factory)[it->first](it->second);
     // if a Checker has been successfully created advance to its pointer in the chain
     if(*current) {
       current = &((*current)->m_next);
@@ -126,6 +136,7 @@ namespace {
   CheckerFactory<CheckerRangeMin> min("rangemin");
   CheckerFactory<CheckerHeightTop> top("heighttop");
   CheckerFactory<CheckerHeightBottom> bottom("heightbottom");
+  CheckerFactory<RangeMutator> range_mutation("rangemutation");
 }
 
 
@@ -185,3 +196,18 @@ bool CheckerHeightBottom::test(double* point) {
     return true;
   return false;
 }
+
+RangeMutator::RangeMutator(const std::string& value) {
+  stringstream s(value);
+  s >> m_range;
+}
+
+bool RangeMutator::test(double* point) {
+  double orig_range = sqrt(point[0]*point[0] + point[1]*point[1] + point[2]*point[2]);
+  double scale_mutation = m_range / orig_range;
+  point[0] *= scale_mutation;
+  point[1] *= scale_mutation;
+  point[2] *= scale_mutation;
+
+  return true;
+}

.svn/pristine/88/88d7ba7604baa0151912d115b5883d7847f0a39d.svn-base

@@ -0,0 +1,203 @@
+/*
+ * fbr_global implementation
+ *
+ * Copyright (C) HamidReza Houshiar
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+#include "slam6d/fbr/fbr_global.h"
+#include <stdexcept>
+
+namespace fbr{
+  string scanFormatToString(IOType format){
+    return io_type_to_libname(format);
+  }
+
+  IOType stringToScanFormat(string format){
+    return formatname_to_io_type(format.c_str());
+  }
+  
+  string projectionMethodToString(projection_method method){
+    string sMethod;
+    switch(method){
+    case EQUIRECTANGULAR:
+      sMethod = "EQUIRECTANGULAR";
+      break;
+    case CYLINDRICAL:
+      sMethod = "CYLINDRICAL";
+      break;
+    case MERCATOR:
+      sMethod = "MERCATOR";
+      break;
+    case RECTILINEAR:
+      sMethod = "RECTILINEAR";
+      break;
+    case PANNINI:
+      sMethod = "PANNINI";
+      break;
+    case STEREOGRAPHIC:
+      sMethod = "STEREOGRAPHIC";
+      break;
+    case ZAXIS:
+      sMethod = "ZAXIS";
+      break;
+    case CONIC:
+      sMethod = "CONIC";
+      break;
+    default:
+      throw std::runtime_error(std::string("projection method ") + to_string(method) + std::string(" could not be matched to a projection method"));
+    }
+    return sMethod;
+  }
+  
+  projection_method stringToProjectionMethod(string method){
+    if(strcasecmp(method.c_str(), "EQUIRECTANGULAR") == 0) return EQUIRECTANGULAR;
+    else if(strcasecmp(method.c_str(), "CYLINDRICAL") == 0) return CYLINDRICAL;
+    else if(strcasecmp(method.c_str(), "MERCATOR") == 0) return MERCATOR;
+    else if(strcasecmp(method.c_str(), "RECTILINEAR") == 0) return RECTILINEAR;
+    else if(strcasecmp(method.c_str(), "PANNINI") == 0) return PANNINI;
+    else if(strcasecmp(method.c_str(), "STEREOGRAPHIC") == 0) return STEREOGRAPHIC;
+    else if(strcasecmp(method.c_str(), "ZAXIS") == 0) return ZAXIS;
+    else if(strcasecmp(method.c_str(), "CONIC") == 0) return CONIC;
+    else throw std::runtime_error(std::string("projection method ") + method + std::string(" is unknown"));
+  }
+  
+  string panoramaMapMethodToString(panorama_map_method method){
+    string sMethod;
+    switch(method){
+    case FARTHEST:
+      sMethod = "FARTHEST";
+      break;
+    case EXTENDED:
+      sMethod = "EXTENDED";
+      break;
+    default:
+      throw std::runtime_error(std::string("panorama map method ") + to_string(method) + std::string(" could not be matched to a panorama map method"));
+    }
+    return sMethod;
+  }
+  
+  panorama_map_method stringToPanoramaMapMethod(string method){
+    if(strcasecmp(method.c_str(), "FARTHEST") == 0) return FARTHEST;
+    else if(strcasecmp(method.c_str(), "EXTENDED") == 0) return EXTENDED;
+    else throw std::runtime_error(std::string("panorama map method ") + method + std::string(" is unknown"));
+  }
+
+  string featureDetectorMethodToString(feature_detector_method method){
+    string sMethod;
+    switch(method){
+    case SIFT_DET:
+      sMethod = "SIFT_DET";
+      break;
+    case SURF_DET:
+      sMethod = "SURF_DET";
+      break;
+    case ORB_DET:
+      sMethod = "ORB_DET";
+      break;
+    case FAST_DET:
+      sMethod = "FAST_DET";
+      break;
+    case STAR_DET:
+      sMethod = "STAR_DET";
+      break;
+    default:
+      throw std::runtime_error(std::string("feature detector method ") + to_string(method) + std::string(" could not be matched to a feature detector method"));
+    }
+    return sMethod;
+  }
+  
+  feature_detector_method stringToFeatureDetectorMethod(string method){
+    if(strcasecmp(method.c_str(), "SIFT") == 0) return SIFT_DET;
+    else if(strcasecmp(method.c_str(), "SURF") == 0) return SURF_DET;
+    else if(strcasecmp(method.c_str(), "ORB") == 0) return ORB_DET;
+    else if(strcasecmp(method.c_str(), "FAST") == 0) return FAST_DET;
+    else if(strcasecmp(method.c_str(), "STAR") == 0) return STAR_DET;
+    else throw std::runtime_error(std::string("feature detector method ") + method + std::string(" is unknown"));
+  }
+  
+  string featureDescriptorMethodToString(feature_descriptor_method method){
+    string sMethod;
+    switch(method){
+    case SIFT_DES:
+      sMethod = "SIFT_DES";
+      break;
+    case SURF_DES:
+      sMethod = "SURF_DES";
+      break;
+    case ORB_DES:
+      sMethod = "ORB_DES";
+      break;
+    default:
+      throw std::runtime_error(std::string("feature descriptor method ") + to_string(method) + std::string(" could not be matched to a feature descriptor method"));
+    }
+    return sMethod;
+  }
+  
+  feature_descriptor_method stringToFeatureDescriptorMethod(string method){
+    if(strcasecmp(method.c_str(), "SIFT") == 0) return SIFT_DES;
+    else if(strcasecmp(method.c_str(), "SURF") == 0) return SURF_DES;
+    else if(strcasecmp(method.c_str(), "ORB") == 0) return ORB_DES;
+    else throw std::runtime_error(std::string("feature descriptor method ") + method + std::string(" is unknown"));
+  }
+
+  string matcherMethodToString(matcher_method method){
+    string sMethod;
+    switch(method){
+    case BRUTEFORCE:
+      sMethod = "BRUTEFORCE";
+      break;
+    case FLANN:
+      sMethod = "FLANN";
+      break;
+    case KNN:
+      sMethod = "KNN";
+      break;
+    case RADIUS:
+      sMethod = "RADIUS";
+      break;
+    case RATIO:
+      sMethod = "RATIO";
+      break;
+    default:
+      throw std::runtime_error(std::string("matcher method ") + to_string(method) + std::string(" could not be matched to a matcher method"));
+    }
+    return sMethod;
+  }
+  
+  matcher_method stringToMatcherMethod(string method){
+    if(strcasecmp(method.c_str(), "BRUTEFORCE") == 0) return BRUTEFORCE;
+    else if(strcasecmp(method.c_str(), "FLANN") == 0) return FLANN;
+    else if(strcasecmp(method.c_str(), "KNN") == 0) return KNN;
+    else if(strcasecmp(method.c_str(), "RADIUS") == 0) return RADIUS;
+    else if(strcasecmp(method.c_str(), "RATIO") == 0) return RATIO;
+    else throw std::runtime_error(std::string("matcher method ") + method + std::string(" is unknown"));
+  }
+  
+  string registrationMethodToString(registration_method method){
+    string sMethod;
+    switch(method){
+    case ALL:
+      sMethod = "ALL";
+      break;
+    case RANSAC:
+      sMethod = "RANSAC";
+      break;
+    case DISABLE:
+      sMethod = "DISABLE";
+      break;
+    default:
+      throw std::runtime_error(std::string("registration method ") + to_string(method) + std::string(" could not be matched to a registration method"));
+    }
+    return sMethod;
+  }
+
+  registration_method stringToRegistrationMethod(string method){
+    if(strcasecmp(method.c_str(), "ALL") == 0) return ALL;
+    else if(strcasecmp(method.c_str(), "RANSAC") == 0) return RANSAC;
+    else if(strcasecmp(method.c_str(), "DISABLE") == 0) return DISABLE;
+    else throw std::runtime_error(std::string("registration method ") + method + std::string(" is unknown"));
+  }
+}

.svn/pristine/8c/8c21e837ab6a20d36ac51c93d7f4ea0467c71eed.svn-base

@@ -0,0 +1,80 @@
+#ifndef MANAGED_SCAN_H
+#define MANAGED_SCAN_H
+
+#include "scan.h"
+#include "scanserver/sharedScan.h"
+
+
+
+class ManagedScan : public Scan {
+public:
+  static void openDirectory(const std::string& path, IOType type,
+    int start, int end = -1);
+  static void closeDirectory();
+
+  static std::size_t getMemorySize();
+
+  virtual ~ManagedScan();
+
+  virtual void setRangeFilter(double max, double min);
+  virtual void setHeightFilter(double top, double bottom);
+  virtual void setRangeMutation(double range);
+  
+  virtual void setReductionParameter(double voxelSize, int nrpts = 0,
+    PointType pointtype = PointType());
+  void setShowReductionParameter(double voxelSize, int nrpts = 0,
+    PointType pointtype = PointType());
+  virtual void setOcttreeParameter(double reduction_voxelSize,
+    double octtree_voxelSize, PointType pointtype,
+    bool loadOct, bool saveOct);
+
+  virtual const char* getIdentifier() const { return m_shared_scan->getIdentifier(); }
+
+  virtual DataPointer get(const std::string& identifier);
+  virtual void get(unsigned int types);
+  virtual DataPointer create(const std::string& identifier, unsigned int size);
+  virtual void clear(const std::string& identifier);
+
+  virtual unsigned int readFrames();
+  virtual void saveFrames();
+  virtual unsigned int getFrameCount();
+  virtual void getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type);
+
+protected:
+  virtual void createSearchTreePrivate();
+  virtual void calcReducedOnDemandPrivate();
+  virtual void addFrame(AlgoType type);
+
+private:
+  //! Reference to the shared scan
+  SharedScan* m_shared_scan;
+
+  //! SharedScan vector to be deleted on closeDirectory
+  static SharedScanVector* shared_scans;
+
+  //! Flag to keep track of whether we have to update/create reduced points at the start
+  bool m_reduced_ready;
+
+  //! Flag to reset the persistent frames on write actions in slam to avoid a clear call
+  bool m_reset_frames_on_write;
+
+  //! Voxelsize of the octtree used for reduction
+  double show_reduction_voxelSize;
+
+  //! Which point to take out of the reduction octtree, 0 for center
+  int show_reduction_nrpts;
+
+  //! Pointtype used for the reduction octtree
+  PointType show_reduction_pointtype;
+
+
+  ManagedScan(SharedScan* shared_scan);
+
+  //! Create reduced points for show
+  void calcReducedShow();
+
+  //! Create Octtree for show
+  void createOcttree();
+};
+
+#endif //MANAGED_SCAN_H

.svn/pristine/92/92cd0adc086dceffc013da8326fde6266309c8b1.svn-base

@@ -29,8 +29,9 @@ public:
   PointFilter(const std::string& params);
   ~PointFilter();
 
-  PointFilter& setRange(float maxDist, float minDist);
+  PointFilter& setRange(double maxDist, double minDist);
   PointFilter& setHeight(double top, double bottom);
+  PointFilter& setRangeMutator(double range);
 
   //! Serialization function to convert it into a string, usable in the constructor
   std::string getParams();
@@ -52,7 +53,7 @@ private:
 
   // factory magic
   template<typename T> friend struct CheckerFactory;
-  static std::map<std::string, Checker* (*)(const std::string&)> factory;
+  static std::map<std::string, Checker* (*)(const std::string&)>* factory;
 };
 
 class Checker {
@@ -71,7 +72,7 @@ public:
 template<typename T>
 struct CheckerFactory {
   //! Instanciate in the source code with the to be created class as template argument and associated key string as constructor argument
-  CheckerFactory(const std::string& key) { PointFilter::factory[key] = CheckerFactory<T>::create; }
+  CheckerFactory(const std::string& key) { (*PointFilter::factory)[key] = CheckerFactory<T>::create; }
   //! Automated create function, safely returning a nullpointer if the constructor throws for unwanted values
   static Checker* create(const std::string& value) { try { return new T(value); } catch(...) { return 0; } }
 };
@@ -108,6 +109,14 @@ private:
   double m_bottom;
 };
 
-#include "scanserver/pointfilter.icc"
+class RangeMutator : public Checker {
+public:
+  RangeMutator(const std::string& value);
+  virtual bool test(double* point);
+private:
+  double m_range;
+};
+
+#include "pointfilter.icc"
 
 #endif //POINT_FILTER_H

.svn/pristine/b2/b2d00fe98f5c6bb8763ce27e98d517118863f239.svn-base

@@ -0,0 +1,416 @@
+#ifndef SCAN_H
+#define SCAN_H
+
+#include "io_types.h"
+#include "data_types.h"
+#include "point_type.h"
+#include "ptpair.h"
+
+#include <string>
+#include <vector>
+
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/locks.hpp>
+
+//! SearchTree types
+enum nns_type {
+  simpleKD, ANNTree, BOCTree
+};
+
+class Scan;
+typedef std::vector<Scan*> ScanVector;
+
+class SearchTree;
+class ANNkd_tree;
+
+/** HOWTO scan
+First: Load scans (if you want to use the scanmanager, use ManagedScan)
+
+  BasicScan/ManagedScan::openDirectory(path, type, start, end);
+
+Pass it to functions (by reference to link it to the same instance) or store it in a global variable
+
+After loading you might want to set parameters
+
+  for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) {
+    Scan* scan = *it;
+    scan->setRangeFilter(maxDist, minDist);
+    scan->setHeightFilter(top, bottom); // thermo
+    scan->setReductionParameter(voxelSize, nrpts[, pointtype]);
+    scan->setSearchTreeParameter(nns_method, use_cuda);
+  }
+
+Access the contained data, will be loaded and calculated on demand
+
+  DataXYZ xyz = scan->get("xyz");
+  DataXYZ reduced = scan->get("xyz reduced");
+  DataRGB rgb = scan->get("rgb");
+  
+  xyz[i][0..2]
+  reflectance[i]
+  
+  unsigned int size = scan->size("xyz reduced");
+
+In order to use the prefetching of all requested data field in the scanserver, mark them for use. This is relevant for efficiency, which would otherwise cause loading the files each time another data field is requested.
+
+  scan->get(DATA_XYZ | DATA_RGB | ...);
+
+Under circumstances the data fields are not available (e.g. no color in uos-type scans)
+
+  DataRGB rgb = scan->get("rgb");
+  if(rgb.valid()) { ok, do something }
+  
+If backward-compability to pointer arrays is needed, the PointerArray class can adapt
+
+  BOctTree(PointerArray(scan->get("xyz")).get(), scan->size("xyz"), ...);
+
+If data isn't needed anymore, flag it for removal
+
+  scan->clear("xyz");
+  scan->clear(DATA_XYZ | DATA_RGB | ...);
+  
+Creating data fields with the correct byte size
+
+  scan->create("xyz somethingelse", sizeof(double)*3*N);
+
+Reading frames in show:
+
+  unsigned int size = scan->readFrames();
+  
+  const double* pose;
+  AlgoType type;
+  scan->getFrame(i, pose, type);
+
+Last, if program ends, clean up
+
+  Scan::closeDirectory(scans);
+
+**/
+
+
+
+/**
+ * This class bundles common features of different scan implementations by
+ * abstraction. It handles the algorithmic parts and leaves IO and other
+ * features to the deriving classes.
+ */
+class Scan {
+  //friend class SearchTree; // TODO: is this neccessary?
+public:
+  enum AlgoType {
+    INVALID, ICP, ICPINACTIVE, LUM, ELCH, LOOPTORO, LOOPHOGMAN, GRAPHTORO,
+    GRAPHHOGMAN
+  };
+  
+  // delete copy-ctor and assignment, scans shouldn't be copied by basic class
+  //  Scan(const Scan& other) = delete;
+  //  Scan& operator=(const Scan& other) = delete;
+  
+  virtual ~Scan();
+
+  //! Holder of all scans - also used in transform for adding frames for each scan at the same time
+  static std::vector<Scan*> allScans;
+  
+  /**
+    * Attempt to read a directory under \a path and return its read scans.
+    * No scans are loaded at this point, only checked if all exist.
+    * 
+    * @param scanserver whether to use managed scans in the scanserver or not
+    * @param path to the directory containing the scans
+    * @param type determining which ScanIO to use
+    * @param start first scan to use
+    * @param end last scan to use, -1 means from start to last available
+    */
+  static void openDirectory(bool scanserver, const std::string& path, IOType type,
+    int start, int end = -1);
+  
+  /**
+   * "Close" a directory by deleting all its scans and emptying the
+   * Scan::allScans vector.
+   */
+  static void closeDirectory();
+  
+  
+  /* Input filtering and parameter functions */
+  
+  
+  //! Input filtering for all points based on their euclidean length
+  virtual void setRangeFilter(double max, double min) = 0;
+  
+  //! Input filtering for all points based on their height
+  virtual void setHeightFilter(double top, double bottom) = 0;
+
+  //! Input mutation to set range of all points to a constant value;
+  virtual void setRangeMutation(double range) { }
+  
+  //! Set reduction parameters, but don't reduce yet
+  virtual void setReductionParameter(double voxelSize, int nrpts = 0,
+    PointType pointtype = PointType());
+  
+  //! Set SearchTree type, but don't create it yet
+  void setSearchTreeParameter(int nns_method, bool cuda_enabled);
+  
+  /**
+   * Set octtree parameters for show
+   * @param loadOct will load the serialized octtree from disk regardless
+   * @param saveOct serialize octtree if not loaded by loadOct after creation
+   */
+  virtual void setOcttreeParameter(double reduction_voxelSize,
+    double octtree_voxelSize, PointType pointtype,
+    bool loadOct, bool saveOct);
+
+
+  /* Basic getter functions */
+
+
+  inline const double* get_rPos() const;
+  inline const double* get_rPosTheta() const;
+  inline const double* get_rPosQuat() const;
+  //! Pose matrix after initial and match transformations (org+dalign)
+  inline const double* get_transMat() const;
+  //! Original pose matrix after initial transform
+  inline const double* get_transMatOrg() const;
+  //! Accumulated delta transformation matrix 
+  inline const double* getDAlign() const;
+  
+  inline SearchTree* getSearchTree();
+  inline ANNkd_tree* getANNTree() const;
+  
+  virtual const char* getIdentifier() const = 0;
+  
+  //! Determine the maximum number of reduced points in \a scans
+  static unsigned int getMaxCountReduced(ScanVector& scans);
+  
+  
+  /* Functions for altering data fields, implementation specific */
+  
+  
+  /**
+   * Get the data field \a identifier, calculate it on demand if neccessary.
+   *
+   * If "xyz reduced" or "xyz reduced original" is requested, the reduction is
+   * started with "xyz" as input.
+   */
+  virtual DataPointer get(const std::string& identifier) = 0;
+
+  /**
+   * Load the requested IODataTypes, joined by |, from the scan file.
+   *
+   * This feature is neccessary to load multiple data fields at once, not all
+   * one by one with each get("...") access.
+   */
+  virtual void get(unsigned int types) = 0;
+  
+  /**
+   * Creates a data field \a identifier with \a size bytes.
+   */
+  virtual DataPointer create(const std::string& identifier, unsigned int size) = 0;
+  
+  /**
+   * Clear the data field \a identifier, removing its allocated memory if
+   * possible or marking it for prioritized removal.
+   */
+  virtual void clear(const std::string& identifier) = 0;
+
+  //! Extension to clear for more than one identifier, e.g. clear(DATA_XYZ | DATA_RGB);
+  void clear(unsigned int types);
+  
+  /**
+   * Get the size of \a identifier as if it were requested and size() called
+   * upon its type specialized DataPointer class.
+   * e.g size<DataXYZ>("xyz reduced")
+   */
+  template<typename T>
+  unsigned int size(const std::string& identifier) {
+    return (T(get(identifier))).size();
+  }
+
+
+  /* Frame handling functions */
+
+
+  /**
+   * Open the .frames-file and read its contents. If not read, the frame list
+   * will be empty.
+   * @return count of frames if file has been read, zero otherwise
+   */
+  virtual unsigned int readFrames() = 0;
+  
+  /**
+   * Write the accumulated frames into a .frames-file.
+   */
+  virtual void saveFrames() = 0;
+  
+  //! Count of frames
+  virtual unsigned int getFrameCount() = 0;
+  
+  //! Get contents of a frame, pass matrix pointer and type by reference
+  virtual void getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type) = 0;
+  
+protected:
+  /**
+   * Called from transform, this will add its current transMat pose with
+   * the given type as a frame into the list of frames
+   */
+  virtual void addFrame(AlgoType type) = 0;
+  
+public:
+  
+  
+  /* Direct creation of reduced points and search tree */
+  
+  
+  //! Apply reduction and initial transMatOrg transformation
+  void toGlobal();
+  
+  //! Copy reduced points to original and create search tree on it
+  void createSearchTree();
+
+
+  /* Common transformation and matching functions */
+  void mergeCoordinatesWithRoboterPosition(Scan* prevScan);
+  void transformAll(const double alignxf[16]);
+  void transformAll(const double alignQuat[4], const double alignt[3]);
+  
+  void transform(const double alignxf[16],
+    const AlgoType type, int islum = 0);
+  void transform(const double alignQuat[4],
+    const double alignt[3], const AlgoType type, int islum = 0);
+  void transformToMatrix(double alignxf[16],
+    const AlgoType type, int islum = 0);
+  void transformToEuler(double rP[3], double rPT[3],
+    const AlgoType type, int islum = 0);
+  void transformToQuat(double rP[3], double rPQ[4],
+    const AlgoType type, int islum = 0);
+  
+  // Scan matching functions
+  static void getPtPairs(std::vector<PtPair> *pairs,
+    Scan* Source, Scan* Target,
+    int thread_num,
+    int rnd, double max_dist_match2, double &sum,
+    double *centroid_m, double *centroid_d);
+  static void getNoPairsSimple(std::vector<double*> &diff,
+    Scan* Source, Scan* Target,
+    int thread_num,
+    double max_dist_match2);
+  static void getPtPairsSimple(std::vector<PtPair> *pairs,
+    Scan* Source, Scan* Target,
+    int thread_num,
+    int rnd, double max_dist_match2,
+    double *centroid_m, double *centroid_d);
+  static void getPtPairsParallel(std::vector<PtPair> *pairs,
+    Scan* Source, Scan* Target,
+    int thread_num, int step,
+    int rnd, double max_dist_match2,
+    double *sum,
+    double centroid_m[OPENMP_NUM_THREADS][3],
+    double centroid_d[OPENMP_NUM_THREADS][3]);
+
+protected:
+  /**
+   * The pose of the scan
+   * Note: rPos/rPosTheta and transMat _should_
+   *       always represent the same pose!!!
+   */
+  double rPos[3],    //!< 3D position
+    rPosTheta[3],    //!< 3D rotation in Euler representation 
+    rQuat[4],        //!< 3D rotation in Quaternion representation
+    transMat[16],    //!< (4x4) transformation matrix
+    transMatOrg[16]; //!< The original pose of the scan, e.g., from odometry
+
+  /**
+   * The dalignxf transformation represents the delta transformation virtually applied
+   * to the tree and is used to compute are actual corresponding points.
+   */
+  double dalignxf[16];
+
+  //! Run ICP on GPU instead of CPU
+  bool cuda_enabled;
+  
+  //! Defines the method used for nearest neighbor search and which tree to use
+  int nns_method;
+  
+  //! SearchTree for point pair matching, works on the search points
+  SearchTree* kd;
+  
+  //! This KD tree is created only for the CUDA usages
+  ANNkd_tree* ann_kd_tree;
+  
+  //! Voxelsize of the octtree used for reduction
+  double reduction_voxelSize;
+  
+  //! Which point to take out of the reduction octtree, 0 for center
+  int reduction_nrpts;
+  
+  //! Pointtype used for the reduction octtree
+  PointType reduction_pointtype;
+  
+  //! Type of the searchtree to be created
+  int searchtree_nnstype;
+  
+  //! Use CUDA for searching
+  bool searchtree_cuda_enabled;
+
+  //! Flag whether "xyz reduced" has been initialized for this Scan yet
+  bool m_has_reduced;
+  
+  //! Reduction value used for octtree input
+  double octtree_reduction_voxelSize;
+  
+  //! Voxelsize used in the octtree itself
+  double octtree_voxelSize;
+  
+  //! Pointtype for the Octtree
+  PointType octtree_pointtype;
+  
+  //! Flags to load or save the octtrees from/to storage
+  bool octtree_loadOct, octtree_saveOct;
+  
+  /**
+   * Basic initializing constructor calling the initalization function.
+   * Can only be called from deriving classes.
+   */
+  Scan();
+  
+  /**
+   * This function handles the reduction of points. It builds a lock for
+   * multithread-safety and calls caldReducedOnDemandPrivate.
+   *
+   * The intention is to reduce points, transforme them to the initial pose and
+   * then copy them to original for the SearchTree.
+   */
+  void calcReducedOnDemand();
+
+  //! Create specific SearchTree variants matching the capability of the Scan
+  virtual void createSearchTreePrivate() = 0;
+  
+  //! Create reduced points in a multithread-safe environment matching the capability of the Scan
+  virtual void calcReducedOnDemandPrivate() = 0;
+
+  //! Internal function of transform which alters the reduced points
+  void transformReduced(const double alignxf[16]);
+  
+  //! Internal function of transform which handles the matrices
+  void transformMatrix(const double alignxf[16]);
+  
+  //! Creating reduced points
+  void calcReducedPoints();
+  
+  //! Copies reduced points to original points without any transformation.
+  void copyReducedToOriginal();
+  
+  //! Inverse functionality of copyReducedToOriginal.
+  void copyOriginalToReduced();
+
+private:
+  //! flag for openDirectory and closeDirectory to distinguish the scans
+  static bool scanserver;
+
+  //! Mutex for safely reducing points and creating the search tree just once in a multithreaded environment  
+  // it can not be compiled  in wein32 use boost 1.48, therefore we remeove it  temporarily
+//  boost::mutex m_mutex_reduction, m_mutex_create_tree;
+};
+
+#include "scan.icc"
+
+#endif //SCAN_H

.svn/pristine/d6/d6a5a93ffea0d6a4ad66c8e0a5793f94c07f21ac.svn-base

@@ -0,0 +1,196 @@
+/**
+ * @file
+ * @brief Scan containing all interprocess shared data and management values.
+ *
+ * @author Thomas Escher
+ */
+
+#ifndef SHARED_SCAN_H
+#define SHARED_SCAN_H
+
+#include "scanserver/frame.h"
+#include "slam6d/io_types.h"
+#include "slam6d/data_types.h"
+#include "slam6d/pointfilter.h"
+
+#include <boost/interprocess/managed_shared_memory.hpp>
+#include <boost/interprocess/smart_ptr/shared_ptr.hpp>
+#include <boost/interprocess/containers/string.hpp>
+#include <boost/interprocess/containers/vector.hpp>
+
+// hide the boost namespace and shorten others
+namespace
+{
+  namespace ip = boost::interprocess;
+  // the segment manager providing the allocations
+  typedef ip::managed_shared_memory::segment_manager SegmentManager;
+}
+
+// allocator and type for a scan vector
+class SharedScan;
+typedef ip::allocator<ip::offset_ptr<SharedScan>, SegmentManager> SharedScanAllocator;
+typedef ip::vector<ip::offset_ptr<SharedScan>, SharedScanAllocator> SharedScanVector;
+
+// allocator and type for a shared string
+typedef ip::allocator<char, SegmentManager> CharAllocator;
+typedef ip::basic_string<char, std::char_traits<char>, CharAllocator> SharedString;
+
+// shared pointer for shared strings
+typedef ip::managed_shared_ptr<SharedString, ip::managed_shared_memory>::type SharedStringSharedPtr;
+
+class CacheObject;
+
+
+
+/**
+ * @brief Central class representing a single scan file in shared memory.
+ *
+ * This class identifies a scan file by its path, identificator and io type, which are given to the ScanIO to read a scan.
+ * It holds all neccessary information like pose, frames and cached data from the scan. The cached data is contained in CacheObjects, one for each data channel and two reduced (one transformed, one untransformed). The access to the CacheObjects' data is given via a derivation from CacheData via MultiArray-types, which imitates a double**-array for easy use.
+ * All calls to the server are relayed to the ClientInterface and handled there. Access to the CacheObjects causing a cache miss also invoke a server call to the CacheManager.
+ */
+class SharedScan
+{
+  // private static class access for onChacheMiss/onAllocation
+  friend class CacheObject;
+public:
+  //! Constructor by identifiers
+  SharedScan(const ip::allocator<void, SegmentManager> & allocator,
+    const SharedStringSharedPtr& dir_path_ptr, const char* io_identifier,
+    IOType iotype);
+
+  //! Deconstructor
+  ~SharedScan();
+
+  //! Equality operator based on identifier, directory and type
+  bool operator==(const SharedScan& r) const;
+  
+  //! Filter parameters for range checks when loading from file, invalidate cache for scan CacheObject if it differs
+  void setRangeParameters(double max_dist, double min_dist);
+
+  //! Filter parameters for height checks when loading from file, invalidate cache for scan CacheObject if it differs
+  void setHeightParameters(double top, double bottom);
+
+  //! Filter parameters for the range mutator for showing the spheres, invalidate cache for scan CacheObject if it differs
+  void setRangeMutationParameters(double range);
+    
+  //! Set parameters and invalidate cache for reduced CacheObjects if it differs
+  void setReductionParameters(const char* params);
+
+  //! Set parameters and invalidate cache for reduced CacheObjects if it differs
+  void setShowReductionParameters(const char* params);
+
+  //! Set parameters and invalidate cache for reduced CacheObjects if it differs
+  void setOcttreeParameters(const char* params);
+
+  //! Add a new frame with the current transformation and given type
+  void addFrame(double* transformation, unsigned int type);
+  
+  //! Save frames into a file for later use
+  void saveFrames();
+
+  //! Clear existing frames
+  void clearFrames();
+  
+  //! Get contained frames
+  const FrameVector& getFrames();
+  
+  //! Get pose from pose file
+  double* getPose();
+  
+  //! Get specific cached data
+  DataXYZ getXYZ();
+  DataRGB getRGB();
+  DataReflectance getReflectance();
+  DataAmplitude getAmplitude();
+  DataType getType();
+  DataDeviation getDeviation();
+  
+  //! Reduced transformed points
+  DataXYZ getXYZReduced();
+  
+  //! Create a new set of reduced points
+  DataXYZ createXYZReduced(unsigned int size);
+  
+  //! Reduced untransformed points
+  DataXYZ getXYZReducedOriginal();
+  
+  //! Create a new set of reduced points originals
+  DataXYZ createXYZReducedOriginal(unsigned int size);
+  
+  //! Individual reduced points to use in show if requested
+  TripleArray<float> getXYZReducedShow();
+  
+  //! Create a new set of reduced points for use in show
+  TripleArray<float> createXYZReducedShow(unsigned int size);
+  
+  //! Cached tree structure for show
+  DataPointer getOcttree();
+  
+  //! Create a cached tree structure for show
+  DataPointer createOcttree(unsigned int size);
+  
+  //! ScanHandler related prefetching values to combine loading of separate cache objects
+  void prefetch(unsigned int type) { m_prefetch |= type; }
+  
+  //! Return prefetch values
+  unsigned int getPrefetch() const { return m_prefetch; }
+  
+  //! Clear prefetch values
+  void clearPrefetch() { m_prefetch = 0; }
+  
+  // IO-specific getters
+  inline const char* getDirPath() const { return m_dir_path_ptr->c_str(); }
+  inline const char* getIdentifier() const { return m_io_identifier.c_str(); }
+  inline IOType getIOType() const { return m_iotype; }
+  inline float getMaxDist() const { return m_max_dist; }
+  inline float getMinDist() const { return m_min_dist; }
+  inline double getRangeMutator() const { return m_range_mutator_param; }
+  inline double getHeightTop() const { return m_height_top; }
+  inline double getHeightBottom() const { return m_height_bottom; }
+  
+  //! Assembles an PointFilter with range/height parameters (if set) to use process-locally
+  PointFilter getPointFilter() const;
+
+private:
+  SharedStringSharedPtr m_dir_path_ptr;
+  SharedString m_io_identifier;
+  IOType m_iotype;
+  unsigned int m_prefetch;
+  double m_max_dist, m_min_dist;
+  double m_height_top, m_height_bottom;
+  double m_range_mutator_param;
+  bool m_range_mutator_param_set, m_range_param_set, m_height_param_set;
+  SharedString m_reduction_parameters;
+  SharedString m_show_parameters;
+  SharedString m_octtree_parameters;
+  bool m_load_frames_file;
+
+protected:
+  ip::offset_ptr<double> m_pose;
+  ip::offset_ptr<CacheObject> m_xyz, m_rgb, m_reflectance, m_amplitude, m_type, m_deviation,
+    m_xyz_reduced, m_xyz_reduced_original,
+    m_show_reduced, m_octtree;
+  FrameVector m_frames;
+
+  //! invalidate full cache objects
+  void invalidateFull();
+
+  //! invalidate reduced cache objects
+  void invalidateReduced();
+
+  //! invalidate show related cache objects
+  void invalidateShow();
+
+private:
+  //! Static callback for cache misses to send a request to the server interface
+  static void onCacheMiss(CacheObject* obj);
+
+  //! Static callback for cache object creation calls
+  static void onAllocation(CacheObject* obj, unsigned int size);
+
+  //! Static callback for cache object invalidation
+  static void onInvalidation(CacheObject* obj);
+};
+
+#endif //SHARED_SCAN_H

.svn/pristine/e4/e4d8522facb6f6415298f42185b70da086fb4a32.svn-base

@@ -0,0 +1,451 @@
+/*
+ * show_menu implementation
+ *
+ * Copyright (C) Kai Lingemann, Andreas Nuechter
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+/**
+ * @file
+ * @brief Functions for the menu panels of the viewer software
+ * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
+ * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
+ */
+#include "show/colormanager.h"
+
+// GUI variables
+
+GLUI *glui1,  ///< pointer to the glui window(s)
+     *glui2;  ///< pointer to the glui window(s)
+
+/** GLUI spinner for the fog */
+GLUI_Spinner    *fog_spinner;
+
+/** GLUI spinner for the point size */
+GLUI_Spinner    *ps_spinner;
+
+/** GLUI spinner for the current angle */
+GLUI_Spinner    *cangle_spinner;
+/** GLUI spinner for the current angle */
+GLUI_Spinner    *pzoom_spinner;
+/** GLUI spinner for the factor for the image size */
+GLUI_Spinner    *image_spinner;
+/** GLUI_Spinner for the depth to select groups of points */
+GLUI_Spinner    *depth_spinner;
+GLUI_Spinner    *brushsize_spinner;
+GLUI_Spinner    *frame_spinner;
+GLUI_Spinner    *fps_spinner;
+GLUI_Spinner    *farplane_spinner;
+GLUI_Spinner    *nearplane_spinner;
+GLUI_Spinner    *lod_spinner;
+
+int window_id_menu1, ///< menue window ids
+    window_id_menu2; ///< menue window ids
+
+/** User IDs for callbacks */
+#define BOX_ID               201
+/** User IDs for callbacks */
+#define ENABLE_ID            300
+/** User IDs for callbacks */
+#define DISABLE_ID           301
+/** User IDs for callbacks */
+#define SHOW_ID              302
+/** User IDs for callbacks */
+#define HIDE_ID              303
+
+/** Pointer to the panels */
+GLUI_Panel      *ground_panel;
+/** Pointer to the panels */
+GLUI_Panel      *points_panel;
+/** Pointer to the panels */
+GLUI_Panel      *wireframe_panel;
+/** Pointer to the panels */
+GLUI_Panel      *path_panel;
+/** Pointer to the panels */
+GLUI_Panel      *pose_panel;
+/** Pointer to the panels */
+GLUI_Panel      *selection_panel;
+/** Pointer to the panels */
+GLUI_Panel      *color_panel;
+/** Pointer to the panels */
+GLUI_Panel      *camera_panel;
+/** Pointer to the panels */
+GLUI_Panel      *nav_panel;
+/** Pointer to the panels */
+GLUI_Panel      *mode_panel;
+/** Pointer to the panels */
+GLUI_Panel      *settings_panel;
+/** Pointer to the panels */
+GLUI_Panel      *advanced_panel;
+/** Pointer to the button */
+GLUI_Button     *button1;
+/** Pointer to the edit text box*/
+GLUI_EditText *path_filename_edit;
+/** Pointer to the edit text box*/
+GLUI_EditText *pose_filename_edit;
+/** Pointer to the edit text box*/
+GLUI_EditText *selection_filename_edit;
+
+/** Pointer to the rotation button */
+GLUI_Rotation  *rotButton;
+
+/** used for GLUI menue */
+float obj_pos_button[3];
+/** used for GLUI menue */
+GLfloat view_rotate_button[16];
+/** used for GLUI menue */
+GLfloat obj_pos_button_old[3]; 
+
+/** used for GLUI menue */
+GLfloat X_button;
+/** used for GLUI menue */
+GLfloat Y_button;
+/** used for GLUI menue */
+GLfloat Z_button;
+/** GLUI spinner for choosing the camera */
+GLUI_Spinner    *cam_spinner;
+/** GLUI spinner for choosing the animation delay */
+GLUI_Spinner    *anim_spinner;
+/** Panel for the camera controls **/
+GLUI_Panel *cam_panel;
+/** ListBox for choosing which value to map to a color  */
+GLUI_Listbox    *value_listbox;
+/** ListBox for choosing which color map to use  */
+GLUI_Listbox    *colormap_listbox;
+GLUI_Spinner    *mincol_spinner;
+GLUI_Spinner    *maxcol_spinner;
+
+/** Checkboxes for changing point display mode **/
+GLUI_Checkbox *always_box;
+GLUI_Checkbox *never_box;
+/** Checkbox for changing interpolation mode **/
+GLUI_Checkbox *interpol_box;
+
+/**
+ * Generate the menu for the application.
+ * It consists of control and selection menus.
+ */
+void newMenu()
+{
+  /*** Create the bottom subwindow ***/
+  glui2 = GLUI_Master.create_glui("3D_Viewer - Controls");
+  window_id_menu2 = glui2->get_glut_window_id();
+  glutSetWindow(window_id_menu2);
+
+  glutPositionWindow(START_X, START_Y + START_HEIGHT + 65);
+  glutSetWindow(window_id);
+  glui2->set_main_gfx_window( window_id );
+
+  settings_panel = glui2->add_panel("Settings: ");
+  
+  cangle_spinner = glui2->add_spinner_to_panel(settings_panel, "Field of View :   ", GLUI_SPINNER_FLOAT, &cangle);
+  cangle_spinner->set_float_limits( 1.0, 180.0 );
+  cangle_spinner->set_speed( 20.0 );
+  cangle_spinner->set_float_val(60.0);
+  cangle_spinner->set_alignment( GLUI_ALIGN_RIGHT );
+
+  pzoom_spinner = glui2->add_spinner_to_panel(settings_panel, "Parallel Zoom :", GLUI_SPINNER_FLOAT, &pzoom);
+  pzoom_spinner->set_float_limits( 10.0, 50000.0 );
+  pzoom_spinner->set_speed( 5.0 );
+  pzoom_spinner->set_float_val(2000.0);
+  pzoom_spinner->set_alignment( GLUI_ALIGN_RIGHT );
+  pzoom_spinner->disable();
+
+  glui2->add_column( true );
+
+  mode_panel = glui2->add_panel("Mode: ");
+  
+  /****** Top view *****/
+  glui2->add_button_to_panel(mode_panel, "Top view", 0, callTopView )->set_alignment( GLUI_ALIGN_CENTER );
+
+  
+  /****** Reset button *****/
+  glui2->add_button_to_panel(mode_panel, "Reset position", 0, resetView )->set_alignment( GLUI_ALIGN_CENTER );
+  glui2->add_column( true );
+
+  /****** Add Camera View *****/
+
+ 
+  camera_panel = glui2->add_panel("Camera: ");
+  cam_spinner = glui2->add_spinner_to_panel(camera_panel, "Choose Camera", GLUI_SPINNER_INT, &cam_choice);
+  cam_spinner->set_int_limits( 0, 0 );
+  cam_spinner->set_speed( 1 );
+  cam_spinner->set_alignment( GLUI_ALIGN_LEFT );
+  
+  glui2->add_button_to_panel(camera_panel, "Add Camera", 1, callAddCamera )->set_alignment( GLUI_ALIGN_CENTER );
+  glui2->add_button_to_panel(camera_panel, "Delete Camera", 0, callDeleteCamera )->set_alignment( GLUI_ALIGN_CENTER );
+  
+  /******* Other navigation controls**********/
+  glui2->add_column(true);
+
+  nav_panel = glui2->add_panel("Navigation: ");
+  rotButton = glui2->add_rotation_to_panel(nav_panel, "Rotation", view_rotate_button, -1, update_view_rotate);
+
+  glui2->add_column_to_panel(nav_panel, true );
+  glui2->add_translation_to_panel(nav_panel, "Move XY", GLUI_TRANSLATION_XY,
+						    obj_pos_button, -1, update_view_translation);
+
+  glui2->add_column_to_panel(nav_panel, false );
+  glui2->add_translation_to_panel(nav_panel, "Move X", GLUI_TRANSLATION_X,
+						    &obj_pos_button[0], -1, update_view_translation);
+
+  glui2->add_column_to_panel(nav_panel, false );
+  glui2->add_translation_to_panel(nav_panel, "Move Y", GLUI_TRANSLATION_Y,
+						    &obj_pos_button[1], -1, update_view_translation);
+
+  glui2->add_column_to_panel(nav_panel, false );
+  glui2->add_translation_to_panel(nav_panel, "Move Z", GLUI_TRANSLATION_Z,
+						    &obj_pos_button[2], -1, update_view_translation);
+
+  glui2->add_column_to_panel(nav_panel, false);
+  glui2->add_checkbox_to_panel(nav_panel, "MouseNav", &cameraNavMouseMode );
+  
+  static int dummy4;
+  always_box = glui2->add_checkbox_to_panel(nav_panel, "Always all Points", &dummy4, 0, &changePointMode);
+  glui2->set_glutMouseFunc(CallBackMouseFuncMoving);
+  static int dummy5 = 1;
+  never_box =  glui2->add_checkbox_to_panel(nav_panel, "Always reduce Points", &dummy5, 1, &changePointMode );
+  
+  /*** Create the right subwindow ***/
+  glui1 = GLUI_Master.create_glui("3D_Viewer - Selection");
+  window_id_menu1 = glui1->get_glut_window_id();
+  glutSetWindow(window_id_menu1);
+  glutPositionWindow(START_X + START_WIDTH + 50, START_Y + 30);
+  glutSetWindow(window_id);
+  glui1->set_main_gfx_window( window_id );
+
+  glui1->add_checkbox( "Draw Points", &show_points );
+  glui1->add_checkbox( "Draw Camera", &show_cameras);
+  glui1->add_checkbox( "Draw Path", &show_path);
+  ps_spinner = glui1->add_spinner( "Point Size:", GLUI_SPINNER_FLOAT, &pointsize);
+  ps_spinner->set_float_limits( 0.0000001, 10.0 );
+  ps_spinner->set_speed( 25.0 );
+  ps_spinner->set_float_val(pointsize);
+  ps_spinner->set_alignment( GLUI_ALIGN_LEFT );  
+
+
+  /**** Fog Panel *****/
+  
+  GLUI_Panel *fogt_panel = glui1->add_rollout("Fog :", false );
+  fogt_panel ->set_alignment( GLUI_ALIGN_LEFT );
+  GLUI_RadioGroup *fogt = glui1-> add_radiogroup_to_panel( fogt_panel, &show_fog );
+  glui1->add_radiobutton_to_group( fogt, "No Fog" );
+  glui1->add_radiobutton_to_group( fogt, "Fog Exp" );
+  glui1->add_radiobutton_to_group( fogt, "Fog Exp2" );
+  glui1->add_radiobutton_to_group( fogt, "Fog Linear" );
+  glui1->add_radiobutton_to_group( fogt, "inverted Fog Exp" );
+  glui1->add_radiobutton_to_group( fogt, "inverted Fog Exp2" );
+  glui1->add_radiobutton_to_group( fogt, "inverted Fog Linear" );
+
+  
+  
+  
+  fog_spinner = glui1->add_spinner( "Fog Density:", GLUI_SPINNER_FLOAT, &fogDensity);
+  fog_spinner->set_float_limits( 0.0, 1.0 );
+  fog_spinner->set_speed( 0.5 );
+  fog_spinner->set_float_val(0.001);
+  fog_spinner->set_alignment( GLUI_ALIGN_LEFT );  
+
+
+  /****** Color Controls *****/
+  color_panel = glui1->add_rollout("Color :", false );
+  color_panel ->set_alignment( GLUI_ALIGN_LEFT );
+
+  GLUI_Panel *color_ro = glui1->add_rollout_to_panel(color_panel, "Color values:", false);
+  color_ro->set_alignment(GLUI_ALIGN_LEFT);
+
+  GLUI_RadioGroup *color_rog = glui1->add_radiogroup_to_panel( color_ro, &listboxColorVal, 0, &mapColorToValue );
+  glui1->add_radiobutton_to_group( color_rog, "height");
+  GLUI_RadioButton *rbrefl =  glui1->add_radiobutton_to_group( color_rog, "reflectance");
+  GLUI_RadioButton *rbampl = glui1->add_radiobutton_to_group( color_rog, "amplitude");
+  GLUI_RadioButton *rbdevi = glui1->add_radiobutton_to_group( color_rog, "deviation");
+  GLUI_RadioButton *rbtype = glui1->add_radiobutton_to_group( color_rog, "type");
+  //if (!(types & PointType::USE_REFLECTANCE)) rbrefl->disable(); 
+  //if (!(types & PointType::USE_AMPLITUDE)) rbampl->disable();
+  //if (!(types & PointType::USE_DEVIATION)) rbdevi->disable(); 
+  //if (!(types & PointType::USE_TYPE)) rbtype->disable(); 
+  if (!(pointtype.hasReflectance())) rbrefl->disable(); 
+  if (!(pointtype.hasAmplitude())) rbampl->disable();
+  if (!(pointtype.hasDeviation())) rbdevi->disable(); 
+  if (!(pointtype.hasType())) rbtype->disable(); 
+
+  GLUI_Panel *colorm_ro = glui1->add_rollout_to_panel(color_panel, "Colormap:", false);
+  colorm_ro->set_alignment(GLUI_ALIGN_LEFT);
+
+  GLUI_RadioGroup *colorm_rog = glui1->add_radiogroup_to_panel(colorm_ro, &listboxColorMapVal, 0, &changeColorMap);
+  glui1->add_radiobutton_to_group(colorm_rog, "Solid");
+  glui1->add_radiobutton_to_group(colorm_rog, "Grey");
+  glui1->add_radiobutton_to_group(colorm_rog, "HSV");
+  glui1->add_radiobutton_to_group(colorm_rog, "Jet");
+  glui1->add_radiobutton_to_group(colorm_rog, "Hot");
+  glui1->add_radiobutton_to_group(colorm_rog, "Rand");
+  glui1->add_radiobutton_to_group(colorm_rog, "SHSV");
+
+  GLUI_Panel *scans_color = glui1->add_rollout_to_panel(color_panel, "Color type:", false);
+  scans_color->set_alignment(GLUI_ALIGN_LEFT);
+  GLUI_RadioGroup *scans_colored = glui1->add_radiogroup_to_panel(scans_color, &colorScanVal, 0, &setScansColored);
+  glui1->add_radiobutton_to_group(scans_colored, "None");
+  glui1->add_radiobutton_to_group(scans_colored, "Id Scans by Color");
+  GLUI_RadioButton *colorb = glui1->add_radiobutton_to_group( scans_colored, "Color by Points");
+  if (!(pointtype.hasColor())) colorb->disable(); 
+
+  mincol_spinner = glui1->add_spinner_to_panel(color_panel, "Min Val:", GLUI_SPINNER_FLOAT, &mincolor_value, 0, &minmaxChanged);
+  mincol_spinner->set_alignment(GLUI_ALIGN_RIGHT);
+  maxcol_spinner = glui1->add_spinner_to_panel(color_panel, "Max Val:", GLUI_SPINNER_FLOAT, &maxcolor_value, 0, &minmaxChanged);
+  maxcol_spinner->set_alignment(GLUI_ALIGN_RIGHT); 
+  glui1->add_button_to_panel(color_panel, "Reset Min/Max", 0, &resetMinMax )->set_alignment( GLUI_ALIGN_CENTER );
+
+  glui1->add_separator();
+ 
+  /****** Invert button *****/
+  glui1->add_button( "Invert", 0, invertView )->set_alignment( GLUI_ALIGN_CENTER );
+  /****** Animate button *****/
+  anim_spinner = glui1->add_spinner( "Anim delay:", GLUI_SPINNER_INT, &anim_delay);
+  anim_spinner->set_int_limits( 0, 100 );
+  anim_spinner->set_speed( 1 );
+  glui1->add_button( "Animate", 0, startAnimation )->set_alignment( GLUI_ALIGN_CENTER );
+  
+  glui1->add_separator();
+
+  
+/**** Path panel *******/
+
+  path_panel = glui1->add_rollout("Camera Path :", false );
+  path_panel ->set_alignment( GLUI_ALIGN_LEFT );
+  path_filename_edit = glui1->add_edittext_to_panel(path_panel,"File: ",GLUI_EDITTEXT_TEXT, path_file_name);
+  path_filename_edit->set_alignment( GLUI_ALIGN_LEFT );
+  glui1->add_button_to_panel(path_panel, "Save Path   ", 0, savePath)->set_alignment( GLUI_ALIGN_CENTER);
+  glui1->add_button_to_panel(path_panel, "Load Path   ", 0, loadPath)->set_alignment( GLUI_ALIGN_CENTER);
+  glui1->add_button_to_panel(path_panel, "Load Robot P.", 0, drawRobotPath )->set_alignment( GLUI_ALIGN_CENTER );
+  glui1->add_separator_to_panel(path_panel);
+  glui1->add_checkbox_to_panel(path_panel, "Save Animation", &save_animation);
+  interpol_box = glui1->add_checkbox_to_panel(path_panel, "Interpolate by Distance", &inter_by_dist, -1, &callCameraUpdate);
+  //always_box = glui2->add_checkbox_to_panel(nav_panel, "Always all Points", &dummy4, 0, &changePointMode);
+  //glui1->add_checkbox_to_panel(path_panel, "Interpolate by Distance", &inter_by_dist);
+  glui1->add_button_to_panel(path_panel, "Animate Path", 0, pathAnimate)->set_alignment( GLUI_ALIGN_CENTER);
+
+  /**** Position panel *******/
+  
+  pose_panel = glui1->add_rollout("Position :", false );
+  pose_panel ->set_alignment( GLUI_ALIGN_LEFT );
+  pose_filename_edit = glui1->add_edittext_to_panel(pose_panel,"File: ",GLUI_EDITTEXT_TEXT, pose_file_name);
+  pose_filename_edit->set_alignment( GLUI_ALIGN_LEFT );
+  glui1->add_button_to_panel(pose_panel, "Save Pose   ", 0, savePose)->set_alignment( GLUI_ALIGN_CENTER);
+  glui1->add_button_to_panel(pose_panel, "Load Pose   ", 0, loadPose)->set_alignment( GLUI_ALIGN_CENTER);
+  image_spinner = glui1->add_spinner_to_panel(pose_panel, "Factor :   ",
+GLUI_SPINNER_INT, &factor);
+  image_spinner->set_int_limits( 1, 10 );
+  image_spinner->set_speed( 1 );
+  image_spinner->set_alignment(GLUI_ALIGN_RIGHT);
+  glui1->add_button_to_panel(pose_panel, "Save Image   ", 0, saveImage)->set_alignment( GLUI_ALIGN_CENTER);
+  
+ 
+  glui1->add_separator();
+  
+  /**** Selection panel ******/
+  selection_panel = glui1->add_rollout("Selection :", false );
+  selection_panel ->set_alignment( GLUI_ALIGN_LEFT );
+  
+  selection_filename_edit = glui1->add_edittext_to_panel(selection_panel,"File: ",GLUI_EDITTEXT_TEXT, selection_file_name);
+  selection_filename_edit->set_alignment( GLUI_ALIGN_LEFT );
+  glui1->add_button_to_panel(selection_panel, "Save selected points   ", 0, saveSelection)->set_alignment( GLUI_ALIGN_CENTER);
+
+  glui1->add_button_to_panel(selection_panel, "Clear selected points   ", 0, clearSelection)->set_alignment( GLUI_ALIGN_CENTER);
+  glui1->add_checkbox_to_panel(selection_panel, "Select/Unselect", &selectOrunselect);
+  glui1->add_checkbox_to_panel(selection_panel, "Select Voxels", &select_voxels);
+  depth_spinner = glui1->add_spinner_to_panel(selection_panel, "Depth :   ",
+      GLUI_SPINNER_INT, &selection_depth);
+  depth_spinner->set_int_limits( 1, 100 );
+  depth_spinner->set_speed( 1 );
+  depth_spinner->set_alignment(GLUI_ALIGN_RIGHT);
+  brushsize_spinner = glui1->add_spinner_to_panel(selection_panel, "Brushsize :   ",
+      GLUI_SPINNER_INT, &brush_size);
+  brushsize_spinner->set_int_limits( 0, 100 );
+  brushsize_spinner->set_speed( 1 );
+  brushsize_spinner->set_alignment(GLUI_ALIGN_RIGHT);
+  
+ 
+  glui1->add_separator();
+  /**** Advanced panel ******/
+  if (advanced_controls) {
+    advanced_panel = glui1->add_rollout("Advanced :", false );
+    advanced_panel->set_alignment( GLUI_ALIGN_LEFT );
+
+    //  glui1->add_edittext_to_panel(advanced_panel,"Frame #: ",GLUI_EDITTEXT_TEXT, current_frame)->set_alignment( GLUI_ALIGN_LEFT );
+    frame_spinner = glui1->add_spinner_to_panel(advanced_panel, "Frame #:   ",
+        GLUI_SPINNER_INT, &current_frame);
+    frame_spinner->set_int_limits( 0, MetaMatrix[0].size()-1 );
+    frame_spinner->set_speed( 10 );
+    frame_spinner->set_alignment(GLUI_ALIGN_RIGHT);
+
+    fps_spinner = glui1->add_spinner_to_panel(advanced_panel, "FPS :   ",
+        GLUI_SPINNER_FLOAT, &idealfps);
+    fps_spinner->set_int_limits( 0, 100 );
+    fps_spinner->set_speed( 1 );
+    fps_spinner->set_alignment(GLUI_ALIGN_RIGHT);
+
+    farplane_spinner = glui1->add_spinner_to_panel(advanced_panel, "farplane :   ",
+        GLUI_SPINNER_FLOAT, &maxfardistance);
+    farplane_spinner->set_float_limits( 1, 100000 );
+    farplane_spinner->set_speed( 1 );
+    farplane_spinner->set_alignment(GLUI_ALIGN_RIGHT);
+    
+    nearplane_spinner = glui1->add_spinner_to_panel(advanced_panel, "nearplane :   ",
+        GLUI_SPINNER_FLOAT, &neardistance);
+    nearplane_spinner->set_int_limits( 1, 100000 );
+    nearplane_spinner->set_speed( 1 );
+    nearplane_spinner->set_alignment(GLUI_ALIGN_RIGHT);
+
+    glui1->add_button_to_panel(advanced_panel, "Cycle LOD", 0,(GLUI_Update_CB)cycleLOD )->set_alignment( GLUI_ALIGN_CENTER );
+
+    lod_spinner = glui1->add_spinner_to_panel(advanced_panel, "lod speed :   ",
+        GLUI_SPINNER_FLOAT, &adaption_rate);
+    lod_spinner->set_float_limits( 0, 3.0 );
+    lod_spinner->set_speed( 0.1 );
+    lod_spinner->set_alignment(GLUI_ALIGN_RIGHT);
+    
+    glui1->add_separator();
+  }
+
+  /****** A 'quit' button *****/
+  glui1->add_button( "Quit", 0,(GLUI_Update_CB)exit )->set_alignment( GLUI_ALIGN_CENTER );
+
+  glui1->set_glutMouseFunc(CallBackMouseFuncMoving);
+  /**** Link windows to GLUI, and register idle callback ******/  
+  glutSetWindow(window_id);
+  glui1->set_main_gfx_window( window_id );  // right
+  glui2->set_main_gfx_window( window_id ); // bottom
+  glui1->sync_live();
+  glui2->sync_live();
+
+  // cout << "Called : myNewMenu()...."<<endl;
+  // cout << "show_points: " << show_points << endl;
+  GLUI_Master.set_glutMouseFunc( CallBackMouseFunc );
+  GLUI_Master.set_glutKeyboardFunc( CallBackInterfaceFunc );
+  GLUI_Master.set_glutIdleFunc( CallBackIdleFunc );
+  GLUI_Master.set_glutSpecialFunc( CallBackSpecialFunc );
+}
+
+
+/**
+ * This function is called when a user starts to animate the generated path
+ */
+void pathAnimate(int dummy) {
+  
+  //signal that the screen needs to be repainted for animation
+  haveToUpdate = 6;
+  path_iterator = 0;
+ 
+}
+
+
+/**
+ * This function clears the selected points  
+ */
+void clearSelection(int dummy) {
+  for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) 
+    selected_points[iterator].clear();
+}
+

include/scanserver/sharedScan.h

@@ -71,6 +71,9 @@ public:
   //! Filter parameters for height checks when loading from file, invalidate cache for scan CacheObject if it differs
   void setHeightParameters(double top, double bottom);
 
+  //! Filter parameters for the range mutator for showing the spheres, invalidate cache for scan CacheObject if it differs
+  void setRangeMutationParameters(double range);
+    
   //! Set parameters and invalidate cache for reduced CacheObjects if it differs
   void setReductionParameters(const char* params);
 
@@ -142,7 +145,8 @@ public:
   inline IOType getIOType() const { return m_iotype; }
   inline float getMaxDist() const { return m_max_dist; }
   inline float getMinDist() const { return m_min_dist; }
-  inline double geHeightTop() const { return m_height_top; }
+  inline double getRangeMutator() const { return m_range_mutator_param; }
+  inline double getHeightTop() const { return m_height_top; }
   inline double getHeightBottom() const { return m_height_bottom; }
   
   //! Assembles an PointFilter with range/height parameters (if set) to use process-locally
@@ -155,7 +159,8 @@ private:
   unsigned int m_prefetch;
   double m_max_dist, m_min_dist;
   double m_height_top, m_height_bottom;
-  bool m_range_param_set, m_height_param_set;
+  double m_range_mutator_param;
+  bool m_range_mutator_param_set, m_range_param_set, m_height_param_set;
   SharedString m_reduction_parameters;
   SharedString m_show_parameters;
   SharedString m_octtree_parameters;

include/slam6d/basicScan.h

@@ -21,6 +21,7 @@ public:
 
   virtual void setRangeFilter(double max, double min);
   virtual void setHeightFilter(double top, double bottom);
+  virtual void setRangeMutation(double range);
 
   virtual const char* getIdentifier() const { return m_identifier.c_str(); }
 
@@ -45,8 +46,8 @@ private:
 
   IOType m_type;
 
-  double m_filter_max, m_filter_min, m_filter_top, m_filter_bottom;
+  double m_filter_max, m_filter_min, m_filter_top, m_filter_bottom, m_range_mutation;
-  bool m_filter_range_set, m_filter_height_set;
+  bool m_filter_range_set, m_filter_height_set, m_range_mutation_set;
 
   std::map<std::string, std::pair<unsigned char*, unsigned int>> m_data;
 

include/slam6d/fbr/fbr_global.h

@@ -36,6 +36,7 @@ namespace fbr{
     PANNINI,
     STEREOGRAPHIC,
     ZAXIS,
+    CONIC,
   };
   /**
    * @enum panorama_map_method

include/slam6d/globals.icc

@@ -303,7 +303,7 @@ inline void MMult(const T *M1,
 }
 
 /**
- * Transforms a vactor with a matrix (P = M * V)
+ * Transforms a vector with a matrix (P = M * V)
  * @param M the transformation matrix
  * @param v the initial vector
  * @param p the transformt vector

include/slam6d/managedScan.h

@@ -18,6 +18,8 @@ public:
 
   virtual void setRangeFilter(double max, double min);
   virtual void setHeightFilter(double top, double bottom);
+  virtual void setRangeMutation(double range);
+  
   virtual void setReductionParameter(double voxelSize, int nrpts = 0,
     PointType pointtype = PointType());
   void setShowReductionParameter(double voxelSize, int nrpts = 0,

include/slam6d/pointfilter.h

@@ -31,6 +31,7 @@ public:
 
   PointFilter& setRange(double maxDist, double minDist);
   PointFilter& setHeight(double top, double bottom);
+  PointFilter& setRangeMutator(double range);
 
   //! Serialization function to convert it into a string, usable in the constructor
   std::string getParams();
@@ -108,6 +109,14 @@ private:
   double m_bottom;
 };
 
+class RangeMutator : public Checker {
+public:
+  RangeMutator(const std::string& value);
+  virtual bool test(double* point);
+private:
+  double m_range;
+};
+
 #include "pointfilter.icc"
 
 #endif //POINT_FILTER_H

include/slam6d/scan.h

@@ -140,6 +140,9 @@ public:
   //! Input filtering for all points based on their height
   virtual void setHeightFilter(double top, double bottom) = 0;
 
+  //! Input mutation to set range of all points to a constant value;
+  virtual void setRangeMutation(double range) { }
+  
   //! Set reduction parameters, but don't reduce yet
   virtual void setReductionParameter(double voxelSize, int nrpts = 0,
     PointType pointtype = PointType());

src/scanserver/sharedScan.cc

@@ -28,6 +28,8 @@ SharedScan::SharedScan(const ip::allocator<void, SegmentManager> & allocator,
   m_prefetch(0),
   m_max_dist(0.0), m_min_dist(0.0),
   m_height_top(0.0), m_height_bottom(0.0),
+  m_range_mutator_param(0.0),
+  m_range_mutator_param_set(false),
   m_range_param_set(false), m_height_param_set(false),
   m_reduction_parameters(allocator),
   m_show_parameters(allocator),
@@ -84,6 +86,20 @@ void SharedScan::setHeightParameters(double top, double bottom)
   m_height_param_set = true;
 }
 
+void SharedScan::setRangeMutationParameters(double range)
+{
+  // if a non-first set differs from the previous ones, invalidate all COs
+  if(m_range_mutator_param_set) {
+    if(range != m_range_mutator_param) {
+      invalidateFull();
+      invalidateReduced();
+      invalidateShow();
+    }
+  }
+  m_range_mutator_param = range;
+  m_range_mutator_param_set = true;
+}
+
 void SharedScan::setReductionParameters(const char* params)
 {
   // if a non-first set differs from the previous ones, invalidate reduced COs
@@ -118,6 +134,9 @@ PointFilter SharedScan::getPointFilter() const
     r.setRange(m_max_dist, m_min_dist);
   if(m_height_param_set)
     r.setHeight(m_height_top, m_height_bottom);
+  if(m_range_mutator_param_set)
+    r.setRangeMutator(m_range_mutator_param);
+  
   return r;
 }
 

src/show/show_common.cc

@@ -140,6 +140,11 @@ GLdouble Rquat[4]         ={0.0, 0.0, 0.0, 1.0};
 GLdouble X               = 0.0, Y               = 0.0, Z               = 0.0;
 GLdouble RVX = 0.0, RVY = 0.0, RVZ = 0.0;
 
+/**
+ * Center of Mass coordinates
+ */
+GLdouble CoM[3] = { 0.0, 0.0, 0.0 };
+
 /**
  * parallel zoom (similar to apex angle) for parallel projection
  */
@@ -391,6 +396,8 @@ void usage(char* prog)
 	  << "         only works when using octree display" << endl
     << bold << "  -c, --color" << endl << normal
 	  << "         use color RGB values for coloring point clouds" << endl
+    << bold << "  -b" << normal << " NR, " << bold << "--sphere=" << normal << "NR" << endl
+	  << "         map all measurements on a sphere (of radius NRcm)" << endl
     << bold << "  --saveOct" << endl << normal
 	  << "         stores all used scans as octrees in the given directory" << endl
 	  << "         All reflectivity/amplitude/deviation/type settings are stored as well." << endl
@@ -421,7 +428,8 @@ void usage(char* prog)
  */
 int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxDist, int& minDist, 
               double &red, bool &readInitial, int &octree, PointType &ptype, float &fps, string &loadObj,
-              bool &loadOct, bool &saveOct, int &origin, double &scale, IOType &type, bool& scanserver)
+              bool &loadOct, bool &saveOct, int &origin, double &scale, IOType &type, bool& scanserver, 
+	      double& sphereMode)
 {
   unsigned int types = PointType::USE_NONE;
   start   = 0;
@@ -440,7 +448,7 @@ int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxD
     { "origin",          optional_argument,   0,  'o' },
     { "format",          required_argument,   0,  'f' },
     { "fps",             required_argument,   0,  'F' },
-    { "scale",           required_argument,   0,  'S' },
+    { "scale",           required_argument,   0,  'C' },
     { "start",           required_argument,   0,  's' },
     { "end",             required_argument,   0,  'e' },
     { "reduce",          required_argument,   0,  'r' },
@@ -459,10 +467,11 @@ int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxD
     { "loadOct",         no_argument,         0,  '1' },
     { "advanced",        no_argument,         0,  '2' },
     { "scanserver",      no_argument,         0,  'S' },
+    { "sphere",          required_argument,   0,  'b' },
     { 0,           0,   0,   0}                    // needed, cf. getopt.h
   };
 
-  while ((c = getopt_long(argc, argv,"F:f:s:e:r:m:M:O:o:l:C:SwtRadhTc", longopts, NULL)) != -1) {
+  while ((c = getopt_long(argc, argv,"F:f:s:e:r:m:M:O:o:l:C:SwtRadhTcb", longopts, NULL)) != -1) {
     switch (c) {
       case 's':
         w_start = atoi(optarg);
@@ -549,6 +558,9 @@ int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxD
       case '2':
         advanced_controls = true; 
         break;
+      case 'b':
+        sphereMode = atof(optarg);
+        break;
       default:
         abort ();
     }
@@ -574,6 +586,7 @@ int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxD
 
 void setResetView(int origin) {
   if (origin == 1) {
+    // set origin to the pose of the first scan
     double *transmat = MetaMatrix[0].back();
     cout << transmat << endl;
 
@@ -589,15 +602,54 @@ void setResetView(int origin) {
     quat[2] = Rquat[2];
     quat[3] = Rquat[3];
   } else if (origin == 2) {
-    double center[3];
+    // set origin to the center of the first octree
+    double center[3], center_transformed[3];
 #ifdef USE_COMPACT_TREE
     ((compactTree*)octpts[0])->getCenter(center);
 #else
     ((Show_BOctTree<sfloat>*)octpts[0])->getCenter(center);
 #endif
-    RVX = -center[0];
+    VTrans(MetaMatrix[0].back(), center, center_transformed);
-    RVY = -center[1];
+    RVX = -center_transformed[0];
-    RVZ = -center[2];
+    RVY = -center_transformed[1];
+    RVZ = -center_transformed[2];
+    X = RVX;
+    Y = RVY;
+    Z = RVZ;
+  } else if (origin == 3) {
+    // set origin to the center of mass of all scans
+  for (size_t i = 0; i < octpts.size(); ++i) {
+    vector <sfloat*> points;
+#ifdef USE_COMPACT_TREE
+    ((compactTree*)octpts[i])->AllPoints( points );
+#else
+    BOctTree<sfloat>* cur_tree = ((Show_BOctTree<sfloat>*)octpts[i])->getTree();
+    cur_tree->AllPoints( points );
+#endif
+
+    cout << "Scan " << i << " size: " << points.size() << endl;
+    double centroid[3] = {0., 0., 0.};
+    double centroid_transformed[3];;
+    for (size_t j = 0; j < points.size(); ++j) {
+      for (unsigned int k = 0; k < 3; ++k)
+        centroid[k] += points[j][k];
+    }
+    for (unsigned int k = 0; k < 3; ++k) {
+      centroid[k] /= (double)points.size();
+    }
+    VTrans(MetaMatrix[i].back(), centroid, centroid_transformed);
+    for (unsigned int k = 0; k < 3; ++k) {
+	 CoM[k] += centroid_transformed[k];
+    }
+  }
+  for (unsigned int k = 0; k < 3; ++k)
+    CoM[k] /= octpts.size() * 1.;
+
+  cout << "Center of Mass at: " << CoM[0] << ", " << CoM[1] << ", " << CoM[2] << endl;
+
+  RVX = -CoM[0];
+  RVY = -CoM[1];
+  RVZ = -CoM[2];
   X = RVX;
   Y = RVY;
   Z = RVZ;
@@ -846,6 +898,7 @@ void initShow(int argc, char **argv){
   int origin = 0;
   double scale = 0.01; // in m
   bool scanserver = false;
+  double sphereMode = 0.0;
 
   pose_file_name = new char[1024];
   path_file_name = new char[1024];
@@ -856,7 +909,8 @@ void initShow(int argc, char **argv){
   strncpy(selection_file_name, "selected.3d", 1024);
 
   parseArgs(argc, argv, dir, start, end, maxDist, minDist, red, readInitial,
-  octree, pointtype, idealfps, loadObj, loadOct, saveOct, origin, scale, type, scanserver);
+    octree, pointtype, idealfps, loadObj, loadOct, saveOct, origin, scale,
+    type, scanserver, sphereMode);
 
   // modify all scale dependant variables
   scale = 1.0 / scale;
@@ -905,7 +959,8 @@ void initShow(int argc, char **argv){
   for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) {
     Scan* scan = *it;
     scan->setRangeFilter(maxDist, minDist);
-    if(red > 0) {
+    if (sphereMode > 0.0) scan->setRangeMutation(sphereMode);
+    if (red > 0) {
       // scanserver differentiates between reduced for slam and reduced for show, can handle both at the same time
       if(scanserver) {
         dynamic_cast<ManagedScan*>(scan)->setShowReductionParameter(red, octree);

src/show/show_menu.cc

@@ -140,7 +140,7 @@ void newMenu()
 
   settings_panel = glui2->add_panel("Settings: ");
   
-  cangle_spinner = glui2->add_spinner_to_panel(settings_panel, "Apex Angle :   ", GLUI_SPINNER_FLOAT, &cangle);
+  cangle_spinner = glui2->add_spinner_to_panel(settings_panel, "Field of View :   ", GLUI_SPINNER_FLOAT, &cangle);
   cangle_spinner->set_float_limits( 1.0, 180.0 );
   cangle_spinner->set_speed( 20.0 );
   cangle_spinner->set_float_val(60.0);

src/slam6d/basicScan.cc

@@ -103,8 +103,10 @@ void BasicScan::init()
   m_filter_min = 0.0;
   m_filter_top = 0.0;
   m_filter_bottom = 0.0;
+  m_range_mutation = 0.0;
   m_filter_range_set = false;
   m_filter_height_set = false;
+  m_range_mutation_set = false;
 }
 
 
@@ -122,6 +124,12 @@ void BasicScan::setHeightFilter(double top, double bottom)
   m_filter_height_set = true;
 }
 
+void BasicScan::setRangeMutation(double range)
+{
+  m_range_mutation_set = true;
+  m_range_mutation = range;
+}
+
 void BasicScan::get(unsigned int types)
 {
   ScanIO* sio = ScanIO::getScanIO(m_type);
@@ -138,6 +146,8 @@ void BasicScan::get(unsigned int types)
     filter.setRange(m_filter_max, m_filter_min);
   if(m_filter_height_set)
     filter.setHeight(m_filter_top, m_filter_bottom);
+  if(m_range_mutation_set)
+    filter.setRangeMutator(m_range_mutation);
 
   sio->readScan(m_path.c_str(),
                 m_identifier.c_str(),

src/slam6d/fbr/fbr_global.cc

@@ -43,6 +43,9 @@ namespace fbr{
     case ZAXIS:
       sMethod = "ZAXIS";
       break;
+    case CONIC:
+      sMethod = "CONIC";
+      break;
     default:
       throw std::runtime_error(std::string("projection method ") + to_string(method) + std::string(" could not be matched to a projection method"));
     }
@@ -57,6 +60,7 @@ namespace fbr{
     else if(strcasecmp(method.c_str(), "PANNINI") == 0) return PANNINI;
     else if(strcasecmp(method.c_str(), "STEREOGRAPHIC") == 0) return STEREOGRAPHIC;
     else if(strcasecmp(method.c_str(), "ZAXIS") == 0) return ZAXIS;
+    else if(strcasecmp(method.c_str(), "CONIC") == 0) return CONIC;
     else throw std::runtime_error(std::string("projection method ") + method + std::string(" is unknown"));
   }
   

src/slam6d/fbr/feature_based_registration.cc

@@ -40,17 +40,17 @@ struct information{
  */
 void usage(int argc, char** argv){
   printf("\n");
-  printf("USAGE: %s dir firstScanNumber secondScanNumber \n", argv[0]);
+  printf("USAGE: %s dir -s firstScanNumber -e secondScanNumber \n", argv[0]);
   printf("\n");
   printf("\n");
   printf("\tOptions:\n");
-  printf("\t\t-F scanFormat\t\t input scan file format [RIEGL_TXT|RXP|ALL SLAM6D SCAN_IO]\n");
+  printf("\t\t-f scanFormat\t\t input scan file format [RIEGL_TXT|RXP|ALL SLAM6D SCAN_IO]\n");
   printf("\t\t-W iWidth\t\t panorama image width\n");
   printf("\t\t-H iHeight\t\t panorama image height\n");
-  printf("\t\t-p pMethod\t\t projection method [EQUIRECTANGULAR|CYLINDRICAL|MERCATOR|RECTILINEAR|PANNINI|STEREOGRAPHIC|ZAXIS]\n");
+  printf("\t\t-p pMethod\t\t projection method [EQUIRECTANGULAR|CONIC|CYLINDRICAL|MERCATOR|RECTILINEAR|PANNINI|STEREOGRAPHIC|ZAXIS]\n");
   printf("\t\t-N nImage\t\t number of images used for some projections\n");
   printf("\t\t-P pParam\t\t special projection parameter (d for Pannini and r for stereographic)\n");
-  printf("\t\t-f fMethod\t\t feature detection method [SURF|SIFT|ORB|FAST|STAR]\n");
+  printf("\t\t-F fMethod\t\t feature detection method [SURF|SIFT|ORB|FAST|STAR]\n");
   printf("\t\t-d dMethod\t\t feature description method [SURF|SIFT|ORB]\n");
   printf("\t\t-m mMethod\t\t feature matching method [BRUTEFORCE|FLANN|KNN|RADIUS|RATIO]\n");
   printf("\t\t-D minDistance \t\t threshold for min distance in registration process\n");
@@ -64,17 +64,16 @@ void usage(int argc, char** argv){
   printf("\tExamples:\n");
   printf("\tUsing Bremen City dataset:\n");
   printf("\tLoading scan000.txt and scan001.txt:\n");
-  printf("\t\t %s ~/dir/to/bremen_city 0 1\n", argv[0]);
+  printf("\t\t %s ~/dir/to/bremen_city -s 0 -e 1\n", argv[0]);
   printf("\tLoading scan005.txt and scan006.txt and output panorma images and feature images and match images in ~/dir/to/bremen_city/out dir:\n");
-  printf("\t\t %s -V 1 -O ~/dir/to/bremen_city/out/ ~/dir/to/bremen_city 5 6 \n", argv[0]);
+  printf("\t\t %s -V 1 -O ~/dir/to/bremen_city/out/ ~/dir/to/bremen_city -s 5 -e 6 \n", argv[0]);
   printf("\tLoading scan010.txt and scan011.txt using Mercator projection and SURF feature detector and SIFT descriptor:\n");
-  printf("\t\t %s -p MERCATOR -f SURF -d SIFT -O ~/dir/to/bremen_city/out/ ~/dir/to/bremen_city 10 11 \n", argv[0]);
+  printf("\t\t %s -p MERCATOR -F SURF -d SIFT -O ~/dir/to/bremen_city/out/ ~/dir/to/bremen_city -s 10 -e 11 \n", argv[0]);
   printf("\n");
   exit(1);
 }
 
 void parssArgs(int argc, char** argv, information& info){
-  cout<<"parssArgs"<<endl;
   time_t rawtime;
   struct tm *timeinfo;
   time(&rawtime);
@@ -106,10 +105,16 @@ void parssArgs(int argc, char** argv, information& info){
   int c;
   opterr = 0;
   //reade the command line and get the options
-  while ((c = getopt (argc, argv, "F:W:H:p:N:P:f:d:m:D:E:I:M:r:V:O:")) != -1)
+  while ((c = getopt (argc, argv, "F:W:H:p:N:P:f:d:m:D:E:I:M:r:V:O:s:e:")) != -1)
     switch (c)
       {
-      case 'F':
+      case 's':
+	info.fScanNumber = atoi(optarg);
+	break;
+      case 'e':
+	info.sScanNumber = atoi(optarg);
+	break;
+      case 'f':
 	info.sFormat = stringToScanFormat(optarg);
 	break;
       case 'W':
@@ -127,7 +132,7 @@ void parssArgs(int argc, char** argv, information& info){
       case 'P':
 	info.pParam = atoi(optarg);
 	break;
-      case 'f':
+      case 'F':
 	info.fMethod = stringToFeatureDetectorMethod(optarg);
 	break;
       case 'd':
@@ -152,7 +157,6 @@ void parssArgs(int argc, char** argv, information& info){
 	info.rMethod = stringToRegistrationMethod(optarg);
 	break;
       case 'V':
-	cout<<"verboos"<<endl;
 	info.verbose = atoi(optarg);
 	break;
       case 'O':
@@ -188,15 +192,15 @@ void parssArgs(int argc, char** argv, information& info){
     usage(argc, argv);
   }
 
-  if (optind > argc - 3)
+  if (optind > argc - 1)
     {
       cout<<"Too few input arguments. At least dir and two scan numbers are required."<<endl;
       usage(argc, argv);
     }
   
   info.dir = argv[optind];
-  info.fScanNumber = atoi(argv[optind+1]);
+  //info.fScanNumber = atoi(argv[optind+1]);
-  info.sScanNumber = atoi(argv[optind+2]);
+  //info.sScanNumber = atoi(argv[optind+2]);
   if(info.outDir.empty()) info.outDir = info.dir;
   else if(info.outDir.compare(info.outDir.size()-1, 1, "/") != 0) info.outDir += "/";
 }
@@ -291,7 +295,6 @@ int main(int argc, char** argv){
   string out;
   cv::Mat outImage;
   parssArgs(argc, argv, info);
-  cout<<"out of parssArgs"<<endl;
   if(info.verbose >= 1) informationDescription(info);
 
   scan_cv fScan (info.dir, info.fScanNumber, info.sFormat);

src/slam6d/fbr/panorama.cc

@@ -134,6 +134,64 @@ namespace fbr{
       }
     }
 
+    //CONIC projection
+    if(pMethod == CONIC){
+      // set up maximum latitude and longitude angles of the robot 
+			double MIN_VERT_ANGLE = MIN_ANGLE * M_PI / 180.0, MAX_VERT_ANGLE = MAX_ANGLE * M_PI / 180.0,
+             MIN_HORIZ_ANGLE = -M_PI, MAX_HORIZ_ANGLE = M_PI;
+      // set up initial parameters according to MathWorld: http://mathworld.wolfram.com/AlbersEqual-AreaConicProjection.html
+      double Lat0 = 0., Long0 = 0.;
+      double Phi1 = -40. * M_PI / 180.0, Phi2 = 60 * M_PI / 180.0;
+      double n = (sin(Phi1) + sin(Phi2)) / 2.;
+      double C = sqr(cos(Phi1)) + 2 * n * sin(Phi1);
+      double Rho0 = sqrt(C - 2 * n * sin(Lat0)) / n;
+			// set up max values for x and y and add the longitude to x axis and latitude to y axis
+			double xmax = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MAX_HORIZ_ANGLE - Long0));
+			double xmin = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MIN_HORIZ_ANGLE - Long0));
+      double xFactor = (double) iWidth / ( xmax - xmin );
+      int widthMax = iWidth - 1;
+			double ymin = Rho0 - (1./n * sqrt(C - 2*n*sin(MIN_VERT_ANGLE)) ) * cos(n * ( 0. - Long0 ));
+			double ymax = Rho0 - (1./n * sqrt(C - 2*n*sin(MAX_VERT_ANGLE)) ) * cos(n * (MAX_HORIZ_ANGLE - Long0 ));
+      double yFactor = (double) iHeight / ( ymax - ymin );
+      //shift all the values to positive points on image 
+      int heightMax = iHeight - 1;
+      cv::MatIterator_<cv::Vec4f> it, end; 
+
+      for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); it != end; ++it){
+        double kart[3], polar[3], phi, theta, range;
+        kart[0] = (*it)[2]/100;
+        kart[1] = (*it)[0]/-100;
+        kart[2] = (*it)[1]/100;
+        toPolar(kart, polar);
+        //theta == polar[0] == scan [4]
+        //phi == polar[1] == scan [5]
+        //range == polar[2] == scan [3]
+        theta = polar[0] * 180 / M_PI;
+        phi = polar[1] * 180 / M_PI;
+        range = polar[2];
+        //phi == longitude == horizantal angle of view of [0:360] 
+        phi = 180.0 - phi;
+        phi *= M_PI / 180.0;
+        //theta == latitude == vertical angle of view of [-40:60]
+        theta -= 90;
+        theta *= -1;
+        theta *= M_PI / 180.0;
+
+				// add minimum x position as an offset
+        int x = (int) ( xFactor * (sqrt(C - 2 * n * sin( theta) ) / n * sin(n * (phi - Long0)) + fabs(xmin) ) );
+        if (x < 0) x = 0;
+        if (x > widthMax) x = widthMax;
+        
+        // add minimum y position as an offset
+        int y = (int) ( yFactor * (Rho0 - (1/n * sqrt(C - 2 * n * sin( theta) ) ) * cos(n * (phi - Long0)) + fabs( ymin ) ) );
+        y = heightMax - y;
+        if (y < 0) y = 0;
+        if (y > heightMax) y = heightMax;
+        //create the iReflectance iRange and map
+        map(x, y, it, range);
+      }
+    }
+    
     //CYLINDRICAL projection
     if(pMethod == CYLINDRICAL){
       //adding the longitude to x and tan(latitude) to y

src/slam6d/fbr/scan_cv.cc

@@ -25,10 +25,20 @@ namespace fbr{
   void scan_cv::convertScanToMat(){
     bool scanserver = false;
     Scan::openDirectory(scanserver, sDir, sFormat, sNumber, sNumber);
+    if(Scan::allScans.size() == 0){
+      cerr << "No scans found. Did you use the correct format?" <<endl;
+      exit(-1);
+    }
     cout<<"loading "<<sDir<<" with scan number " <<sNumber<<"."<<endl;
     Scan * source = * Scan::allScans.begin();
     DataXYZ xyz = source->get("xyz");
-    DataReflectance xyz_reflectance = source->get("reflectance");
+    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;
+    }
     nPoints = xyz.size();
     scan.create(nPoints,1,CV_32FC(4));
     scan = cv::Scalar::all(0); 

src/slam6d/managedScan.cc

@@ -120,6 +120,12 @@ void ManagedScan::setHeightFilter(double top, double bottom)
   m_shared_scan->setHeightParameters(top, bottom);
 }
 
+void ManagedScan::setRangeMutation(double range)
+{
+  m_shared_scan->setRangeMutationParameters(range);
+}
+
+
 void ManagedScan::setReductionParameter(double voxelSize, int nrpts, PointType pointtype)
 {
   Scan::setReductionParameter(voxelSize, nrpts, pointtype);

src/slam6d/pointfilter.cc

@@ -20,6 +20,8 @@ using std::runtime_error;
 using std::cout;
 using std::endl;
 
+#include <cmath>
+
 
 
 map<string, Checker* (*)(const string&)>* PointFilter::factory = new map<string, Checker* (*)(const string&)>;
@@ -77,6 +79,14 @@ PointFilter& PointFilter::setHeight(double top, double bottom)
   return *this;
 }
 
+PointFilter& PointFilter::setRangeMutator(double range)
+{
+  m_changed = true;
+  stringstream s_range; s_range << range;
+  m_params["rangemutation"] = s_range.str();
+  return *this;
+}
+
 std::string PointFilter::getParams()
 {
   stringstream s;
@@ -126,6 +136,7 @@ namespace {
   CheckerFactory<CheckerRangeMin> min("rangemin");
   CheckerFactory<CheckerHeightTop> top("heighttop");
   CheckerFactory<CheckerHeightBottom> bottom("heightbottom");
+  CheckerFactory<RangeMutator> range_mutation("rangemutation");
 }
 
 
@@ -185,3 +196,18 @@ bool CheckerHeightBottom::test(double* point) {
     return true;
   return false;
 }
+
+RangeMutator::RangeMutator(const std::string& value) {
+  stringstream s(value);
+  s >> m_range;
+}
+
+bool RangeMutator::test(double* point) {
+  double orig_range = sqrt(point[0]*point[0] + point[1]*point[1] + point[2]*point[2]);
+  double scale_mutation = m_range / orig_range;
+  point[0] *= scale_mutation;
+  point[1] *= scale_mutation;
+  point[2] *= scale_mutation;
+
+  return true;
+}