Landmarks select_landmarks_random(RandomAccessIterator begin, RandomAccessIterator end, DefaultScalarType ratio)
{
Landmarks landmarks;
landmarks.reserve(end-begin);
for (RandomAccessIterator iter=begin; iter!=end; ++iter)
landmarks.push_back(iter-begin);
random_shuffle(landmarks.begin(),landmarks.end());
landmarks.erase(landmarks.begin() + static_cast<unsigned int>(landmarks.size()*ratio),landmarks.end());
return landmarks;
}
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());
}
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);
}
}
}
// 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;
}
}
}
