通过交互式感知从一组有限的假设中引导机器人生态感知

#include "cmm/gmm.hpp" #include "cmm/gmm_estimator.hpp" #include <map> #include <chrono> #include <cmath> using namespace cmm; std::vector<double> CollabMM::compute_estimation(const Eigen::VectorXd& X) const{ if([&]() -> bool { for(int i = 0; i < _nbr_class; i++) {if(!_model.at(i).empty()) return false;} return true;}()) return std::vector<double>(_nbr_class,1./(double)_nbr_class); // Estimator<CollabMM> estimator(this, X); return estimation<CollabMM>(this,X); } //SCORE_CALCULATOR #ifndef NO_PARALLEL void CollabMM::_score_calculator::operator()(const tbb::blocked_range<size_t>& r){ double sum = _sum; for(size_t i = r.begin(); i != r.end(); ++i){ if(_samples[i].first == _label || _all_samples) sum += std::log(_samples.estimations[i][_label]); } _sum = sum; } #endif double CollabMM::_score_calculator::compute(){ #ifdef NO_PARALLEL _sum = 0; for(int i = 0; i < _samples.size(); i++){ if(_samples[i].first == _label || _all_samples) _sum += std::log(_samples.estimations[i][_label]); } #else tbb::parallel_reduce(tbb::blocked_range<size_t>(0,_samples.size()),*this); #endif return _sum/((double)_samples.get_data(_label).size()); } //--SCORE_CALCULATOR void CollabMM::update_factors(){ for(int i = 0; i < _nbr_class; i++) _update_factors(i); } void CollabMM::_update_factors(int lbl){ double sum_size = 0; for(auto& components : _model[lbl]) sum_size += components->size(); for(auto& c : _model[lbl]){ c->set_factor((double)c->size()/((double)sum_size)); } } void CollabMM::new_component(const Eigen::VectorXd& sample, int label){ Component::Ptr component(new Component(_dimension,label)); component->add(sample); component->update_parameters(); _model[label].push_back(component); update_factors(); } void CollabMM::knn(const Eigen::VectorXd& center, Data& output, int k){ double min_dist, dist; int min_index; Data cpy_samples(_samples); for(int j = 0; j < k; j++){ min_dist = sqrt((cpy_samples[0].second - center).transpose()*(cpy_samples[0].second - center)); min_index = 0; for(int i = 1; i < cpy_samples.size(); i++){ dist = sqrt((cpy_samples[i].second - center).transpose()*(cpy_samples[i].second - center)); if(dist < min_dist){ min_index = i; min_dist = dist; } } output.add(cpy_samples[min_index]); cpy_samples.erase(min_index); } } double CollabMM::confidence(const Eigen::VectorXd& X) const{ int size = 0; for(int i = 0; i < _model.size() ; i++){ if(_model.at(i).size() < 5) continue; size += _model.at(i).size(); } if(size == 0) return 0; //* Look for the closest consistent (size >= 5) component of X Eigen::VectorXd distances(size); int k = 0; // double denominator = 0; for(int i = 0 ; i < _model.size(); i++){ for (int j = 0; j < _model.at(i).size(); j++) { if(_model.at(i).size() < 5) continue; distances(k) = _model.at(i).at(j)->distance(X); k++; } } int r=0,c=0; distances.minCoeff(&r,&c); //*/ //* Compute the real indexes of the components in the model int lbl = 0, s = _model.at(lbl).size(); while(r >= s){ if(s >= 5) r = r - s; lbl++; s = _model.at(lbl).size(); } //*/ return _model.at(lbl).at(r)->compute_multivariate_normal_dist(X)/ _model.at(lbl).at(r)->compute_multivariate_normal_dist(_model.at(lbl).at(r)->get_mu()); } double CollabMM::loglikelihood(){ _score_calculator sc(this,_samples); return sc.compute(); } double CollabMM::loglikelihood(int label){ _score_calculator sc(this,_samples,false,label); return sc.compute(); } bool CollabMM::_merge(const Component::Ptr& comp){ // if comp have too few samples or there is only one component in lbl model abort if(comp->size() < 5 || _model[comp->get_label()].size() == 1) return false; int lbl = comp->get_label(); int ind; for(ind = 0; ind < _model[lbl].size(); ind++) if(_model[lbl][ind].get() == comp.get()) break; if(ind == _model[lbl].size()) return false; //* Capture time. #ifdef VERBOSE std::cout << "merge function" << std::endl; std::chrono::system_clock::time_point timer; timer = std::chrono::system_clock::now(); #endif //*/ CollabMM candidate; double score, candidate_score; if(_llhood_drive) score = loglikelihood(); //* Find the closest component within the same class Eigen::VectorXd distances(_model[lbl].size()); int r, c; for (int j = 0; j < _model[lbl].size(); j++) { if(_model[lbl][j] == comp){ distances(j) = 1000000; continue; } distances(j) = comp->distance(_model[lbl][j]->get_mu()); } distances.minCoeff(&r,&c); //*/ // if(_model[lbl][r]->get_samples().size() < 5) // return false; if(comp->intersect(_model[lbl][r])){// check instersection if(_llhood_drive){ candidate = CollabMM(_model); candidate.set_samples(_samples); candidate.model()[lbl][ind]->merge(candidate.model()[lbl][r]); candidate.model()[lbl].erase(candidate.model()[lbl].begin() + r); candidate.update_factors(); candidate._estimate_training_dataset(); candidate_score = candidate.loglikelihood(); #ifdef VERBOSE std::cout << candidate_score << " >? " << score << std::endl; #endif } if(!_llhood_drive || candidate_score > score){ #ifdef VERBOSE std::cout << "-_- MERGE _-_" << std::endl; #endif _model[lbl][ind]->merge(_model[lbl][r]); _model[lbl].erase(_model[lbl].begin() + r); update_factors(); //* Display time spent for the algorithm. //* Display time spent for the algorithm. #ifdef VERBOSE std::cout << "Merge finish, time spent : " << std::chrono::duration_cast<std::chrono::milliseconds>( std::chrono::system_clock::now() - timer).count() << std::endl; #endif //*/ return true; } } //* Display time spent for the algorithm. #ifdef VERBOSE std::cout << "Merge finish, time spent : " << std::chrono::duration_cast<std::chrono::milliseconds>( std::chrono::system_clock::now() - timer).count() << std::endl; #endif //*/ return false; } std::pair<double,double> CollabMM::_coeff_intersection(int ind1, int lbl1, int ind2, int lbl2){ std::pair<double,double> coeffs; Eigen::VectorXd eigenval, eigenval2, diff_mu; Eigen::MatrixXd eigenvect, eigenvect2; _model[lbl1][ind1]->compute_eigenvalues(eigenval,eigenvect); _model[lbl2][ind2]->compute_eigenvalues(eigenval2,eigenvect2); diff_mu = _model[lbl1][ind1]->get_mu() - _model[lbl1][ind1]->get_mu(); coeffs.first = diff_mu.dot(eigenvect.col(0)) - diff_mu.squaredNorm()*diff_mu.squaredNorm(); coeffs.second = diff_mu.dot(eigenvect2.col(0)) - diff_mu.squaredNorm()*diff_mu.squaredNorm(); return coeffs; } double CollabMM::_component_score(int i, int lbl){ Data knn_output; knn(_model[lbl][i]->get_mu(), knn_output,_model[lbl][i]->size()); _score_calculator sc(this,knn_output,lbl); return sc.compute(); } bool CollabMM::_split(const Component::Ptr& comp){ //*If the component have less than 4 element abort if(comp->size() < 5) return false; //*/ if(_max_nb_components != 0 && _model[comp->get_label()].size() >= _max_nb_components){ #ifdef VERBOSE std::cout << "maximum number of components reach : " << _max_nb_components << std::endl; std::co