/* Calculates and returns the chance of an invalid election based * on 500 trials with 2 candidates A and B. Candidate A should be the * winner by the percentSpread margin but votes are cast with percentError * as the probability of votes being inverted. Expects the following * parameters, the number of voters in the election, The percentage spread * between the two candidates and the voting error percentage. */ double chanceOfInvalid(int numVoters, double percentSpread, double percentError) { int numInvalid = 0; // first calculate the number of voters for each candidate A and B int votersForA, votersForB; double percentVotersA = 50.0 + (percentSpread * 100.0) / 2.0; double percentVotersB = 50.0 - (percentSpread * 100.0) / 2.0; calculateVoters(votersForA, numVoters, percentVotersA); calculateVoters(votersForB, numVoters, percentVotersB); int totalVotesA = 0, totalVotesB = 0; // loop for the 500 trials for (int i = 0; i < NUM_TRIALS; i++) { // cast votes for each of the candidate with the error percent castVotes(totalVotesA, votersForA, percentError); castVotes(totalVotesB, votersForB, percentError); if (totalVotesA < totalVotesB) numInvalid++; } // return the chance of an invalid election return (double(numInvalid) / double(NUM_TRIALS)) * 100.0; }
void NormalHoughProposer::houghVote(Entry &query, Entry &target, bin_t& bins) { // Compute the reference point for the R-table Eigen::Vector4f centroid4; compute3DCentroid(*(target.cloud), centroid4); Eigen::Vector3f centroid(centroid4[0], centroid4[1], centroid4[2]); assert(query.keypoints->size() == query.features->size()); assert(target.keypoints->size() == target.features->size()); // Figure out bin dimension Eigen::Vector4f query_min4, query_max4; getMinMax3D(*query.cloud, query_min4, query_max4); Eigen::Vector3f query_min(query_min4[0], query_min4[1], query_min4[2]); Eigen::Vector3f query_max(query_max4[0], query_max4[1], query_max4[2]); Eigen::Affine3f t; getTransformation(0, 0, 0, M_PI, 0.5, 1.5, t); int correctly_matched = 0; for (unsigned int i = 0; i < query.keypoints->size(); i++) { std::vector<int> feature_indices; std::vector<float> sqr_distances; int num_correspondences = 2; if (!pcl_isfinite (query.features->points.row(i)(0))) continue; int num_found = target.tree->nearestKSearch(*query.features, i, num_correspondences, feature_indices, sqr_distances); for (int j = 0; j < num_found; j++) { // For each one of the feature correspondences int feature_index = feature_indices[j]; Eigen::Vector3f query_keypoint = query.keypoints->at(i).getVector3fMap(); Eigen::Vector3f target_keypoint = target.keypoints->at(feature_index).getVector3fMap(); target_keypoint = t * target_keypoint; if ((query_keypoint - target_keypoint).norm() < 0.05) { correctly_matched++; } // Get corresponding target keypoint, and calculate its r to its centroid PointNormal correspondence = target.keypoints->at(feature_index); // Since features correspond to the keypoints Eigen::Vector3f r = correspondence.getVector3fMap() - centroid; // Calculate the rotation transformation from the target normal to the query normal Eigen::Vector3f target_normal = correspondence.getNormalVector3fMap(); target_normal.normalize(); Eigen::Vector3f query_normal = query.keypoints->at(i).getNormalVector3fMap(); query_normal.normalize(); double angle = acos( target_normal.dot(query_normal) / (target_normal.norm() * query_normal.norm()) ); Eigen::Vector3f axis = target_normal.normalized().cross(query_normal.normalized()); axis.normalize(); Eigen::Affine3f rot_transform; rot_transform = Eigen::AngleAxisf (angle, axis); // Check that the rotation matrix is correct Eigen::Vector3f projected = rot_transform * target_normal; projected.normalize(); // Transform r based on the difference between the normals Eigen::Vector3f transformed_r = rot_transform * r; for (int k = 0; k < num_angles_; k++) { float query_angle = (float(k) / num_angles_) * 2.0f * float (M_PI); Eigen::Affine3f query_rot; query_rot = Eigen::AngleAxisf(query_angle, query_normal.normalized()); Eigen::Vector3f guess_r = query_rot * transformed_r; Eigen::Vector3f centroid_est = query.keypoints->at(i).getVector3fMap() - guess_r; Eigen::Vector3f region = query_max - query_min; Eigen::Vector3f bin_size = region / float (bins_); Eigen::Vector3f diff = (centroid_est - query_min); Eigen::Vector3f indices = diff.cwiseQuotient(bin_size); castVotes(indices, bins); } } } }