static List GetPointsWithNNeighbors(int n, BinaryMap * image, List minutiae)
{
for ( int i=0; i < image->width; ++i ) {
for ( int j=0; j < image->height; ++j ) {
if ( CountNeighbors(i, j, image) == n && BinaryMap_GetBit(image, i, j) == 1) {
Minutia * minutia = calloc(1, sizeof(*minutia));
minutia->minutiaType = GetMinutiaType(n);
minutia->position = (Point) {
.x = i, .y = j
};
List_AddData(&minutiae, minutia);
minutiaeLocations.data[i][j] = minutia->minutiaType;
}
}
}
void CProprioceptiveFeatureVector::ComputeFeatureValues_onlynbrsandactuators()
{
Real f1, f2, f3, f4, f5, f6;
unsigned unNbrCount = CountNeighbors(FEATURE_RANGE);
unsigned unNbrCount1 = CountNeighbors(15.0f);
unsigned unNbrCount2 = CountNeighbors(30.0f);
unsigned unNbrCount3 = CountNeighbors(45.0f);
/*
* Time since the robot was first observed
*/
Real CurrentStepNumber = m_sSensoryData.m_rTime;
unsigned num_nbrs_threshold = 1u; Real queue_length_threshold = 0.5f;
/*f1 = TrackNeighborsInQueue(CurrentStepNumber, unNbrCount, num_nbrs_threshold,
m_iShortTimeWindowLength, queue_length_threshold,
m_unSumTimeStepsNbrs_ShortRangeTimeWindow, m_unQueueIndex_ShortRangeTimeWindow, m_punNbrs_ShortRangeTimeWindow);
f2 = TrackNeighborsInQueue(CurrentStepNumber, unNbrCount, num_nbrs_threshold,
m_iMediumTimeWindowLength, queue_length_threshold,
m_unSumTimeStepsNbrs_MediumRangeTimeWindow, m_unQueueIndex_MediumRangeTimeWindow, m_punNbrs_MediumRangeTimeWindow);
f3 = TrackNeighborsInQueue(CurrentStepNumber, unNbrCount, num_nbrs_threshold,
m_iLongTimeWindowLength, queue_length_threshold,
m_unSumTimeStepsNbrs_LongRangeTimeWindow, m_unQueueIndex_LongRangeTimeWindow, m_punNbrs_LongRangeTimeWindow);*/
queue_length_threshold = 0.25f;
f1 = TrackNeighborsInQueue(CurrentStepNumber, unNbrCount1, num_nbrs_threshold,
m_iLongTimeWindowLength, queue_length_threshold,
m_unSumTimeStepsNbrs_ShortRangeTimeWindow, m_unQueueIndex_ShortRangeTimeWindow, m_punNbrs_ShortRangeTimeWindow);
queue_length_threshold = 0.5f;
f2 = TrackNeighborsInQueue(CurrentStepNumber, unNbrCount1, num_nbrs_threshold,
m_iLongTimeWindowLength, queue_length_threshold,
m_unSumTimeStepsNbrs_MediumRangeTimeWindow, m_unQueueIndex_MediumRangeTimeWindow, m_punNbrs_MediumRangeTimeWindow);
queue_length_threshold = 0.75f;
f3 = TrackNeighborsInQueue(CurrentStepNumber, unNbrCount1, num_nbrs_threshold,
m_iLongTimeWindowLength, queue_length_threshold,
m_unSumTimeStepsNbrs_LongRangeTimeWindow, m_unQueueIndex_LongRangeTimeWindow, m_punNbrs_LongRangeTimeWindow);
m_fEstimated_Dist_ShortTimeWindow = TrackRobotDisplacement(CurrentStepNumber, vec_RobPos_ShortRangeTimeWindow);
m_fEstimated_Dist_MediumTimeWindow = TrackRobotDisplacement(CurrentStepNumber, vec_RobPos_MediumRangeTimeWindow);
m_fEstimated_Dist_LongTimeWindow = TrackRobotDisplacement(CurrentStepNumber, vec_RobPos_LongRangeTimeWindow);
/*if (m_sSensoryData.m_unRobotId == 1u)
{
std::cout << " Dist - short time window " << m_fEstimated_Dist_ShortTimeWindow << std::endl;
std::cout << " Dist - medium time window " << m_fEstimated_Dist_MediumTimeWindow << std::endl;
std::cout << " Dist - long time window " << m_fEstimated_Dist_LongTimeWindow << std::endl;
}*/
Real disp_ShortWindow_Threshold = 0.25f;
Real disp_MediumWindow_Threshold = 0.25f;
Real disp_LongWindow_Threshold = 0.25f;
// MaxLinearSpeed in cm / control cycle
// f4 = (m_fEstimated_Dist_ShortTimeWindow > (disp_ShortWindow_Threshold * ((Real)m_iShortTimeWindowLength) * m_sRobotData.MaxLinearSpeed)) ? 1.0f: 0.0f;
// f5 = (m_fEstimated_Dist_MediumTimeWindow > (disp_MediumWindow_Threshold * ((Real)m_iMediumTimeWindowLength) * m_sRobotData.MaxLinearSpeed)) ? 1.0f: 0.0f;
// f6 = (m_fEstimated_Dist_LongTimeWindow > (disp_LongWindow_Threshold * ((Real)m_iLongTimeWindowLength) * m_sRobotData.MaxLinearSpeed)) ? 1.0f: 0.0f;
/*
* Better to have a fixed absolute threshold instead of a fixed percentage as small percentages at large timewindows would be greatly affected by noise for the small time windows.
*/
f4 = (m_fEstimated_Dist_ShortTimeWindow > (4.0f)) ? 1.0f: 0.0f;
f5 = (m_fEstimated_Dist_MediumTimeWindow > (4.0f)) ? 1.0f: 0.0f;
f6 = (m_fEstimated_Dist_LongTimeWindow > (4.0f)) ? 1.0f: 0.0f;
m_pfFeatureValues[0] = f1;
m_pfFeatureValues[1] = f2;
m_pfFeatureValues[2] = f3;
m_pfFeatureValues[3] = f4;
m_pfFeatureValues[4] = f5;
m_pfFeatureValues[5] = f6;
assert(CProprioceptiveFeatureVector::NUMBER_OF_FEATURES == 6);
}
void CProprioceptiveFeatureVector::ComputeFeatureValues_NoAngularVelocityUsed()
{
Real f1, f2, f5, f6;
unsigned unNbrCount1 = CountNeighbors(5.0f);
unsigned unNbrCount2 = CountNeighbors(10.0f);
/*
* Time since the robot was first observed
*/
Real CurrentStepNumber = m_sSensoryData.m_rTime;
// Sensors
//1st: set if bot has atleast one neighbor in range 0-30cm in the majority of of past X time-steps
//2nd: set if bot has atleast one neighbor in range 30-60cm in the majority of of past X time-steps
unsigned num_nbrs_threshold = 2u; Real queue_length_threshold;
queue_length_threshold = 0.750f;
f1 = TrackNeighborsInQueue(CurrentStepNumber, unNbrCount1, num_nbrs_threshold,
m_iLongTimeWindowLength, queue_length_threshold,
m_unSumTimeStepsNbrs_ShortRangeTimeWindow, m_unQueueIndex_ShortRangeTimeWindow, m_punNbrs_ShortRangeTimeWindow);
queue_length_threshold = 0.750f;
f2 = TrackNeighborsInQueue(CurrentStepNumber, unNbrCount2-unNbrCount1, num_nbrs_threshold,
m_iLongTimeWindowLength, queue_length_threshold,
m_unSumTimeStepsNbrs_MediumRangeTimeWindow, m_unQueueIndex_MediumRangeTimeWindow, m_punNbrs_MediumRangeTimeWindow);
Real tmp = TrackRobotDisplacement(CurrentStepNumber, vec_RobPos_ShortRangeTimeWindow);
m_sSensoryData.vec_linearspeeds[m_sSensoryData.vec_linearspeeds_index] = tmp;
m_sSensoryData.LinearAcceleration = tmp - m_sSensoryData.vec_linearspeeds[(m_sSensoryData.vec_linearspeeds_index + 1u)%m_sSensoryData.vec_linearspeeds.size()];
m_sSensoryData.vec_linearspeeds_index = (m_sSensoryData.vec_linearspeeds_index + 1) % m_sSensoryData.vec_linearspeeds.size();
// Sensors-motor interactions
// Set if the occurance of the following event, atleast once in time window X
// 3rd: distance to nbrs 0-6 && change in angular acceleration
// 4th: no neighbors detected && change in angular acceleration
bool neighbours_present = (unNbrCount2>0)?true:false;
if(neighbours_present &&
(m_sSensoryData.LinearAcceleration >= 1.5f ||
m_sSensoryData.LinearAcceleration <= -1.5f))
{
m_piLastOccuranceEvent[2] = CurrentStepNumber;
}
if(!neighbours_present &&
(m_sSensoryData.LinearAcceleration >= 1.5f ||
m_sSensoryData.LinearAcceleration <= -1.5f))
{
m_piLastOccuranceEvent[3] = CurrentStepNumber;
}
for(unsigned int featureindex = 2; featureindex <=3; featureindex++)
{
if ((((int)CurrentStepNumber) - m_piLastOccuranceEvent[featureindex]) <= 300u)
{
m_pfAllFeatureValues[featureindex] = 1.0;
}
else
{
m_pfAllFeatureValues[featureindex] = 0.0;
}
}
m_fEstimated_Dist_MediumTimeWindow = TrackRobotDisplacement(CurrentStepNumber, vec_RobPos_MediumRangeTimeWindow);
m_fEstimated_Dist_LongTimeWindow = TrackRobotDisplacement(CurrentStepNumber, vec_RobPos_LongRangeTimeWindow);
/*if (m_sSensoryData.m_unRobotId == 8u)
{
std::cout << " LinearAcceleration " << m_sSensoryData.LinearAcceleration << std::endl;
std::cout << " Dist - short time window " << tmp << std::endl;
std::cout << " Dist - medium time window " << m_fEstimated_Dist_MediumTimeWindow << std::endl;
std::cout << " Dist - long time window " << m_fEstimated_Dist_LongTimeWindow << std::endl;
}*/
// Motors
//5th: distance travelled by bot in past 5s and 10s. Higher than 5% of max-possible distance travelled is accepted as feature=1.
Real disp_MediumWindow_Threshold = 0.20f;
Real disp_LongWindow_Threshold = 0.20f;
// MaxLinearSpeed in cm / control cycle
f5 = (m_fEstimated_Dist_MediumTimeWindow > (disp_MediumWindow_Threshold * ((Real)m_iMediumTimeWindowLength) * m_sRobotData.MaxLinearSpeed)) ? 1.0f: 0.0f;
f6 = (m_fEstimated_Dist_LongTimeWindow > (disp_LongWindow_Threshold * ((Real)m_iLongTimeWindowLength) * m_sRobotData.MaxLinearSpeed)) ? 1.0f: 0.0f;
m_pfFeatureValues[0] = f1;
m_pfFeatureValues[1] = f2;
m_pfFeatureValues[2] = m_pfAllFeatureValues[2];
m_pfFeatureValues[3] = m_pfAllFeatureValues[3];
m_pfFeatureValues[4] = f5;
m_pfFeatureValues[5] = f6;
assert(CProprioceptiveFeatureVector::NUMBER_OF_FEATURES == 6);
}
void CProprioceptiveFeatureVector::ComputeFeatureValues()
{
CProprioceptiveFeatureVector::FEATURE_RANGE = 30.0f;
unsigned unCloseRangeNbrCount = CountNeighbors(FEATURE_RANGE/2.0f);
unsigned unFarRangeNbrCount = CountNeighbors(FEATURE_RANGE) - unCloseRangeNbrCount;
bool neighbours_present = ((unCloseRangeNbrCount + unFarRangeNbrCount) > 0) ? true : false;
int CurrentStepNumber = m_sSensoryData.m_rTime;
// Feature (from LS to MS bits in FV)
// Sensors
//1st: set if bot has atleast one neighbor in range 0-30cm in the majority of of past X time-steps
//2nd: set if bot has atleast one neighbor in range 30-60cm in the majority of of past X time-steps
if(CurrentStepNumber >= m_iEventSelectionTimeWindow)
{
// decision based on the last X time-steps
if(m_unSumTimeStepsNbrsRange0to30 > (unsigned)(0.5*(double)m_iEventSelectionTimeWindow))
m_pfAllFeatureValues[0] = 1.0;
else
m_pfAllFeatureValues[0] = 0.0;
if(m_unSumTimeStepsNbrsRange30to60 > (unsigned)(0.5*(double)m_iEventSelectionTimeWindow))
m_pfAllFeatureValues[1] = 1.0;
else
m_pfAllFeatureValues[1] = 0.0;
// removing the fist entry of the moving time window from the sum
m_unSumTimeStepsNbrsRange0to30 -= m_punNbrsRange0to30AtTimeStep[m_unNbrsCurrQueueIndex];
m_unSumTimeStepsNbrsRange30to60 -= m_punNbrsRange30to60AtTimeStep[m_unNbrsCurrQueueIndex];
}
// adding new values into the queue
if (unCloseRangeNbrCount > 0)
{
m_punNbrsRange0to30AtTimeStep[m_unNbrsCurrQueueIndex] = 1;
m_unSumTimeStepsNbrsRange0to30++;
}
else
m_punNbrsRange0to30AtTimeStep[m_unNbrsCurrQueueIndex] = 0;
if (unFarRangeNbrCount > 0)
{
m_punNbrsRange30to60AtTimeStep[m_unNbrsCurrQueueIndex] = 1;
m_unSumTimeStepsNbrsRange30to60++;
}
else
m_punNbrsRange30to60AtTimeStep[m_unNbrsCurrQueueIndex] = 0;
m_unNbrsCurrQueueIndex = (m_unNbrsCurrQueueIndex + 1) % m_iEventSelectionTimeWindow;
// Sensors-motor interactions
// Set if the occurance of the following event, atleast once in time window X
// 3rd: distance to nbrs 0-6 && change in angular acceleration
// 4th: no neighbors detected && change in angular acceleration
if(neighbours_present &&
(m_sSensoryData.AngularAcceleration > m_tAngularAccelerationThreshold ||
m_sSensoryData.AngularAcceleration < -m_tAngularAccelerationThreshold))
{
m_piLastOccuranceEvent[2] = CurrentStepNumber;
}
if(!neighbours_present &&
(m_sSensoryData.AngularAcceleration > m_tAngularAccelerationThreshold ||
m_sSensoryData.AngularAcceleration < -m_tAngularAccelerationThreshold))
{
m_piLastOccuranceEvent[3] = CurrentStepNumber;
}
for(unsigned int featureindex = 2; featureindex <=3; featureindex++)
{
if ((CurrentStepNumber - m_piLastOccuranceEvent[featureindex]) <= m_iEventSelectionTimeWindow)
{
m_pfAllFeatureValues[featureindex] = 1.0;
}
else
{
m_pfAllFeatureValues[featureindex] = 0.0;
}
}
// Motors
//5th: distance travelled by bot in past 100 time-steps. Higher than 5% of max-possible distance travelled is accepted as feature=1.
CVector2 vecAgentPos = m_sSensoryData.pos;
m_fCumulativeDistTravelled += m_sSensoryData.dist;
if(CurrentStepNumber >= m_iDistTravelledTimeWindow)
{
// distance travelled in last 100 time-steps
m_fSquaredDistTravelled = (vecAgentPos.GetX() - m_pvecCoordAtTimeStep[m_unCoordCurrQueueIndex].GetX()) *
(vecAgentPos.GetX() - m_pvecCoordAtTimeStep[m_unCoordCurrQueueIndex].GetX()) +
(vecAgentPos.GetY() - m_pvecCoordAtTimeStep[m_unCoordCurrQueueIndex].GetY()) *
(vecAgentPos.GetY() - m_pvecCoordAtTimeStep[m_unCoordCurrQueueIndex].GetY());
//pos_ref = m_pvecCoordAtTimeStep[m_unCoordCurrQueueIndex];
// removing the distance travelled in the first time-step of the moving time-window from the queue
m_fCumulativeDistTravelled -= m_pfDistAtTimeStep[m_unCoordCurrQueueIndex];
// decision based on distance travelled in the last 100 time-steps
/*if(m_fCumulativeDistTravelled >= m_fCumulativeDistThreshold)
m_pfAllFeatureValues[4] = 1.0;
else
m_pfAllFeatureValues[4] = 0.0;*/
/*if(m_fSquaredDistTravelled >= m_fSquaredDistThreshold)
m_pfAllFeatureValues[4] = 1.0;
else
m_pfAllFeatureValues[4] = 0.0;*/
}
m_fEstimated_Dist_LongTimeWindow = TrackRobotDisplacement(CurrentStepNumber, vec_RobPos_LongRangeTimeWindow);
m_pfAllFeatureValues[4] = (m_fEstimated_Dist_LongTimeWindow > sqrt(m_fSquaredDistThreshold)) ? 1.0f: 0.0f;
//std::cout << " cumulative distance " << m_fCumulativeDistTravelled << std::endl;
//std::cout << " LinearSpeed " << m_sSensoryData.LinearSpeed << " angular speed " << m_sSensoryData.AngularSpeed << std::endl;
//std::cout << " LinearAcceleration " << m_sSensoryData.LinearAcceleration << " angular acceleration " << m_sSensoryData.AngularAcceleration << std::endl;
// adding new coordinate values into the queue
m_pvecCoordAtTimeStep[m_unCoordCurrQueueIndex] = vecAgentPos;
// adding distance travelled at last time-step into queue
m_pfDistAtTimeStep[m_unCoordCurrQueueIndex] = m_sSensoryData.dist;
m_unCoordCurrQueueIndex = (m_unCoordCurrQueueIndex + 1) % m_iDistTravelledTimeWindow;
if((m_sSensoryData.AngularAcceleration > m_tAngularAccelerationThreshold ||
m_sSensoryData.AngularAcceleration < -m_tAngularAccelerationThreshold))
{
m_piLastOccuranceEvent[5] = CurrentStepNumber;
}
// 6th: set if the robot changed its angular speed (per control cycle) atleast once in the past time-window.
if ((CurrentStepNumber - m_piLastOccuranceEvent[5]) <= m_iEventSelectionTimeWindow)
{
m_pfAllFeatureValues[5] = 1.0;
}
else
{
m_pfAllFeatureValues[5] = 0.0;
}
//6th: linear speed, higher than 5% of max. speed is accepted as feature=1 OR angular speed higher than 5% of max. angular speed is accepted as feature=1
/*m_pfAllFeatureValues[5] = (m_sSensoryData.LinearSpeed >= m_fVelocityThreshold) ? 1.0:0.0;*/
// adding the selected features into the feature vector
for(size_t i = 0; i < NUMBER_OF_FEATURES; ++i)
m_pfFeatureValues[i] = m_pfAllFeatureValues[i];
}
