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