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