// get list of features in reconstruction in each image
void hulo::listReconFeat(const SfM_Data &sfm_data,
map<int, vector<int>> &imgFeatList) {
const Landmarks landmarks = sfm_data.structure;
for (Landmarks::const_iterator iterL = landmarks.begin();
iterL != landmarks.end(); iterL++) {
// iterL->second.obs is Observations which is Hash_Map<IndexT, Observation>
for (Observations::const_iterator iterO = iterL->second.obs.begin();
iterO != iterL->second.obs.end(); iterO++) {
imgFeatList[iterO->first].push_back(iterO->second.id_feat);
}
}
}
EmbeddingResult triangulate(RandomAccessIterator begin, RandomAccessIterator end, PairwiseCallback distance_callback,
const Landmarks& landmarks, const DenseVector& landmark_distances_squared,
EmbeddingResult& landmarks_embedding, unsigned int target_dimension)
{
timed_context context("Landmark triangulation");
bool* to_process = new bool[end-begin];
fill(to_process,to_process+(end-begin),true);
DenseMatrix embedding((end-begin),target_dimension);
for (Landmarks::const_iterator iter=landmarks.begin();
iter!=landmarks.end(); ++iter)
{
to_process[*iter] = false;
embedding.row(*iter).noalias() = landmarks_embedding.first.row(iter-landmarks.begin());
}
for (unsigned int i=0; i<target_dimension; ++i)
landmarks_embedding.first.col(i).array() /= landmarks_embedding.second(i);
// landmarks_embedding.first.transposeInPlace();
RandomAccessIterator iter;
DenseVector distances_to_landmarks;
//#pragma omp parallel private(distances_to_landmarks)
{
distances_to_landmarks = DenseVector(landmarks.size());
//#pragma omp for private(iter) schedule(static)
for (iter=begin; iter<end; ++iter)
{
if (!to_process[iter-begin])
continue;
for (unsigned int i=0; i<distances_to_landmarks.size(); ++i)
{
DefaultScalarType d = distance_callback(*iter,begin[landmarks[i]]);
distances_to_landmarks(i) = d*d;
}
//distances_to_landmarks.array().square();
distances_to_landmarks -= landmark_distances_squared;
embedding.row(iter-begin).noalias() = -0.5*landmarks_embedding.first.transpose()*distances_to_landmarks;
}
}
delete[] to_process;
return EmbeddingResult(embedding,DenseVector());
}
Landmarks select_landmarks_random(RandomAccessIterator begin, RandomAccessIterator end, ScalarType ratio)
{
Landmarks landmarks;
landmarks.reserve(end-begin);
for (RandomAccessIterator iter=begin; iter!=end; ++iter)
landmarks.push_back(iter-begin);
tapkee::random_shuffle(landmarks.begin(),landmarks.end());
landmarks.erase(landmarks.begin() + static_cast<IndexType>(landmarks.size()*ratio),landmarks.end());
return landmarks;
}
// convert sfm_data.getLandmarks() to MapViewFeatTo3D
void hulo::structureToMapViewFeatTo3D(const Landmarks &landmarks, hulo::MapViewFeatTo3D &map){
// iterate over landmark in landmarks
for(Landmarks::const_iterator iter = landmarks.begin(); iter != landmarks.end(); iter++){
const Landmark &lnmk = iter->second;
// iterate over observation in each landmark
for(Observations::const_iterator iterObs = lnmk.obs.begin(); iterObs!=lnmk.obs.end(); iterObs++){
// insert 3D points into map
map[iterObs->first][iterObs->second.id_feat] = iter->first;
}
}
}
DenseSymmetricMatrix compute_distance_matrix(RandomAccessIterator begin, RandomAccessIterator /*end*/,
Landmarks& landmarks, PairwiseCallback callback)
{
timed_context context("Multidimensional scaling distance matrix computation");
const IndexType n_landmarks = landmarks.size();
DenseSymmetricMatrix distance_matrix(n_landmarks,n_landmarks);
#pragma omp parallel shared(begin,landmarks,distance_matrix,callback) default(none)
{
IndexType i_index_iter,j_index_iter;
#pragma omp for nowait
for (i_index_iter=0; i_index_iter<n_landmarks; ++i_index_iter)
{
for (j_index_iter=i_index_iter; j_index_iter<n_landmarks; ++j_index_iter)
{
ScalarType d = callback.distance(begin[landmarks[i_index_iter]],begin[landmarks[j_index_iter]]);
d *= d;
distance_matrix(i_index_iter,j_index_iter) = d;
distance_matrix(j_index_iter,i_index_iter) = d;
}
}
}
return distance_matrix;
}
DenseMatrix triangulate(RandomAccessIterator begin, RandomAccessIterator end, PairwiseCallback distance_callback,
Landmarks& landmarks, DenseVector& landmark_distances_squared,
EigendecompositionResult& landmarks_embedding, IndexType target_dimension)
{
timed_context context("Landmark triangulation");
const IndexType n_vectors = end-begin;
const IndexType n_landmarks = landmarks.size();
bool* to_process = new bool[n_vectors];
std::fill(to_process,to_process+n_vectors,true);
DenseMatrix embedding(n_vectors,target_dimension);
for (IndexType index_iter=0; index_iter<n_landmarks; ++index_iter)
{
to_process[landmarks[index_iter]] = false;
embedding.row(landmarks[index_iter]).noalias() = landmarks_embedding.first.row(index_iter);
}
for (IndexType i=0; i<target_dimension; ++i)
landmarks_embedding.first.col(i).array() /= landmarks_embedding.second(i);
#pragma omp parallel shared(begin,end,to_process,distance_callback,landmarks, \
landmarks_embedding,landmark_distances_squared,embedding) default(none)
{
DenseVector distances_to_landmarks(n_landmarks);
IndexType index_iter;
#pragma omp for nowait
for (index_iter=0; index_iter<n_vectors; ++index_iter)
{
if (!to_process[index_iter])
continue;
for (IndexType i=0; i<n_landmarks; ++i)
{
ScalarType d = distance_callback.distance(begin[index_iter],begin[landmarks[i]]);
distances_to_landmarks(i) = d*d;
}
//distances_to_landmarks.array().square();
distances_to_landmarks -= landmark_distances_squared;
embedding.row(index_iter).noalias() = -0.5*landmarks_embedding.first.transpose()*distances_to_landmarks;
}
}
delete[] to_process;
return embedding;
}
DenseSymmetricMatrix compute_distance_matrix(RandomAccessIterator begin, Landmarks landmarks,
PairwiseCallback callback)
{
timed_context context("Multidimensional scaling distance matrix computation");
DenseMatrix distance_matrix(landmarks.size(),landmarks.size());
for (Landmarks::const_iterator i_iter=landmarks.begin(); i_iter!=landmarks.end(); ++i_iter)
{
for (Landmarks::const_iterator j_iter=i_iter; j_iter!=landmarks.end(); ++j_iter)
{
DefaultScalarType d = callback(*(begin+*i_iter),*(begin+*j_iter));
d *= d;
distance_matrix(i_iter-landmarks.begin(),j_iter-landmarks.begin()) = d;
distance_matrix(j_iter-landmarks.begin(),i_iter-landmarks.begin()) = d;
}
}
return distance_matrix;
};
void LandmarkDetector::nosetip(Landmarks &l)
{
//qDebug() << " nose tip";
int x = -1;
int y = -1;
peakDensity.maxIndex(x, y);
if (x == -1 && y == -1)
{
std::cerr << "Nose coordinates not found" << std::endl;
return;
}
bool ok;
cv::Point3d nosetip = converter.MapToMeshCoords(depth, cv::Point2d(x + cropStartX, y + cropStartY), &ok);
if (ok)
{
l.set(Landmarks::Nosetip, nosetip);
}
else
{
std::cerr << "Nose coordinates not found" << std::endl;
}
}
DenseMatrix compute_shortest_distances_matrix(RandomAccessIterator begin, RandomAccessIterator end,
const Landmarks& landmarks, const Neighbors& neighbors, DistanceCallback callback)
{
timed_context context("Distances shortest path relaxing");
const IndexType n_neighbors = neighbors[0].size();
const IndexType N = end-begin;
FibonacciHeap* heap = new FibonacciHeap(N);
bool* s = new bool[N];
bool* f = new bool[N];
DenseMatrix shortest_distances(landmarks.size(),N);
for (IndexType k=0; k<landmarks.size(); k++)
{
// fill s and f with false, fill shortest_D with infinity
for (IndexType j=0; j<N; j++)
{
shortest_distances(k,j) = numeric_limits<DenseMatrix::Scalar>::max();
s[j] = false;
f[j] = false;
}
// set distance from k to k as zero
shortest_distances(k,landmarks[k]) = 0.0;
// insert kth object to heap with zero distance and set f[k] true
heap->insert(landmarks[k],0.0);
f[k] = true;
// while heap is not empty
while (heap->get_num_nodes()>0)
{
// extract min and set (s)olution state as true and (f)rontier as false
ScalarType tmp;
int min_item = heap->extract_min(tmp);
s[min_item] = true;
f[min_item] = false;
// for-each edge (min_item->w)
for (IndexType i=0; i<n_neighbors; i++)
{
// get w idx
int w = neighbors[min_item][i];
// if w is not in solution yet
if (s[w] == false)
{
// get distance from k to i through min_item
ScalarType dist = shortest_distances(k,min_item) + callback(begin[min_item],begin[w]);
// if distance can be relaxed
if (dist < shortest_distances(k,w))
{
// relax distance
shortest_distances(k,w) = dist;
// if w is in (f)rontier
if (f[w])
{
// decrease distance in heap
heap->decrease_key(w, dist);
}
else
{
// insert w to heap and set (f)rontier as true
heap->insert(w, dist);
f[w] = true;
}
}
}
}
}
// clear heap to re-use
heap->clear();
}
delete heap;
delete[] s;
delete[] f;
return shortest_distances;
}
int main () {
textmode (ATC_TEXT_MODE);
clrscr();
Position::MaxX = 30;
Position::MaxY = 30;
char pid;
Position::MaxX = 30;
Position::MaxY = 30;
RadarScreen *r;
Landmarks l;
Traffic t;
Plane *p1;
Gate g (Position (0, 10), 1, D90);
Gate g1 (Position (0, 0), 2, D45);
Gate g2 (Position (10, 0), 3, D0);
Gate g3 (Position (MAX_X, MAX_Y), 4, D225);
// Gate g4 (Position (10, 0), 4, D0);
Airport a1 (Position (5, 5), 1, Heading (D90));
Airport a2 (Position (10, 12), 2, Heading (D45));
Beacon b1 (Position (7,13), 1);
Beacon b2 (Position (1,1), 2);
FlightPath fp (Position (MIN_FIELD_X, MIN_FIELD_Y),
Position (MAX_FIELD_X, MAX_FIELD_Y));
FlightPath fp1 (Position (MIN_FIELD_X, MAX_FIELD_Y),
Position (MAX_FIELD_X, MIN_FIELD_Y));
FlightPath fp2 (Position (10, 1), Position (10, MAX_FIELD_Y));
int i;
l.AddAirport (&a1);
l.AddAirport (&a2);
l.AddBeacon (&b1);
l.AddBeacon (&b2);
l.AddGate (&g);
l.AddGate (&g1);
l.AddGate (&g2);
l.AddGate (&g3);
// l.AddGate (&g4);
l.AddFlightPath (&fp);
l.AddFlightPath (&fp1);
l.AddFlightPath (&fp2);
r = new RadarScreen (&l);
Boolean Crashed = False;
r->Refresh();
pid = t.NewId();
p1 = new Plane (pid, g.NewPlaneCoords(), Prop, &g1);
t.NewAirborne (p1);
p1 = new Plane (t.NewId(), g1.NewPlaneCoords(), Jet, &g);
t.NewAirborne (p1);
p1 = new Plane (t.NewId(), g2.NewPlaneCoords(), Jet, &g);
t.NewAirborne (p1);
r->DisplayPlanes (&t);
for (i = 0; i < 34; i++) {
delay (500);
if (i == 17) {
t.SearchAirborne ('a');
t.FoundAirborne() -> AlterTargHeading (D270, AntiClockwise);
t.SearchAirborne ('b');
t.FoundAirborne() -> MakeCircle (AntiClockwise);
}
r->UnDisplayPlanes (&t);
if (i % 2 == 0) {
t.StepJets();
} else {
t.StepAll();
}
r->DisplayPlanes (&t);
if (!Crashed && t.Crashed ()) {
cout << '\a';
Crashed = True;
}
}
textmode (C80);
_setcursortype (_NORMALCURSOR);
clrscr();
return 0;
}
int main(int argc, char* argv[])
{
int numOfParticles , numOfSamples;
double landmarkThreshold;
const char *filepath = NULL;
const char *database = NULL;
config_t cfg, *cf;
cf = &cfg;
config_init(cf);
if (!config_read_file(cf, "config.cfg"))
return(EXIT_FAILURE);
config_lookup_int(cf, "particles" , &numOfParticles);
config_lookup_int(cf, "samples" , &numOfSamples);
config_lookup_float(cf, "landmarkThreshold" , &landmarkThreshold);
config_lookup_string(cf, "filepath" , &filepath);
config_lookup_string(cf, "database" , &database);
DBWrapper dbwr(database);
SMC smc;
Utilities ut;
dataBuffer dataPoints = ut.readFile("-----", filepath);
int timeStates = dataPoints.size();
Params baseparams;
vector < StateProgression > particles(numOfParticles, timeStates);
smc.infer( &particles, & dataPoints, baseparams, numOfParticles , numOfSamples);
Landmarks observations;
for(unsigned int i = 0; i< particles[0].stateProg[0].size(); i++){
Landmark land(smc.numOfLandmarks , particles[0].stateProg[0][i],i);
observations.addLandMark(land);
}
Landmarks landmarks = dbwr.getCurrentLandmarks();
vector<double> current_observations;
std::map<int, int > landmark_associations;
for(unsigned int i=0; i<observations.size(); i++)
{
if(landmarks.size()==0)
{
dbwr.insertLandmark(& observations.landmarks[i].distribution);
landmarks = dbwr.getCurrentLandmarks();
current_observations.push_back(0);
continue;
}
vector< vector< double > > distanceFeatures = landmarks.extractDistances(& observations.landmarks[i], & ut);
bool found = false;
for(unsigned int ijk = 0 ; ijk<distanceFeatures.size(); ijk++){
if( distanceFeatures[ijk][0]<.3 && distanceFeatures[ijk][1]<20 && found ==false){
landmark_associations.insert(std::make_pair(i, landmarks.landmarks[ijk].getId()));
current_observations.push_back(ijk);
found =true;
break;
}
}
if(found==false){
dbwr.insertLandmark(& observations.landmarks[i].distribution);
current_observations.push_back(observations.size());
landmarks = dbwr.getCurrentLandmarks();
landmark_associations.insert(std::make_pair(current_observations.back(), landmarks.landmarks.back().getId()));
}
}
ofstream myfile;
myfile.open ("/home/panos/Desktop/aggregated.txt",std::ofstream::out | std::ofstream::app);
myfile << "[";
for(unsigned int i =0 ; i< dataPoints[0].size() ; i++)
myfile << dataPoints[0][i][0] << " " << dataPoints[0][i][1] << " " << dataPoints[0][i][2] << " " << landmark_associations[particles[0].assignments[i]]<< endl;
myfile<< "]";
myfile.close();
return 0;
}
void LandmarkDetector::innerEyeCorners(Landmarks &l)
{
cv::Point3d nosetip = l.get(Landmarks::Nosetip);
cv::Point2d nose2d = converter.MeshToMapCoords(depth, cv::Point2d(nosetip.x, nosetip.y));
nose2d.x -= cropStartX;
nose2d.y -= cropStartY;
// Y coordinate
int h = depth.h;
Face::LinAlg::Vector pitsAlongY(h);
for (int y = 0; y < h; y++)
{
int count = 0;
for (int x = nose2d.x - stripeWidth/2; x <= nose2d.x + stripeWidth/2; x++)
{
count = curvature.pits.isSet(x, y) ? count+1 : count;
}
pitsAlongY(y) = count;
}
Face::LinAlg::Vector smoothedPits = pitsAlongY.smooth(pitsStripeSmoothKernel);
int eyes2dY = smoothedPits.maxIndex(nose2d.y - maxYDistanceFromNosetipToEyes,
nose2d.y - minYDistanceFromNosetipToEyes);
if (eyes2dY < 0)
{
std::cerr << "Y coordinate of inner eye corners not found" << std::endl;
return;
}
// X coordinate
int w = depth.w;
Face::LinAlg::Vector pitsAlongX(w);
for (int x = 0; x < w; x++)
{
int count = 0;
for (int y = eyes2dY - stripeWidth/2; y <= eyes2dY + stripeWidth/2; y++)
{
count = curvature.pits.isSet(x,y) ? count+1 : count;
}
pitsAlongX(x) = count;
}
smoothedPits = pitsAlongX.smooth(pitsStripeSmoothKernel);
int leftEye2dX = smoothedPits.maxIndex(nose2d.x - maxXDistanceFromNosetipToEyes,
nose2d.x - minXDistanceFromNosetipToEyes);
if (leftEye2dX < 0)
{
std::cerr << "X coordinate of left inner eye corner not found" << std::endl;
}
else
{
l.set(Landmarks::LeftInnerEye,
converter.MapToMeshCoords(depth, cv::Point2d(leftEye2dX + cropStartX, eyes2dY + cropStartY)));
}
int rightEye2dX = smoothedPits.maxIndex(nose2d.x + minXDistanceFromNosetipToEyes,
nose2d.x + maxXDistanceFromNosetipToEyes);
if (rightEye2dX < 0)
{
std::cerr << "X coordinate of right inner eye corner not found" << std::endl;
}
else
{
l.set(Landmarks::RightInnerEye,
converter.MapToMeshCoords(depth, cv::Point2d(rightEye2dX + cropStartX, eyes2dY + cropStartY)));
}
}
