MovementDetector.cpp

#include "MovementDetector.h"

GRT_BEGIN_NAMESPACE

MovementDetector::MovementDetector( const UINT numDimensions, const Float upperThreshold, const Float lowerThreshold, const Float gamma, const UINT searchTimeout ) {
 classType = "MovementDetector";
 infoLog.setProceedingText("[MovementDetector]");
 debugLog.setProceedingText("[DEBUG MovementDetector]");
 errorLog.setProceedingText("[ERROR MovementDetector]");
 trainingLog.setProceedingText("[TRAINING MovementDetector]");
 warningLog.setProceedingText("[WARNING MovementDetector]");

 this->numInputDimensions = numDimensions;
 this->numOutputDimensions = 1;
 this->upperThreshold = upperThreshold;
 this->lowerThreshold = lowerThreshold;
 this->gamma = gamma;
 this->searchTimeout = searchTimeout;
 this->trained = true;
 
 reset();
}
 
MovementDetector::~MovementDetector(){
 
}
 
bool MovementDetector::predict_( VectorFloat &input ){
 
 movementDetected = false;
 noMovementDetected = false;
 
 if( !trained ){
 errorLog << "predict_(VectorFloat &input) - AdaBoost Model Not Trained!" << std::endl;
 return false;
 }
 
 if( input.size() != numInputDimensions ){
 errorLog << "predict_(VectorFloat &input) - The size of the input vector (" << input.size() << ") does not match the num features in the model (" << numInputDimensions << std::endl;
 return false;
 }
 
 //Compute the movement index, unless we are in the first sample
 Float x = 0;
 if( !firstSample ){
 for(UINT n=0; n<numInputDimensions; n++){
 x += SQR( input[n] - lastSample[n] );
 }
 movementIndex = (movementIndex*gamma) + sqrt( x );
 }
 
 //Flag that this is not the first sample and store the input for the next prediction
 firstSample = false;
 lastSample = input;
 
 switch( state ){
 case SEARCHING_FOR_MOVEMENT:
 if( movementIndex >= upperThreshold ){
 movementDetected = true;
 state = SEARCHING_FOR_NO_MOVEMENT;
 }
 break;
 case SEARCHING_FOR_NO_MOVEMENT:
 if( movementIndex < lowerThreshold ){
 noMovementDetected = true;
 state = SEARCH_TIMEOUT;
 searchTimer.start();
 }
 break;
 case SEARCH_TIMEOUT:
 // searchTimeout is cast because of a C4018 warning on visual (signed/unsigned incompatibility)
 if( searchTimer.getMilliSeconds() >= (signed long)searchTimeout ){
 state = SEARCH_TIMEOUT;
 searchTimer.stop();
 }
 break;
 }
 
 
 return true;
}

bool MovementDetector::clear(){
 
 trained = false;
 lastSample.clear();
 
 reset();
 
 return true;
}

bool MovementDetector::reset(){
 
 state = SEARCH_TIMEOUT;
 searchTimer.start();
 firstSample = true;
 movementDetected = false;
 noMovementDetected = false;
 movementIndex = 0;
 
 return true;
}

bool MovementDetector::saveModelToFile( std::fstream &file ) const{
 
 //Write the header info
 file << "GRT_MOVEMENT_DETECTOR_MODEL_FILE_V1.0\n";
 
 //Write the base settings to the file
 if( !MLBase::saveBaseSettingsToFile(file) ){
 errorLog <<"saveModelToFile(fstream &file) - Failed to save ML base settings to file!" << std::endl;
 return false;
 }
 
 //Write the detector variables
 file << "SearchTimeout: " << searchTimeout << std::endl;
 file << "UpperThreshold: " << upperThreshold << std::endl;
 file << "LowerThreshold: " << lowerThreshold << std::endl;
 file << "Gamma: " << gamma << std::endl;
 
 return true;
}

bool MovementDetector::loadModelFromFile( std::fstream &file ){
 
 clear();
 
 if(!file.is_open())
 {
 errorLog << "loadModelFromFile(string filename) - Could not open file to load model!" << std::endl;
 return false;
 }
 
 std::string word;
 file >> word;
 
 //Write the header info
 if( word != "GRT_MOVEMENT_DETECTOR_MODEL_FILE_V1.0" ){
 errorLog <<"loadModelFromFile(fstream &file) - Failed to read file header!" << std::endl;
 return false;
 }
 
 //Load the base settings from the file
 if( !MLBase::loadBaseSettingsFromFile(file) ){
 errorLog << "loadModelFromFile(string filename) - Failed to load base settings from file!" << std::endl;
 return false;
 }
 
 file >> word;
 if( word != "SearchTimeout:" ){
 errorLog <<"loadModelFromFile(fstream &file) - Failed to read SearchTimeout header!" << std::endl;
 return false;
 }
 file >> searchTimeout;
 
 file >> word;
 if( word != "UpperThreshold:" ){
 errorLog <<"loadModelFromFile(fstream &file) - Failed to read UpperThreshold header!" << std::endl;
 return false;
 }
 file >> upperThreshold;
 
 file >> word;
 if( word != "LowerThreshold:" ){
 errorLog <<"loadModelFromFile(fstream &file) - Failed to read LowerThreshold header!" << std::endl;
 return false;
 }
 file >> lowerThreshold;
 
 file >> word;
 if( word != "Gamma:" ){
 errorLog <<"loadModelFromFile(fstream &file) - Failed to read Gamma header!" << std::endl;
 return false;
 }
 file >> gamma;
 
 return true;
}

Float MovementDetector::getUpperThreshold() const {
 return upperThreshold;
}

Float MovementDetector::getLowerThreshold() const {
 return lowerThreshold;
}

Float MovementDetector::getMovementIndex() const {
 return movementIndex;
}

Float MovementDetector::getGamma() const {
 return gamma;
}

bool MovementDetector::getMovementDetected() const {
 return movementDetected;
}

bool MovementDetector::getNoMovementDetect() const {
 return noMovementDetected;
}

UINT MovementDetector::getState() const {
 return state;
}

UINT MovementDetector::getSearchTimeout() const {
 return searchTimeout;
}

bool MovementDetector::setUpperThreshold(const Float upperThreshold) {
 this->upperThreshold = upperThreshold;
 return true;
}

bool MovementDetector::setLowerThreshold(const Float lowerThreshold) {
 this->lowerThreshold = lowerThreshold;
 return true;
}

bool MovementDetector::setGamma(const Float gamma) {
 this->gamma = gamma;
 return true;
}

bool MovementDetector::setSearchTimeout(const UINT searchTimeout){
 this->searchTimeout = searchTimeout;
 return true;
}

GRT_END_NAMESPACE