void Slic::_InitCenterFeature(int i, int j, FeatureVector &featureVec)
{
featureVec.clear();
uchar *buffer = new uchar[5*5*_dataSize*_bandCount];
_poSrcDS->RasterIO(GF_Read,j-2,i-2,5,5,buffer,5,5,_dataType,_bandCount,0,0,0,0);
double minEdge = std::numeric_limits<double>::max();
int minN;
int minM;
for (int n=1;n<4;++n)
{
for(int m=1;m<4;++m)
{
double edge = 0;
edge += (buffer[(n-1)*5+m]-buffer[(n+1)*5+m])*(buffer[(n-1)*5+m]-buffer[(n+1)*5+m]);
edge += (buffer[n*5+m-1]-buffer[n*5+m+1])*(buffer[n*5+m-1]-buffer[n*5+m+1]);
if (edge < minEdge)
{
minEdge = edge;
minN = n;
minM = m;
}
}
}
int centerIndex = minN*5+minM;
uchar* p = buffer;
for(int k=0;k<_bandCount;++k,p += (5*5*_dataSize))
{
featureVec.push_back( SRCVAL(p,_dataType,centerIndex)/_regularizer);
}
featureVec.push_back(static_cast<double>(j+minM-2)/_regionSize); //x
featureVec.push_back(static_cast<double>(i+minN-2)/_regionSize); //y
delete []buffer;
}
void ClassifySvmSharedCommand::readSharedRAbundVectors(vector<SharedRAbundVector*>& lookup, GroupMap& designMap, LabeledObservationVector& labeledObservationVector, FeatureVector& featureVector) {
for ( int j = 0; j < lookup.size(); j++ ) {
//i++;
vector<individual> data = lookup[j]->getData();
Observation* observation = new Observation(data.size(), 0.0);
string sharedGroupName = lookup[j]->getGroup();
string treatmentName = designMap.getGroup(sharedGroupName);
//std::cout << "shared group name: " << sharedGroupName << " treatment name: " << treatmentName << std::endl;
//labeledObservationVector.push_back(std::make_pair(treatmentName, observation));
labeledObservationVector.push_back(LabeledObservation(j, treatmentName, observation));
//std::cout << " j=" << j << " label : " << lookup[j]->getLabel() << " group: " << lookup[j]->getGroup();
for (int k = 0; k < data.size(); k++) {
//std::cout << " abundance " << data[k].abundance;
observation->at(k) = double(data[k].abundance);
if ( j == 0) {
featureVector.push_back(Feature(k, m->currentSharedBinLabels[k]));
}
}
//std::cout << std::endl;
// let this happen later?
//delete lookup[j];
}
}
bool CButtonMapXml::Deserialize(const TiXmlElement* pElement, FeatureVector& features)
{
const TiXmlElement* pFeature = pElement->FirstChildElement(BUTTONMAP_XML_ELEM_FEATURE);
if (!pFeature)
{
esyslog("Can't find <%s> tag", BUTTONMAP_XML_ELEM_FEATURE);
return false;
}
while (pFeature)
{
const char* name = pFeature->Attribute(BUTTONMAP_XML_ATTR_FEATURE_NAME);
if (!name)
{
esyslog("<%s> tag has no \"%s\" attribute", BUTTONMAP_XML_ELEM_FEATURE, BUTTONMAP_XML_ATTR_FEATURE_NAME);
return false;
}
std::string strName(name);
const TiXmlElement* pUp = nullptr;
const TiXmlElement* pDown = nullptr;
const TiXmlElement* pRight = nullptr;
const TiXmlElement* pLeft = nullptr;
const TiXmlElement* pPositiveX = nullptr;
const TiXmlElement* pPositiveY = nullptr;
const TiXmlElement* pPositiveZ = nullptr;
// Determine the feature type
JOYSTICK_FEATURE_TYPE type;
ADDON::DriverPrimitive primitive;
if (DeserializePrimitive(pFeature, primitive, strName))
{
type = JOYSTICK_FEATURE_TYPE_SCALAR;
}
else
{
pUp = pFeature->FirstChildElement(BUTTONMAP_XML_ELEM_UP);
pDown = pFeature->FirstChildElement(BUTTONMAP_XML_ELEM_DOWN);
pRight = pFeature->FirstChildElement(BUTTONMAP_XML_ELEM_RIGHT);
pLeft = pFeature->FirstChildElement(BUTTONMAP_XML_ELEM_LEFT);
if (pUp || pDown || pRight || pLeft)
{
type = JOYSTICK_FEATURE_TYPE_ANALOG_STICK;
}
else
{
pPositiveX = pFeature->FirstChildElement(BUTTONMAP_XML_ELEM_POSITIVE_X);
pPositiveY = pFeature->FirstChildElement(BUTTONMAP_XML_ELEM_POSITIVE_Y);
pPositiveZ = pFeature->FirstChildElement(BUTTONMAP_XML_ELEM_POSITIVE_Z);
if (pPositiveX || pPositiveY || pPositiveZ)
{
type = JOYSTICK_FEATURE_TYPE_ACCELEROMETER;
}
else
{
esyslog("Feature \"%s\": <%s> tag is not a valid primitive", strName.c_str(), BUTTONMAP_XML_ELEM_FEATURE);
return false;
}
}
}
ADDON::JoystickFeature feature(strName, type);
// Deserialize according to type
switch (type)
{
case JOYSTICK_FEATURE_TYPE_SCALAR:
{
feature.SetPrimitive(primitive);
break;
}
case JOYSTICK_FEATURE_TYPE_ANALOG_STICK:
{
ADDON::DriverPrimitive up;
ADDON::DriverPrimitive down;
ADDON::DriverPrimitive right;
ADDON::DriverPrimitive left;
bool bSuccess = true;
if (pUp && !DeserializePrimitive(pUp, up, strName))
{
esyslog("Feature \"%s\": <%s> tag is not a valid primitive", strName.c_str(), BUTTONMAP_XML_ELEM_UP);
bSuccess = false;
}
if (pDown && !DeserializePrimitive(pDown, down, strName))
{
esyslog("Feature \"%s\": <%s> tag is not a valid primitive", strName.c_str(), BUTTONMAP_XML_ELEM_DOWN);
bSuccess = false;
}
if (pRight && !DeserializePrimitive(pRight, right, strName))
{
esyslog("Feature \"%s\": <%s> tag is not a valid primitive", strName.c_str(), BUTTONMAP_XML_ELEM_RIGHT);
bSuccess = false;
}
if (pLeft && !DeserializePrimitive(pLeft, left, strName))
{
esyslog("Feature \"%s\": <%s> tag is not a valid primitive", strName.c_str(), BUTTONMAP_XML_ELEM_LEFT);
bSuccess = false;
}
if (!bSuccess)
return false;
feature.SetUp(up);
feature.SetDown(down);
feature.SetRight(right);
feature.SetLeft(left);
break;
}
case JOYSTICK_FEATURE_TYPE_ACCELEROMETER:
{
ADDON::DriverPrimitive positiveX;
ADDON::DriverPrimitive positiveY;
ADDON::DriverPrimitive positiveZ;
bool bSuccess = true;
if (pPositiveX && !DeserializePrimitive(pPositiveX, positiveY, strName))
{
esyslog("Feature \"%s\": <%s> tag is not a valid primitive", strName.c_str(), BUTTONMAP_XML_ELEM_POSITIVE_X);
bSuccess = false;
}
if (pPositiveY && !DeserializePrimitive(pPositiveY, positiveY, strName))
{
esyslog("Feature \"%s\": <%s> tag is not a valid primitive", strName.c_str(), BUTTONMAP_XML_ELEM_POSITIVE_Y);
bSuccess = false;
}
if (pPositiveZ && !DeserializePrimitive(pPositiveZ, positiveZ, strName))
{
esyslog("Feature \"%s\": <%s> tag is not a valid primitive", strName.c_str(), BUTTONMAP_XML_ELEM_POSITIVE_Z);
bSuccess = false;
}
if (!bSuccess)
return false;
feature.SetPositiveX(positiveX);
feature.SetPositiveY(positiveY);
feature.SetPositiveZ(positiveZ);
break;
}
default:
break;
}
features.push_back(feature);
pFeature = pFeature->NextSiblingElement(BUTTONMAP_XML_ELEM_FEATURE);
}
return true;
}
void
vtree_user::compute_features( const std::string& cloud_filename,
FeatureVector &feature_vector )
{
typedef pcl::PointXYZ nx_PointT;
typedef pcl::Normal nx_Normal;
typedef pcl::PointCloud<nx_PointT> nx_PointCloud;
typedef pcl::PointCloud<nx_Normal> nx_PointCloud_normal;
typedef pcl::PointCloud<int> nx_PointCloud_int;
typedef pcl::UniformSampling<nx_PointT> nx_Sampler;
typedef pcl::search::KdTree<nx_PointT> nx_SearchMethod;
typedef pcl::NormalEstimation<nx_PointT, nx_Normal> nx_NormalEst;
#if FEATURE == 1
typedef pcl::FPFHSignature33 nx_FeatureType;
typedef pcl::FPFHEstimation<nx_PointT, nx_Normal, nx_FeatureType> nx_FeatureEst;
#elif FEATURE == 2
typedef pcl::PFHSignature125 nx_FeatureType;
typedef pcl::PFHEstimation<nx_PointT, nx_Normal, nx_FeatureType> nx_FeatureEst;
#elif FEATURE == 3
typedef pcl::VFHSignature308 nx_FeatureType;
typedef pcl::VFHEstimation<nx_PointT, nx_Normal, nx_FeatureType> nx_FeatureEst;
#else
#error A valid feature definition is required!
#endif
typedef pcl::PointCloud<nx_FeatureType> nx_PointCloud_feature;
// load the file
nx_PointCloud::Ptr cld_ptr(new nx_PointCloud);
pcl::io::loadPCDFile<nx_PointT> ( cloud_filename, *cld_ptr);
ROS_INFO("[vtree_user] Starting keypoint extraction...");
clock_t tic = clock();
nx_PointCloud::Ptr keypoints( new nx_PointCloud);
nx_PointCloud_int::Ptr keypoint_idx(new nx_PointCloud_int);
nx_Sampler uniform_sampling;
uniform_sampling.setInputCloud ( cld_ptr );
uniform_sampling.setRadiusSearch ( keypoint_radius_ );
uniform_sampling.compute( *keypoint_idx );
pcl::copyPointCloud ( *cld_ptr, keypoint_idx->points, *keypoints);
ROS_INFO("[vtree_user] No of Keypoints found %d", static_cast<int>(keypoint_idx->size()) );
ROS_INFO("[vtree_user] Keypoint extraction took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
if( keypoints->empty() )
{
ROS_WARN("[vtree_user] No keypoints were found...");
return;
}
// Compute normals for the input cloud
ROS_INFO("[vtree_user] Starting normal extraction...");
tic = clock();
nx_PointCloud_normal::Ptr normals (new nx_PointCloud_normal);
nx_SearchMethod::Ptr search_method_xyz (new nx_SearchMethod);
nx_NormalEst norm_est;
norm_est.setInputCloud ( cld_ptr );
norm_est.setSearchMethod ( search_method_xyz );
norm_est.setRadiusSearch ( normal_radius_ );
norm_est.compute ( *normals );
ROS_INFO("[vtree_user] Normal extraction took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
// Get features at the computed keypoints
ROS_INFO("[vtree_user] Starting feature computation...");
tic = clock();
nx_PointCloud_feature::Ptr features(new nx_PointCloud_feature);
nx_FeatureEst feat_est;
feat_est.setInputCloud ( keypoints );
feat_est.setSearchSurface ( cld_ptr );
feat_est.setInputNormals ( normals );
search_method_xyz.reset(new nx_SearchMethod);
feat_est.setSearchMethod ( search_method_xyz );
feat_est.setRadiusSearch ( feature_radius_ );
feat_est.compute ( *features );
ROS_INFO("[vtree_user] No of Features found %d", static_cast<int>(features->size()) );
ROS_INFO("[vtree_user] Feature computation took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
// Rectify the historgram values to ensure they are in [0,100] and create a document
for( nx_PointCloud_feature::iterator iter = features->begin();
iter != features->end(); ++iter)
{
rectify_histogram( iter->histogram );
feature_vector.push_back( FeatureHist( iter->histogram ) );
}
}
void FaceFeaturesBlock::update()
{
try
{
const Image &image = m_imageIn.getReadableRef< Image >();
if( m_haarDetectionDownscaleIn.hasChanged() )
m_faceDetection->greyImgSmlScale( m_haarDetectionDownscaleIn.getReadableRef< double >() );
if( m_cascFileFaceIn.hasChanged() )
m_faceDetection->loadFaceCascade( m_cascFileFaceIn.getReadableRef< std::string >() );
if( m_cascFileEyesIn.hasChanged() )
m_faceDetection->loadEyesCascade( m_cascFileEyesIn.getReadableRef< std::string >() );
if( m_cascFileNoseIn.hasChanged() )
m_faceDetection->loadNoseCascade( m_cascFileNoseIn.getReadableRef< std::string >() );
if( m_cascFileMouthIn.hasChanged() )
m_faceDetection->loadMouthCascade( m_cascFileMouthIn.getReadableRef< std::string >() );
if( m_haarMinNeighboursFaceIn.hasChanged() )
m_faceDetection->minNeighboursFace( m_haarMinNeighboursFaceIn.getReadableRef< unsigned int >() );
if( m_haarMinNeighboursEyesIn.hasChanged() )
m_faceDetection->minNeighboursEyes( m_haarMinNeighboursEyesIn.getReadableRef< unsigned int >() );
if( m_haarMinNeighboursNoseIn.hasChanged() )
m_faceDetection->minNeighboursNose( m_haarMinNeighboursNoseIn.getReadableRef< unsigned int >() );
if( m_haarMinNeighboursMouthIn.hasChanged() )
m_faceDetection->minNeighboursMouth( m_haarMinNeighboursMouthIn.getReadableRef< unsigned int >() );
if( m_haarMinSizeFaceIn.hasChanged() )
m_faceDetection->minSizeFace( m_haarMinSizeFaceIn.getReadableRef< Vec2 >() );
if( m_haarMinSizeEyesIn.hasChanged() )
m_faceDetection->minSizeEyes( m_haarMinSizeEyesIn.getReadableRef< Vec2 >() );
if( m_haarMinSizeNoseIn.hasChanged() )
m_faceDetection->minSizeNose( m_haarMinSizeNoseIn.getReadableRef< Vec2 >() );
if( m_haarMinSizeMouthIn.hasChanged() )
m_faceDetection->minSizeMouth( m_haarMinSizeMouthIn.getReadableRef< Vec2 >() );
if( m_equalizeHistogramIn.hasChanged() )
m_faceDetection->equalizeHist( m_equalizeHistogramIn.getReadableRef< bool >() );
if( m_haarScaleFactorIn.hasChanged() )
m_faceDetection->haarScaleFactor( m_haarScaleFactorIn.getReadableRef< double >() );
if( m_imageIn.hasUpdated() )
{
double dt = m_stopWatch->milliSeconds();
m_timeAccu += dt;
m_timeOverall += dt;
if( m_timeAccu > 1.0 )
{
m_frames = 0;
m_timeAccu = 0.0;
}
m_stopWatch->restart();
m_frames++;
FeatureVector faces;
m_faceDetection->detectFaces( image, faces );
m_faceTracking->track( faces,
m_affinityWeightPosIn.getReadableRef<double>(),
m_affinityWeightSizeIn.getReadableRef<double>(),
m_affinityThresholdIn.getReadableRef<double>(),
m_coherenceWeightDirIn.getReadableRef<double>(),
m_coherenceWeightVelIn.getReadableRef<double>(),
m_coherenceWeightSizeIn.getReadableRef<double>(),
m_coherenceThresholdIn.getReadableRef<double>(),
m_coherenceToleranceDirIn.getReadableRef<double>() / image.getWidth(),
m_coherenceToleranceVelIn.getReadableRef<double>() / image.getWidth(),
m_coherenceToleranceSizeIn.getReadableRef<double>() / image.getWidth(),
m_discardThresholdIn.getReadableRef<double>(),
m_extrapolationDampingIn.getReadableRef<double>(),
m_extrapolationCoherenceRiseIn.getReadableRef<double>(),
m_extrapolationMinSizeFace,
m_timeOverall );
if( m_faceTracking->trackingInfoList().size() )
{
for( TrackingInfoList::iterator itT = m_faceTracking->trackingInfoList().begin(); itT != m_faceTracking->trackingInfoList().end(); ++itT )
{
if( itT->getFaceTrajectory().getEntryCount() )
{
FeatureVector eyesCandidates;
FeatureVector noseCandidates;
FeatureVector mouthCandidates;
m_faceDetection->detectFeatures( image, itT->getFaceTrajectory().getLastRegion(),
eyesCandidates, noseCandidates, mouthCandidates,
m_useEyesIn.getReadableRef< bool >(),
m_useNoseIn.getReadableRef< bool >(),
m_useMouthIn.getReadableRef< bool >() );
itT->addFromFeatureCandidates( eyesCandidates, noseCandidates, mouthCandidates, m_timeOverall, m_affinityWeightPosIn.getReadableRef<double>(), m_affinityWeightSizeIn.getReadableRef<double>() );
}
}
std::vector< FaceDesc > &faces = m_faceOut.getWriteableRef< std::vector< FaceDesc > >();
faces.clear();
for( TrackingInfoList::iterator itT = m_faceTracking->trackingInfoList().begin(); itT != m_faceTracking->trackingInfoList().end(); ++itT )
{
FaceDesc desc( itT->getUserID(), itT->getFaceTrajectory().getLastRegion() );
if( m_useEyesIn.getReadableRef< bool >() )
{
if( itT->getEyeLeftTrajectory().getEntryCount() )
{
desc.eyeLeft( FeatureDesc( itT->getEyeLeftTrajectory().getLastRegion() ) );
}
if( itT->getEyeRightTrajectory().getEntryCount() )
{
desc.eyeRight( FeatureDesc( itT->getEyeRightTrajectory().getLastRegion() ) );
}
}
if( m_useNoseIn.getReadableRef< bool >() && itT->getNoseTrajectory().getEntryCount() )
{
desc.nose( FeatureDesc( itT->getNoseTrajectory().getLastRegion() ) );
}
if( m_useMouthIn.getReadableRef< bool >() && itT->getNoseTrajectory().getEntryCount() )
{
desc.mouth( FeatureDesc( itT->getMouthTrajectory().getLastRegion() ) );
}
faces.push_back( desc );
}
}
else
m_faceOut.discard();
}
else
{
m_faceOut.discard();
}
}
catch( Exception & e )
{
cout << e.message() << " " << e.what() << endl;
e.rethrow();
}
catch( std::exception & e )
{
cout << e.what() << endl;
}
}
