void RandomizeSpriteSystem::OnActorEvent(::ActorEvent const& Evt) { if (Evt.mState != ActorEvent::Added) { return; } auto renderableC( Evt.mActor->Get<IRenderableComponent>() ); if (!renderableC.IsValid()) { return; } int32_t ind = 0; if (!renderableC->GetRandomSprites().empty()) { ind = GetRandomIndex( renderableC->GetRandomSprites() ); } renderableC->SetSpriteIndex( ind ); auto borderC( Evt.mActor->Get<IBorderComponent>() ); if (!borderC.IsValid()) { return; } ind = 0; if (!borderC->GetRandomSprites().empty()) { ind = GetRandomIndex( borderC->GetRandomSprites() ); } borderC->SetSpriteIndex( ind ); }
Genes CreatePropelledAxleGenes(int numberOfPropelledAxlesMax) { Genes propelledAxleGenes; for(int i=0;i<numberOfPropelledAxlesMax;i++) propelledAxleGenes.push_back(0); int numberOfAxlesPropelledRandomlyGenerated = GetRandomIndex(numberOfPropelledAxlesMax); for(int i=0;i<numberOfAxlesPropelledRandomlyGenerated;i++) { int propelledAxleIndexRandomlyGenerated = GetRandomIndex(numberOfPropelledAxlesMax); propelledAxleGenes[propelledAxleIndexRandomlyGenerated] = 1; } return propelledAxleGenes; }
int TournamentSelection(Fitnesses& fitnessForSelection, float tournamentSelectionParameter, int tournamentSize) { std::vector<int> randomIndividualIndex; std::vector<double> randomIndividualFitness; int populationSize = fitnessForSelection.size(); // Select two individuals to participate in the tournament for(int i=0;i<tournamentSize;i++) { // Contains the indices of two individuals participating in the tournament int temporaryIndex = GetRandomIndex(populationSize); randomIndividualIndex.push_back(temporaryIndex); // Index needed to pass to main() double temporaryFitness = fitnessForSelection[temporaryIndex]; randomIndividualFitness.push_back(temporaryFitness); } // Generate random floating point number (double) for tournament selection double randomDouble = GetRandomDouble(); int fittestIndividualIndex, weakestIndividualIndex; // Choose fitter individual if random double is lesser than tournament selection parameter if(randomDouble < tournamentSelectionParameter) { // Return fittest individual fittestIndividualIndex = GetFittestIndividual(randomIndividualIndex, randomIndividualFitness); return fittestIndividualIndex; } else { // Return weakest individual weakestIndividualIndex = GetWeakestIndividual(randomIndividualIndex, randomIndividualFitness); return weakestIndividualIndex; } }
void MutateMachineGeneSet(Genes& geneSetForMutation) { //std::cout<<"Inside machine mutation"<<std::endl; int numberOfGenes = geneSetForMutation.size(); int randomIndex = GetRandomIndex(numberOfGenes); for(int k=0;k<numberOfGenes;k++) geneSetForMutation[k]=0; geneSetForMutation[randomIndex]=1; }
Genes CreateMachineGenes(int numberOfMachines) { Genes machineGenes; for(int i=0;i<numberOfMachines;i++) machineGenes.push_back(0); int machineIndexRandomlyGenerated = GetRandomIndex(numberOfMachines); machineGenes[machineIndexRandomlyGenerated] = 1; return machineGenes; }
Population CrossOver(Population& individualsToCrossover, double crossOverProbability) { Individual individual1 = individualsToCrossover[0]; Individual individual2 = individualsToCrossover[1]; Individual crossedOverIndividual1, crossedOverIndividual2; int crossOverUnitIndex = GetRandomIndex(individual1.size()); // In which Unit does crossover occur? UnitGene crossOverUnitGene1 = individual1[crossOverUnitIndex]; UnitGene crossOverUnitGene2 = individual2[crossOverUnitIndex]; int crossOverGeneSetIndex = GetRandomIndex(crossOverUnitGene1.size()); // Till what gene set in the chosen Unit is the individual preserved? for(int i=0;i<individual1.size();i++) { if(i<crossOverUnitIndex) { crossedOverIndividual1.push_back(individual1[i]); crossedOverIndividual2.push_back(individual2[i]); } else if(i==crossOverUnitIndex) { UnitGene tempUnitGene1, tempUnitGene2; for(int j=0;j<crossOverUnitGene1.size();j++) { if(j<crossOverGeneSetIndex) { tempUnitGene1.push_back(crossOverUnitGene1[j]); tempUnitGene2.push_back(crossOverUnitGene2[j]); } else { tempUnitGene2.push_back(crossOverUnitGene1[j]); tempUnitGene1.push_back(crossOverUnitGene2[j]); } } crossedOverIndividual1.push_back(tempUnitGene1); crossedOverIndividual2.push_back(tempUnitGene2); } else { crossedOverIndividual1.push_back(individual2[i]); crossedOverIndividual2.push_back(individual1[i]); } } Population crossedOverIndividuals{crossedOverIndividual1, crossedOverIndividual2}; return crossedOverIndividuals; }
static std::wstring GetRandomLastname() { static const std::vector<std::wstring> lastnames { L"Smith", L"Jones", L"Lewis", L"Newman", L"Garcia", L"Perez", L"Книжник" }; auto index = GetRandomIndex(lastnames.size()); return lastnames[index]; }
static std::wstring GetRandomFirnstname() { static const std::vector<std::wstring> firstnames { L"John", L"Michael", L"Anthony", L"George", L"Javier", L"Nichole", L"Maria", L"Jose", L"Константин" }; auto index = GetRandomIndex(firstnames.size()); return firstnames[index]; }
UnitGene CrossOverAxleGeneSet(UnitGene& geneSetsToBeCrossedOver){ //std::cout<<"Inside Axle Cross Over"<<std::endl; Genes genesToBeCrossedOver1 = geneSetsToBeCrossedOver[0]; Genes genesToBeCrossedOver2 = geneSetsToBeCrossedOver[1]; Genes crossedOverGeneSet1, crossedOverGeneSet2; int numberOfGenes = genesToBeCrossedOver1.size(); int crossOverPoint = GetRandomIndex(numberOfGenes); //std::cout<<"Crossover Point: "<<crossOverPoint<<std::endl; for(int i=0;i<numberOfGenes;i++) { if(i<crossOverPoint) { crossedOverGeneSet1.push_back(genesToBeCrossedOver1[i]); crossedOverGeneSet2.push_back(genesToBeCrossedOver2[i]); } else { crossedOverGeneSet1.push_back(genesToBeCrossedOver2[i]); crossedOverGeneSet2.push_back(genesToBeCrossedOver1[i]); } } UnitGene crossedOverGeneSets{crossedOverGeneSet1, crossedOverGeneSet2}; return crossedOverGeneSets; }
static ref<CWarehouseReceipt> CreateWarehouseReceipt(db::db_filler::FillDatabaseData const& db_fill_data, const wchar_t* number) { w_ref<CWarehouseReceipt> wh_receipt = db::warehouse_receipt::Create(number); wh_receipt->SetConsigneeName(GetRandomName().c_str()); const auto shipper_name = GetRandomName(); wh_receipt->SetShipperName(shipper_name.c_str()); wh_receipt->SetShipperEmail(CreateEmail(shipper_name).c_str()); for (size_t i = 0; i < GetRandomIndex(db_fill_data.MaxItemsPerWarehouseReceipt); ++i) { db::warehouse_receipt::AddItem(wh_receipt, CreateWarehouseItem()); } for (auto& field_value : GetCustomFieldsValue()) { db::custom_fields::SetCustomField(anyref(wh_receipt), field_value.first.c_str(), field_value.second.c_str()); } return wh_receipt; }
//==============// // ROS Callback // void pc_callback(const sensor_msgs::PointCloud2ConstPtr msg) { pcl::PointCloud<pcl::PointXYZ> temp; pcl::fromROSMsg(*msg, temp); // Make sure the point cloud is in the base-frame listener_.waitForTransform(m_strBaseFrame, msg->header.frame_id, msg->header.stamp, ros::Duration(1.0)); pcl_ros::transformPointCloud(m_strBaseFrame, msg->header.stamp, temp, msg->header.frame_id, m_PointCloud, listener_); //=================// // Point filtering // //=================// m_MapPlaneSections.clear(); unsigned int n = temp.size(); // First count the useful points for (unsigned int i = 0; i < n; i++) { float x = temp[i].x; float y = temp[i].y; float d = hypot(x, y); if (d < 1e-2) { // Bogus point, ignore continue; } x = m_PointCloud[i].x; y = m_PointCloud[i].y; d = hypot(x, y); if (d > m_dMaxRange) { // too far, ignore continue; } double z = floor(m_PointCloud[i].z / m_dDiscretizationPrecision)*m_dDiscretizationPrecision; auto it = m_MapPlaneSections.find(z); if(it != m_MapPlaneSections.end()) it->second.push_back(i); else m_MapPlaneSections[z] = std::vector<size_t>(1, i); } ///////////////////////////////// // Start processing the points // ///////////////////////////////// // Stores the circle found for each section (if a circle was found) std::map<double, std::pair<pcl::PointXYZ, double>> mapCirclesInPlaneSections; for(auto it = m_MapPlaneSections.begin(); it != m_MapPlaneSections.end(); ++it) { std::vector<size_t> vFittingPointsTemp; // Stores the indices of the fitting points for the best model std::vector<size_t> vFittingPoints; size_t best = 0; // Stores the center for the best model pcl::PointXYZ center; // Stores the radius for the best model double radius; // Reference on the vector of points indices which belong to the section pointed by 'it' auto pvPoints = &(it->second); // pvPoint has type std::vector<size_t>* n = pvPoints->size(); // # points in the section 'z = it->first' #ifdef _VERBOSE ROS_INFO("%d useful points in the section at z = %.2f", n, it->first); #endif if(n < THRESHOLD_LOWER_TO_PARSE_PLANE) continue; if(n > THRESHOLD_UPPER_TO_PARSE_PLANE) continue; //////////////// // RANSACK START //////////////// for (unsigned int i = 0; i < (unsigned) m_nSamples; ++i) { vFittingPointsTemp.clear(); pcl::PointXYZ samplePoints[3]; pcl::PointXYZ centerForCurrentIteration; double radiusForCurrentIteration; // Pick up 3 random points with indices taken from pvPoints int buffer[3] = {-1, -1, -1}; // Used to guarantee that we pick 3 different numbers for (int j = 0; j < 3; j++) { bool bNumberAlreadyFound = true; int index = GetRandomIndex(n); while(bNumberAlreadyFound) { for(int i = 0; i <= j; ++i) { if(index == buffer[i]) { index = GetRandomIndex(n); i = -1; } } bNumberAlreadyFound = false; buffer[j] = index; } const pcl::PointXYZ &pt = m_PointCloud[pvPoints->at(index)]; samplePoints[j] = pcl::PointXYZ(pt.x, pt.y, pt.z); } // Calculate the circle which intersects the 3 points chosen GetCenterRadiusFrom3Points2D(centerForCurrentIteration, radiusForCurrentIteration, samplePoints[0], samplePoints[1], samplePoints[2]); #ifdef _VERBOSE /*ROS_INFO("Circle from A(%.5f, %.5f, %.5f) B(%.5f, %.5f, %.5f) C(%.5f, %.5f, %.5f) - Center (%.2f, %.2f, %.2f) Radius %.2f", samplePoints[0].x, samplePoints[0].y, samplePoints[0].z, samplePoints[1].x, samplePoints[1].y, samplePoints[1].z, samplePoints[2].x, samplePoints[2].y, samplePoints[2].z, centerForCurrentIteration.x, centerForCurrentIteration.y, centerForCurrentIteration.z, radiusForCurrentIteration ); */ #endif // Evaluation size_t score = 0; for (auto itPtIndex = pvPoints->begin(); itPtIndex!=pvPoints->end(); ++itPtIndex) { // Calculate the score for this model if (IsInCircle(centerForCurrentIteration, radiusForCurrentIteration, m_PointCloud[*itPtIndex], m_dRansacTolerance)) { score++; vFittingPointsTemp.push_back(*itPtIndex); } } // Update if a better model is found if (score > best) { best = score; center = centerForCurrentIteration; radius = radiusForCurrentIteration; vFittingPoints = vFittingPointsTemp; } } // END OF RANSAC FOR THIS SECTION // ^___^ if (best < THRESHOLD_TO_ACCEPT_CIRCLE_FOUND) continue; if(radius > MAXIMUM_RADIUS) continue; #ifdef _DO_LINEAR_REGRESSION ////////////////////////////////////////// // Linear regression to refine the results Eigen::MatrixXf A(3,3); Eigen::MatrixXf B(3,1); // Initialize the default values of A and B for(unsigned int i = 0; i < 3; i++) { B(i, 0) = 0; for(unsigned int j = 0; j < 3; j++) A(i, j) = 0; } A(2,2) = n; for (auto itPtIndex = vFittingPoints.begin(); itPtIndex != vFittingPoints.end(); ++itPtIndex) { // Assign x,y to the coordinates of the point we are considering. double x = m_PointCloud[*itPtIndex].x; double y = m_PointCloud[*itPtIndex].y; A(0,0) = A(0,0) + x*x; A(0,1) = A(0,1) + x*y; A(0,2) = A(0,2) + x; A(1,0) = A(1,0) + x*y; A(1,1) = A(1,1) + y*y; A(1,2) = A(1,2) + y; A(2,0) = A(2,0) + x; A(2,1) = A(2,1) + y; B(0,0) = B(0,0) + x*(x*x+y*y); B(1,0) = B(1,0) + y*(x*x+y*y); B(2,0) = B(2,0) + x*x+y*y; } Eigen::MatrixXf X = A.colPivHouseholderQr().solve(B); center.x = -X(0)/2.0; center.y = -X(1)/2.0; radius = sqrt(4*X(2)+X(0)*X(0)+X(1)*X(1))/2; #endif // At last add the circle found to mapCirclesInPlaneSections mapCirclesInPlaneSections[it->first] = std::make_pair(center, radius); #ifdef _VERBOSE ROS_INFO("Circle found at z = %.2f with score %d and Center (%.2f, %.2f) Radius %.2f", it->first, (int)best, center.x, center.y, radius ); #endif } #ifdef _VERBOSE ROS_INFO("%d planes with a valid circle inside", mapCirclesInPlaneSections.size() ); #endif //================================================== // CONCAT THE RESULTS FROM mapCirclesInPlaneSections if(mapCirclesInPlaneSections.size() < MIN_NUM_VALID_SECTIONS_TO_ACCEPT_CYLINDER) return; // Not enough acceptable sections to consider that there is a cylinder pcl::PointXYZ meanCenter = pcl::PointXYZ(0, 0, 0); double meanRadius = 0; int numSections = 1; double dBottomCap = DBL_MAX; double dTopCap; for(auto it = mapCirclesInPlaneSections.begin(); it!=mapCirclesInPlaneSections.end(); ++it) { auto itNext = it; ++itNext; #ifdef _VERBOSE ROS_INFO("Distance between centers %.2f Difference in radius %.2f Section count %d", GetDistanceBetweenPoints2D(it->second.first, itNext->second.first), fabs(it->second.second-itNext->second.second), numSections ); #endif if(itNext != mapCirclesInPlaneSections.end()) { if(GetDistanceBetweenPoints2D(it->second.first, itNext->second.first) < TOLERANCE_FOR_PLANE_CONCATENATION && fabs(it->second.second-itNext->second.second) < TOLERANCE_FOR_PLANE_CONCATENATION) { if(dBottomCap == DBL_MAX) dBottomCap = it->first; ++numSections; meanCenter.x += ((itNext->second.first.x + it->second.first.x)/2.0); meanCenter.y += ((itNext->second.first.y + it->second.first.y)/2.0); meanRadius += ((itNext->second.second+it->second.second)/2.0); } else { dTopCap = it->first; meanCenter.x /= numSections; meanCenter.y /= numSections; meanRadius /= numSections; if(numSections >= MIN_NUM_VALID_SECTIONS_TO_ACCEPT_CYLINDER) { geometry_msgs::PointStamped robotSpaceCenter; robotSpaceCenter.point.x = meanCenter.x; robotSpaceCenter.point.y = meanCenter.y; listener_.waitForTransform("/world", m_strBaseFrame, msg->header.stamp, ros::Duration(1.0)); tf::StampedTransform transform; listener_.lookupTransform("/world", m_strBaseFrame, msg->header.stamp, transform); // if(numSections < MIN_NUM_VALID_SECTIONS_TO_ACCEPT_CYLINDER) Not enough acceptable sections to consider that there is a cylinder auto worldSpaceCenter = transform * tf::Vector3(robotSpaceCenter.point.x, robotSpaceCenter.point.y, robotSpaceCenter.point.z); ROS_INFO("Cylinder found: Center (%.2f, %.2f) Radius %.2f Bottom z = %.2f Top z = %.2f", meanCenter.x, meanCenter.y, meanRadius, dBottomCap, dTopCap); ROS_INFO("Cylinder found (World space coordinates): Center (%.2f, %.2f) Radius %.2f Bottom z = %.2f Top z = %.2f", worldSpaceCenter.x(), worldSpaceCenter.y(), meanRadius, dBottomCap, dTopCap); } meanCenter = pcl::PointXYZ(0, 0, 0); meanRadius = 0; numSections = 1; dBottomCap = DBL_MAX; } } else { dTopCap = it->first; meanCenter.x /= numSections; meanCenter.y /= numSections; meanRadius /= numSections; if(numSections >= MIN_NUM_VALID_SECTIONS_TO_ACCEPT_CYLINDER) { geometry_msgs::PointStamped robotSpaceCenter; robotSpaceCenter.point.x = meanCenter.x; robotSpaceCenter.point.y = meanCenter.y; listener_.waitForTransform("/world", m_strBaseFrame, msg->header.stamp, ros::Duration(1.0)); tf::StampedTransform transform; listener_.lookupTransform("/world", m_strBaseFrame, msg->header.stamp, transform); // if(numSections < MIN_NUM_VALID_SECTIONS_TO_ACCEPT_CYLINDER) Not enough acceptable sections to consider that there is a cylinder auto worldSpaceCenter = transform * tf::Vector3(robotSpaceCenter.point.x, robotSpaceCenter.point.y, robotSpaceCenter.point.z); ROS_INFO("Cylinder found: Center (%.2f, %.2f) Radius %.2f Bottom z = %.2f Top z = %.2f", meanCenter.x, meanCenter.y, meanRadius, dBottomCap, dTopCap); ROS_INFO("Cylinder found (World space coordinates): Center (%.2f, %.2f) Radius %.2f Bottom z = %.2f Top z = %.2f", worldSpaceCenter.x(), worldSpaceCenter.y(), meanRadius, dBottomCap, dTopCap); } } // ^___^ } }
static std::wstring GetRandomLocationCode() { std::wstring location_code = L"LOC"; location_code += std::to_wstring(GetRandomIndex(100)); return location_code; }