void Clustering<N, ObjectT, DistanceP>::apply( objectList_T const & objects ) { // iteration loop truncated at maxIt for( int it = 0; it < M_maxIt; it++ ) { if( M_verbose ) std::cout << "=======================" << std::endl << "iteration " << it << std::endl; // clean up object.list for( size_t d = 0; d < N; d++ ) M_objectList[d].clear(); // loop on the object set for( size_t kObj = 0; kObj < objects.size(); kObj++ ) { // compute the distance of the k-th object from all the centroids std::array<real,N> distances; for( size_t d = 0; d < N; d++ ) { distances[d] = M_distancePolicy( objects[kObj], M_centroids[d] ); } // find the minimum distance int minPos = std::min_element( distances.begin(), distances.end() ) - distances.begin(); // insert the object in the list of the closest centroid M_objectList[minPos].push_back( objects[kObj] ); } if( M_verbose ) { for( size_t d = 0; d < N; d++ ) { std::cout << "centroid " << d << " has " << M_objectList[d].size() << " objects" << std::endl; for( size_t j = 0; j < M_objectList[d].size(); j++ ) std::cout << M_objectList[d][j] << std::endl; } } bool const isIncremented = updateCentroids(); printGnuplot( it ); if ( !isIncremented ) { if ( M_verbose ) std::cout << "iteration interrupted at " << it << std::endl; break; } } computeQuality(); }
/*! Every grasp in graspList will be tested and stored in testedGraspList even if their test scores, epsilon quality and volume quality, are not positive, which means they are not stable grasps. */ bool GraspitDBPlanner::testGrasps(TestType t, std::vector<db_planner::Grasp *> graspList, std::vector<db_planner::Grasp *> *testedGraspList) { mTestedGrasps.clear(); if (testedGraspList) { testedGraspList->clear(); } // test each of the grasps in graspList for (int i = 0; i < ((int)graspList.size()); i ++) { GraspitDBGrasp *tempGrasp = new GraspitDBGrasp(*static_cast<GraspitDBGrasp *>(graspList[i])); // load this grasp into the world static_cast<GraspitDBModel *>(mObject)->getGraspableBody()->setTran(transf::IDENTITY); GraspPlanningState *tempState = new GraspPlanningState(tempGrasp->getPreGraspPlanningState()); tempState->setRefTran(transf::IDENTITY); tempGrasp->setPreGraspPlanningState(tempState); float elmts[16]; if (mAligner->Align(tempGrasp->SourceModel(), *mObject, elmts)) { tempGrasp->Transform(elmts); } mHand->getGrasp()->setObject(static_cast<GraspitDBModel *>(mObject)->getGraspableBody()); // apply it to the hand tempGrasp->getPreGraspPlanningState()->execute(); bool testResult; DynamicCode dynCode; // test this grasp if (t == STATIC) { testResult = testGraspStatic(); dynCode = NO_DYNAMICS; } else if (t == DYNAMIC) { testResult = testGraspDynamic(&dynCode); //DBGA("Result: " << testResult << "; dynamics code: " << dynCode); } float eq = -1.0, vq; // decide whether to record if (testResult) { computeQuality(eq, vq); if (!static_cast<GraspitDBModel *>(mObject)->getGraspableBody()->getNumContacts()) { dynCode = DYNAMIC_OBJECT_EJECTED; } else if (eq <= 0.0) { dynCode = DYNAMIC_NO_FC; } } else { return false; } if (eq < 0) { continue; } //we now save all grasps in mTestedGrasps GraspPlanningState *newGrasp = new GraspPlanningState(mHand); newGrasp->setPositionType(SPACE_COMPLETE, false); newGrasp->setPostureType(POSE_DOF, false); //it is possible the object moved after dynamics, so we need to re-set the reference posture newGrasp->setRefTran(static_cast<GraspitDBModel *>(mObject)->getGraspableBody()->getTran()); newGrasp->saveCurrentHandState(); newGrasp->setEpsilonQuality(eq); newGrasp->setVolume(vq); newGrasp->setIndex(i); //this is a hack; just gives us a field to save the dynamics code newGrasp->setDistance((double)dynCode); mTestedGrasps.push_back(newGrasp); //std::cout << "grasp tested, quality: " << eq << std::endl; if (!testedGraspList) { continue; } // record this to synthesize the output db_planner::Grasp *recordGrasp = static_cast<db_planner::Grasp *>(new GraspitDBGrasp(*tempGrasp)); tempState = new GraspPlanningState(static_cast<GraspitDBGrasp *>(recordGrasp)->getPreGraspPlanningState()); tempState->copyFrom(newGrasp); static_cast<GraspitDBGrasp *>(recordGrasp)->setPreGraspPlanningState(tempState); testedGraspList->push_back(recordGrasp); delete tempGrasp; } if (mTestedGrasps.size() != 0) { return true; } return false; }
lpaggreg::HPartition::HPartition(vector<bool> aggregated, shared_ptr<vector<shared_ptr<lpaggreg::Quality> > > qualities, float parameter, lpaggreg::HValuesMetaData metaData): Partition(parameter), aggregated(aggregated), qualities(qualities), metaData(metaData) { computeParts(); computeQuality(); }