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; }
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; };
// 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()); }
// 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; } } }