void addFeatures( Node *node, bool triangulated, std::vector<Feature*> &features )
{
if ( node->camera != NULL )
{
addFeatures( node->camera, triangulated, features );
}
ElementList::iterator it;
for ( it = node->children.begin(); it != node->children.end(); it++ )
{
Node *child = (Node *)it->second;
addFeatures( child, triangulated, features );
}
}
void FeatureMatcher::init( Node *node, bool triangulated, bool averageDescriptors )
{
if ( !features.empty() ) {
delete [] data;
data = NULL;
if ( deleteFeatures ) {
for ( int i = 0; i < features.size(); i++ ) {
delete [] features[i]->descriptor;
delete features[i];
}
}
features.clear();
}
addFeatures( node, triangulated, features );
if ( features.empty() ) return;
if ( averageDescriptors )
{
std::set<Point*> points;
for ( int i = 0; i < features.size(); i++ ) points.insert( features[i]->track->point );
std::vector<Feature*> newfeatures;
for ( std::set<Point*>::iterator it = points.begin(); it != points.end(); it++ )
{
Point *point = *it;
Feature *feature = new Feature;
Track *track = point->track;
feature->track = track;
unsigned int sum[128];
for ( int j = 0; j < 128; j++ ) sum[j] = 0;
int count = 0;
//for ( int i = 0; i < features.size(); i++ )
for ( ElementList::iterator featureit = track->features.begin(); featureit != track->features.end(); featureit++ )
{
// if ( features[i]->track->point != point ) continue;
Feature *myfeature = (Feature*)featureit->second;
if ( myfeature->descriptor == NULL ) continue;
for ( int j = 0; j < 128; j++ ) sum[j] += myfeature->descriptor[j];
count++;
}
feature->descriptor = new unsigned char[128];
for ( int j = 0; j < 128; j++ ) feature->descriptor[j] = sum[j] / count;
newfeatures.push_back( feature );
}
features = newfeatures;
deleteFeatures = true;
}
int count = features.size();
data = new unsigned char[count*128];
unsigned char *ptr = data;
for ( int i = 0; i < count; i++,ptr+=128 ) {
memcpy( ptr, features[i]->descriptor, 128 );
}
index->setData( count, data );
}
void CvMapGenerator::addGameElements()
{
addRivers();
gDLL->logMemState("CvMapGen after add rivers");
addLakes();
gDLL->logMemState("CvMapGen after add lakes");
addFeatures();
gDLL->logMemState("CvMapGen after add features");
addBonuses();
gDLL->logMemState("CvMapGen after add bonuses");
addGoodies();
gDLL->logMemState("CvMapGen after add goodies");
// Call for Python to make map modifications after it's been generated
afterGeneration();
}
/**
* Preclassify just detects no initialized cells and classifies them accordingly.
* It does not change the class of other cells.
*/
void HeightMapClassifier::prepareMap(HeightMap * const map, bool doPreclassify)
{
_curMap = map;
Timing t("prepareMap");
computeNormals();
addFeatures();
if(doPreclassify) {
for(int x = 0; x < curSizeX(); x++) {
for(int z = 0; z < curSizeZ(); z++) {
if(cellUninitialized(x, z)) {
//curModifieableCells()[x][z].setClass(HeightCell::ELC_NOT_INITIALIZED);
curModifieableCells()[x][z].setClass(HeightCell::ELC_FLAT_GROUND);
}
}
}
}
t.printInfo();
}
std::unique_ptr<Bucket> SymbolLayer::createBucket(StyleBucketParameters& parameters) const {
auto bucket = std::make_unique<SymbolBucket>(parameters.tileID.overscaling,
parameters.tileID.z);
bucket->layout = layout;
StyleCalculationParameters p(parameters.tileID.z);
bucket->layout.placement.calculate(p);
if (bucket->layout.placement.value == PlacementType::Line) {
bucket->layout.icon.rotationAlignment.value = RotationAlignmentType::Map;
bucket->layout.text.rotationAlignment.value = RotationAlignmentType::Map;
};
bucket->layout.spacing.calculate(p);
bucket->layout.avoidEdges.calculate(p);
bucket->layout.icon.allowOverlap.calculate(p);
bucket->layout.icon.ignorePlacement.calculate(p);
bucket->layout.icon.optional.calculate(p);
bucket->layout.icon.rotationAlignment.calculate(p);
bucket->layout.icon.image.calculate(p);
bucket->layout.icon.padding.calculate(p);
bucket->layout.icon.rotate.calculate(p);
bucket->layout.icon.keepUpright.calculate(p);
bucket->layout.icon.offset.calculate(p);
bucket->layout.text.rotationAlignment.calculate(p);
bucket->layout.text.field.calculate(p);
bucket->layout.text.font.calculate(p);
bucket->layout.text.maxWidth.calculate(p);
bucket->layout.text.lineHeight.calculate(p);
bucket->layout.text.letterSpacing.calculate(p);
bucket->layout.text.maxAngle.calculate(p);
bucket->layout.text.rotate.calculate(p);
bucket->layout.text.padding.calculate(p);
bucket->layout.text.ignorePlacement.calculate(p);
bucket->layout.text.optional.calculate(p);
bucket->layout.text.justify.calculate(p);
bucket->layout.text.anchor.calculate(p);
bucket->layout.text.keepUpright.calculate(p);
bucket->layout.text.transform.calculate(p);
bucket->layout.text.offset.calculate(p);
bucket->layout.text.allowOverlap.calculate(p);
bucket->layout.icon.size.calculate(StyleCalculationParameters(18));
bucket->layout.text.size.calculate(StyleCalculationParameters(18));
bucket->layout.iconMaxSize = bucket->layout.icon.size;
bucket->layout.textMaxSize = bucket->layout.text.size;
bucket->layout.icon.size.calculate(StyleCalculationParameters(p.z + 1));
bucket->layout.text.size.calculate(StyleCalculationParameters(p.z + 1));
bucket->parseFeatures(parameters.layer, filter);
if (bucket->needsDependencies(parameters.glyphStore, parameters.spriteStore)) {
parameters.partialParse = true;
}
// We do not add features if the parser is in a "partial" state because
// the layer ordering needs to be respected when calculating text
// collisions. Although, at this point, we requested all the resources
// needed by this tile.
if (!parameters.partialParse) {
bucket->addFeatures(parameters.tileUID,
*spriteAtlas,
parameters.glyphAtlas,
parameters.glyphStore,
parameters.collisionTile);
}
return std::move(bucket);
}
/**
* Adds a single feature this factory will make ready on further constructed contacts.
*
* No feature removal is provided, to guard against uncooperative modules removing features other
* modules have set and depend on.
*
* \param feature The feature to add.
*/
void ContactFactory::addFeature(const Feature &feature)
{
addFeatures(Features(feature));
}
/**
* Class constructor.
*
* \param features The features to make ready on constructed contacts.
*/
ContactFactory::ContactFactory(const Features &features)
: mPriv(new Private)
{
addFeatures(features);
}
bool FeatureSet::readFromFile(const char *filename_format, ...)
{
// Max line size when reading .feat files.
static const int LINESIZE = 1024;
// Construct filename from arguments and initialize file stream.
va_list ap;
va_start(ap, filename_format);
#ifdef WIN32
#define FILENAMELENGTH 256
char filename[FILENAMELENGTH];
_vsnprintf(filename, FILENAMELENGTH, filename_format, ap);
std::ifstream input(filename, std::ifstream::binary);
#else
char *filename;
if (vasprintf(&filename, filename_format, ap) == -1) {
return false;
}
std::ifstream input(filename, std::ifstream::binary);
std::free(filename);
#endif
va_end(ap);
if (!input.good()) return false;
std::vector<Feature> features;
// The feature file begins with the word "feat".
// This is followed by the total number of features,
// followed by a list of features.
// Each individual feature begins with a single integer indicating the
// number of vertices in that feature.
// This is followed by a list of vertices in the feature, one per line.
// Empty lines and lines beginning with '#' are ignored.
char line[LINESIZE];
// First we expect the word "feat"
if (input.get() != 'f') return false;
if (input.get() != 'e') return false;
if (input.get() != 'a') return false;
if (input.get() != 't') return false;
// The first number is the total number of polylines.
int numFeatures = 0;
while (input.good()) {
input.getline(line, LINESIZE);
if (line[0] != 0 && line[0] != '\r' && line[0] != '#') {
std::sscanf(line, "%d", &numFeatures);
break;
}
}
Feature feat;
Vec3f v;
int numVertices = 0;
for (int f = 0; f < numFeatures && input.good(); ++f) {
// First find the number of vertices.
while (input.good()) {
input.getline(line, LINESIZE);
if (line[0] != 0 && line[0] != '\r' && line[0] != '#') {
std::sscanf(line, "%d", &numVertices);
break;
}
}
// Then read in the vertices.
int i = 0;
while (i < numVertices && input.good()) {
input.getline(line, LINESIZE);
if (line[0] != 0 && line[0] != '\r' && line[0] != '#') {
std::sscanf(line, "%f %f %f", &v[0], &v[1], &v[2]);
feat.vertices.push_back(v);
++i;
}
}
// Finally, save the feature.
if (feat.vertices.size() > 0) {
features.push_back(feat);
feat.vertices.clear();
}
}
// Now add the features to our set.
addFeatures(features);
return true;
}
FeatureSet::FeatureSet(const std::vector<Feature>& f)
{
addFeatures(f);
}
/**
* Construct a new AccountFactory object.
*
* As in create(), it should be noted that unlike Account::becomeReady(), FeatureCore isn't assumed.
* If no \a features are specified, no Account::becomeReady() call is made at all and the proxy
* won't be Account::isReady().
*
* \param bus The QDBusConnection for proxies constructed using this factory to use.
* \param features The features to make ready on constructed Accounts.
*/
AccountFactory::AccountFactory(const QDBusConnection &bus, const Features &features)
: FixedFeatureFactory(bus)
{
addFeatures(features);
}
bool NNLocalizer::localize( Camera *querycamera )
{
features.clear();
addFeatures( querycamera->node, false, features );
if ( features.empty() ) return false;
std::vector<Match*> matches;
std::cout << "running query with " << features.size() << " features\n";
//findMatches( (*fm), features, matches );
findUniqueMatches( (*fm), features, 0.8, matches );
std::cout << "done matching\n";
std::vector<bool> inliers;
PointPairList point_pairs;
for ( int i = 0; i < matches.size(); i++ )
{
Match *match = matches[i];
Feature *feature = match->feature1;
Feature *queryfeature = match->feature2;
PointPair point_pair;
point_pair.first = project( feature->track->point->position );
point_pair.second = queryfeature->unproject();
point_pairs.push_back( point_pair );
}
std::cout << point_pairs.size() << " matches for pose estimation\n";
Sophus::SE3d best_pose;
int ninliers;
ThreePointPose estimator;
PROSAC prosac;
prosac.num_trials = 5000;
prosac.min_num_inliers = 100;
prosac.inlier_threshold = thresh;
ninliers = prosac.compute( point_pairs.begin(), point_pairs.end(), estimator, inliers );
best_pose = estimator.pose;
// std::vector<Estimator*> estimators( 5000 );
// for ( size_t i = 0; i < estimators.size(); i++ ) estimators[i] = new ThreePointPose;
// PreemptiveRANSAC preemptive_ransac( 10 );
// preemptive_ransac.inlier_threshold = thresh;
// ThreePointPose *best_estimator = NULL;
// ninliers = preemptive_ransac.compute( point_pairs.begin(), point_pairs.end(), estimators, (Estimator**)&best_estimator, inliers );
// best_pose = best_estimator->pose;
// for ( size_t i = 0; i < estimators.size(); i++ ) delete estimators[i];
// estimators.clear();
ElementList::iterator it;
std::cout << matches.size() << " matches; " << ninliers << " inliers\n";
// drawMatches( querycamera->node, matches, std::vector<bool>() );//, inliers );
// exit(1);
for ( int i = 0; i < inliers.size(); i++ )
{
if ( !inliers[i] ) continue;
Match *match = matches[i];
Feature *feature = match->feature1;
Feature *queryfeature = match->feature2;
if ( queryfeature->track != NULL ) continue;
Point *point = feature->track->point;
queryfeature->track = new Track;
queryfeature->track->point = point;
}
for ( int i = 0; i < matches.size(); i++ ) {
if(matches[i])
delete matches[i];
}
RobustLeastSq robustlsq( root );
querycamera->node->pose = best_pose;
bool good = false;
for ( int i = 0; i < 10; i++ )
{
Sophus::SE3d last_pose = querycamera->node->pose;
tracker->verbose = false;
good = tracker->track( querycamera );
std::cout << "tracker tracked " << tracker->ntracked << " / " << tracker->nattempted << "\n";
if ( !good ) break;
if ( good ) {
robustlsq.run( querycamera );
// updatePose( root, querycamera );
Sophus::SE3d update = querycamera->node->pose * last_pose.inverse();
if ( update.log().norm() < 1e-6 ) break;
}
}
return good;
}
