FeatureDetectorUtilities.hh

Go to the documentation of this file.

 
#ifndef __FEATURE_DETECTOR_UTILITIES_H__
#define __FEATURE_DETECTOR_UTILITIES_H__
 
#include "FeatureDetectorMessage.hh"
#include "FeatureDetectorMetaMessage.hh"
#include "FeatureDetectorConfig.hh"
 
using namespace crl::multisense;
using namespace crl::multisense::details;
 
namespace feature_detector
{
 
static CRL_CONSTEXPR DataSource Source_Feature_Left            = Source_Secondary_App_Data_0;
static CRL_CONSTEXPR DataSource Source_Feature_Right           = Source_Secondary_App_Data_1;
static CRL_CONSTEXPR DataSource Source_Feature_Aux             = Source_Secondary_App_Data_2;
static CRL_CONSTEXPR DataSource Source_Feature_Rectified_Left  = Source_Secondary_App_Data_3;
static CRL_CONSTEXPR DataSource Source_Feature_Rectified_Right = Source_Secondary_App_Data_4;
static CRL_CONSTEXPR DataSource Source_Feature_Rectified_Aux   = Source_Secondary_App_Data_5;
 
static CRL_CONSTEXPR int RECOMMENDED_MAX_FEATURES_FULL_RES    = 5000;
static CRL_CONSTEXPR int RECOMMENDED_MAX_FEATURES_QUARTER_RES = 1500;
 
class FeatureDetectorConfig: public crl::multisense::system::SecondaryAppConfig {
 
    private:
 
        FeatureDetectorConfigParams mConfigItems;
 
    public:
        uint32_t numberOfFeatures() const { return mConfigItems.numberOfFeatures; };
 
        bool grouping() const { return mConfigItems.grouping; };
 
        bool motion() const { return mConfigItems.motion; };
 
        uint32_t options() const { return mConfigItems.options; };
 
        void setNumberOfFeatures(const uint32_t &numberOfFeatures)    {
 
            if (numberOfFeatures > RECOMMENDED_MAX_FEATURES_FULL_RES)
            {
                std::cout << "WARNING: The number of features requested is above recommended level!" << '\n';
                std::cout << "If a performance impact is noticed reduce number of features and/or framerate of camera" << '\n';
                std::cout << "The recommended maximum camera settings when using the feature detector is:" << '\n';
                std::cout << "Quarter Res: 15FPS and 1500 Features" << '\n';
                std::cout << "Full    Res:  5FPS and 5000 Features" << '\n';
            }
 
            mConfigItems.numberOfFeatures = numberOfFeatures;
 
        };
 
        void setGrouping(const bool &g)    {
            mConfigItems.grouping = g;
        }
 
        void setMotion(const uint32_t &m)    {
            mConfigItems.motion = m;
        }
 
        void setOptions(const uint32_t &opt)
        {
            mConfigItems.options = opt;
        }
 
        FeatureDetectorConfig()
        {
            mConfigItems.numberOfFeatures = RECOMMENDED_MAX_FEATURES_QUARTER_RES;
            mConfigItems.grouping = true;
            mConfigItems.motion = 1;
            mConfigItems.options = 0;
        };
 
        void serialize( void )
        {
            memcpy(data, &mConfigItems, sizeof(mConfigItems));
            dataLength = sizeof(mConfigItems);
        }
 
};
 
#pragma pack(push,1)
 
struct Feature {
  uint16_t x;
  uint16_t y;
  uint8_t angle;
  uint8_t resp;
  uint8_t octave;
  uint8_t descriptor;
};
 
struct Descriptor {
  uint32_t d[8]; //Descriptor is 32 bytes
};
 
#pragma pack(pop)
 
class Header : public HeaderBase {
 
public:
 
    DataSource source;
    int64_t  frameId;
    uint32_t timeSeconds;
    uint32_t timeNanoSeconds;
    int64_t  ptpNanoSeconds;
    uint16_t octaveWidth;
    uint16_t octaveHeight;
    uint16_t numOctaves;
    uint16_t scaleFactor;
    uint16_t motionStatus;
    uint16_t averageXMotion;
    uint16_t averageYMotion;
    uint16_t numFeatures;
    uint16_t numDescriptors;
    uint16_t observerStatus;
    uint16_t observerNum;
    uint16_t observerIndex;
    int16_t observerDy;
    int16_t observerTheta;
    uint16_t affineCalCount;
    std::vector<Feature> * features;
    std::vector<Descriptor> * descriptors;
 
  Header()
  {
    features = new std::vector<Feature>;
    descriptors = new std::vector<Descriptor>;
  }
 
  ~Header()
  {
    delete features;
    delete descriptors;
  }
};
 
 
Status secondaryAppDataExtract(feature_detector::Header &header, const secondary_app::Header &orig)
{
  using namespace crl::multisense::details;
 
  utility::BufferStreamReader stream(reinterpret_cast<const uint8_t*>(orig.secondaryAppDataP), orig.secondaryAppDataLength);
  FeatureDetector featureDetector(stream, FeatureDetector::VERSION);
 
  utility::BufferStreamReader metaStream(reinterpret_cast<const uint8_t *>(orig.secondaryAppMetadataP), orig.secondaryAppMetadataLength);
  FeatureDetectorMeta _meta(metaStream, FeatureDetectorMeta::VERSION);
 
 
  header.source         = featureDetector.source;
  header.frameId        = _meta.frameId;
  header.timeSeconds    = _meta.timeSeconds;
  header.timeNanoSeconds= _meta.timeNanoSeconds;
  header.ptpNanoSeconds = _meta.ptpNanoSeconds;
  header.octaveWidth    = _meta.octaveWidth;
  header.octaveHeight   = _meta.octaveHeight;
  header.numOctaves     = _meta.numOctaves;
  header.scaleFactor    = _meta.scaleFactor;
  header.motionStatus   = _meta.motionStatus;
  header.averageXMotion = _meta.averageXMotion;
  header.averageYMotion = _meta.averageYMotion;
  header.observerStatus = _meta.observerStatus;
  header.observerNum    = _meta.observerNum;
  header.observerIndex  = _meta.observerIndex;
  header.observerDy     = _meta.observerDy;
  header.observerTheta  = _meta.observerTheta;
  header.affineCalCount = _meta.affineCalCount;
  header.numFeatures    = featureDetector.numFeatures;
  header.numDescriptors = featureDetector.numDescriptors;
 
  const size_t startDescriptor=featureDetector.numFeatures*sizeof(Feature);
 
  uint8_t * dataP = reinterpret_cast<uint8_t *>(featureDetector.dataP);
  for (size_t i = 0; i < featureDetector.numFeatures; i++) {
      feature_detector::Feature f = *reinterpret_cast<feature_detector::Feature *>(dataP + (i * sizeof(Feature)));
      header.features->push_back(f);
  }
 
  for (size_t j = 0;j < featureDetector.numDescriptors; j++) {
      feature_detector::Descriptor d = *reinterpret_cast<feature_detector::Descriptor *>(dataP + (startDescriptor + (j * sizeof(Descriptor))));
      header.descriptors->push_back(d);
  }
 
  return Status_Ok;
}
 
} // namespace util
 
 
#endif /* end of include guard: __FEATURE_DETECTOR_UTILITIES_H__ */