/*! * \param[in] aPos point to insert into the polygon * \param[in] aPoly polygon */ int ImageHolder::posInPolygon(QPoint *aPos, QPolygon *aPoly) const { if (!aPos || !aPoly || aPoly->count() < 2 || aPos->isNull()) { return -1; /* NOTREACHED */ } int x = aPos->x(); int y = aPos->y(); int index = 0; int dist = 100000; int temp = 0; int count = aPoly->count(); QRect rect; for (int i = 0; i < count - 1; i++) { temp = pointToLineDistance( QLine(aPoly->at(i), aPoly->at(i + 1)), *aPos ); rect.setTopLeft(aPoly->at(i)); rect.setBottomRight(aPoly->at(i + 1)); rect = rect.normalized(); if (temp < dist && ((x < rect.right() && rect.left() < x) || (y < rect.bottom() && rect.top() < y))) { dist = temp; index = i + 1; } } /* first and last points */ temp = pointToLineDistance( QLine(aPoly->at(0), aPoly->at(count - 1)), *aPos ); rect.setTopLeft(aPoly->at(0)); rect.setBottomRight(aPoly->at(count - 1)); rect = rect.normalized(); if (temp < dist && ((x < rect.right() && rect.left() < x) || (y < rect.bottom() && rect.top() < y))) { index = 0; } return index; }
template <typename PointT, typename PointNT> void pcl::SampleConsensusModelCylinder<PointT, PointNT>::selectWithinDistance ( const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers) { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) { inliers.clear (); return; } int nr_p = 0; inliers.resize (indices_->size ()); error_sqr_dists_.resize (indices_->size ()); Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); float ptdotdir = line_pt.dot (line_dir); float dirdotdir = 1.0f / line_dir.dot (line_dir); // Iterate through the 3d points and calculate the distances from them to the sphere for (size_t i = 0; i < indices_->size (); ++i) { // Approximate the distance from the point to the cylinder as the difference between // dist(point,cylinder_axis) and cylinder radius Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); // Calculate the point's projection on the cylinder axis float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir; Eigen::Vector4f pt_proj = line_pt + k * line_dir; Eigen::Vector4f dir = pt - pt_proj; dir.normalize (); // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector double d_normal = fabs (getAngle3D (n, dir)); d_normal = (std::min) (d_normal, M_PI - d_normal); double distance = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); if (distance < threshold) { // Returns the indices of the points whose distances are smaller than the threshold inliers[nr_p] = (*indices_)[i]; error_sqr_dists_[nr_p] = distance; ++nr_p; } } inliers.resize (nr_p); error_sqr_dists_.resize (nr_p); }
void ObjectModelCylinder::selectWithinDistance (const Eigen::VectorXd &model_coefficients, double threshold, std::vector<int> &inliers){ assert (model_coefficients.size () == 7); int nr_p = 0; inliers.resize (this->inputPointCloud->getSize()); Eigen::Vector4d line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); Eigen::Vector4d line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); double ptdotdir = line_pt.dot (line_dir); double dirdotdir = 1.0 / line_dir.dot (line_dir); // Iterate through the 3d points and calculate the distances from them to the sphere for (size_t i = 0; i < this->inputPointCloud->getSize(); ++i) { // Aproximate the distance from the point to the cylinder as the difference between // dist(point,cylinder_axis) and cylinder radius Eigen::Vector4d pt = Eigen::Vector4d ((*inputPointCloud->getPointCloud())[i].getX(), (*inputPointCloud->getPointCloud())[i].getY(), (*inputPointCloud->getPointCloud())[i].getZ(), 0); Eigen::Vector4d n = Eigen::Vector4d (this->normals->getNormals()->data()[i].getX(), this->normals->getNormals()->data()[i].getY(), this->normals->getNormals()->data()[i].getZ(), 0); double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); // Calculate the point's projection on the cylinder axis double k = (pt.dot (line_dir) - ptdotdir) * dirdotdir; Eigen::Vector4d pt_proj = line_pt + k * line_dir; Eigen::Vector4d dir = pt - pt_proj; dir.normalize (); // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector double d_normal = fabs (getAngle3D (n, dir)); d_normal = fmin (d_normal, M_PI - d_normal); if (fabs (this->normalDistanceWeight * d_normal + (1 - this->normalDistanceWeight) * d_euclid) < threshold) { // Returns the indices of the points whose distances are smaller than the threshold inliers[nr_p] = i; nr_p++; } } inliers.resize (nr_p); }
bool ObjectModelCylinder::doSamplesVerifyModel (const std::set<int> &indices, const Eigen::VectorXd &model_coefficients, double threshold){ assert (model_coefficients.size () == 7); Eigen::Vector4d pt; for (std::set<int>::iterator it = indices.begin (); it != indices.end (); ++it) { // Aproximate the distance from the point to the cylinder as the difference between // dist(point,cylinder_axis) and cylinder radius // @note need to revise this. pt = Eigen::Vector4d ((*inputPointCloud->getPointCloud())[*it].getX(), (*inputPointCloud->getPointCloud())[*it].getY(), (*inputPointCloud->getPointCloud())[*it].getZ(), 0); if (fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]) > threshold) return (false); } return (true); }
template <typename PointT, typename PointNT> void pcl::SampleConsensusModelCylinder<PointT, PointNT>::getDistancesToModel ( const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) { distances.clear (); return; } distances.resize (indices_->size ()); Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); float ptdotdir = line_pt.dot (line_dir); float dirdotdir = 1.0f / line_dir.dot (line_dir); // Iterate through the 3d points and calculate the distances from them to the sphere for (size_t i = 0; i < indices_->size (); ++i) { // Aproximate the distance from the point to the cylinder as the difference between // dist(point,cylinder_axis) and cylinder radius // @note need to revise this. Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); // Calculate the point's projection on the cylinder axis float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir; Eigen::Vector4f pt_proj = line_pt + k * line_dir; Eigen::Vector4f dir = pt - pt_proj; dir.normalize (); // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector double d_normal = fabs (getAngle3D (n, dir)); d_normal = (std::min) (d_normal, M_PI - d_normal); distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); } }
void ObjectModelCylinder::getDistancesToModel (const Eigen::VectorXd &model_coefficients, std::vector<double> &distances){ assert (model_coefficients.size () == 7); distances.resize (this->inputPointCloud->getSize()); Eigen::Vector4d line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); Eigen::Vector4d line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); double ptdotdir = line_pt.dot (line_dir); double dirdotdir = 1.0 / line_dir.dot (line_dir); // Iterate through the 3d points and calculate the distances from them to the sphere for (size_t i = 0; i < this->inputPointCloud->getSize(); ++i) { // Aproximate the distance from the point to the cylinder as the difference between // dist(point,cylinder_axis) and cylinder radius // Todo to be revised Eigen::Vector4d pt = Eigen::Vector4d ((*inputPointCloud->getPointCloud())[i].getX(), (*inputPointCloud->getPointCloud())[i].getY(), (*inputPointCloud->getPointCloud())[i].getZ(), 0); Eigen::Vector4d n = Eigen::Vector4d (this->normals->getNormals()->data()[i].getX(), this->normals->getNormals()->data()[i].getY(), this->normals->getNormals()->data()[i].getZ(), 0); double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); // Calculate the point's projection on the cylinder axis double k = (pt.dot (line_dir) - ptdotdir) * dirdotdir; Eigen::Vector4d pt_proj = line_pt + k * line_dir; Eigen::Vector4d dir = pt - pt_proj; dir.normalize (); // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector double d_normal = fabs (getAngle3D (n, dir)); d_normal = fmin (d_normal, M_PI - d_normal); distances[i] = fabs (this->normalDistanceWeight * d_normal + (1 - this->normalDistanceWeight) * d_euclid); } }
template <typename PointT, typename PointNT> bool pcl::SampleConsensusModelCylinder<PointT, PointNT>::doSamplesVerifyModel ( const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) { // Needs a valid model coefficients if (model_coefficients.size () != 7) { PCL_ERROR ("[pcl::SampleConsensusModelCylinder::doSamplesVerifyModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); return (false); } for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it) { // Aproximate the distance from the point to the cylinder as the difference between // dist(point,cylinder_axis) and cylinder radius // @note need to revise this. Eigen::Vector4f pt (input_->points[*it].x, input_->points[*it].y, input_->points[*it].z, 0); if (fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]) > threshold) return (false); } return (true); }
int main(int argc, char** argv){ AI theAI; fizGWindow aWindow; fizGTableState state(aWindow, *(theAI.theTable), ballRadius); fizPoint coor = theAI.theTable->getPocketCenter(SE_POCKET); state.randomize(); //state.setBall(CUE, STATIONARY, .5, .6); //state.setBall(TWO, STATIONARY, .1, .1); //state.setBall(TEN, STATIONARY, .2, .2); //state.setBall(THREE, STATIONARY, 1, 1.2); //misses shot //state.setBall(THREE, STATIONARY, .3, 2.3); //wtf?? problem finding phi, should be fixed vec2 p1(0,0); vec2 p2(3,3); vec2 p3(3,2); double testDist = pointToLineDistance(p1, p2, p3); printf("%f\n", testDist); /* double x = coor.x; double y = coor.y; fizGShot loadedShot; if(argc > 1){ double a,b, phi, theta, V; loadedShot.load(argv[1], state, a, b, phi, theta, V); } fizGShot theShot; shot bestShot; double score = theAI.chooseShot(state, 1, bestShot, solids); //bestShot.phi = 90; //bestShot.theta = 20; ShotStatus result = theShot.execute(*(theAI.theTable), state, bestShot.a, bestShot.b, bestShot.theta, bestShot.phi, bestShot.v); // write the shot data to file system("rm -f shotViz_info.txt"); system("rm -f shotViz_shot.txt"); ofstream info("shotViz_info.txt"); info << "shotViz_shot.txt ? ? ? ? ? ? ? ? ? ? ? ?" << endl; info.close(); bool savedOk = theShot.save("shotViz_shot.txt", false); // Visualize the shot using shotViz if (result == 0) { bool manualExit = true; // visualize the shot if (savedOk) { if (manualExit) system("./shotVizOld -m &"); else system("./shotVizOld &"); cout << "Done." << endl; } else cout << "ERROR generating shot preview: shot save result was bad." << endl; }*/ return 0; }
bool ObjectModelCylinder::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXd &model_coefficients){ assert (samples.size () == 2); if (!this->normals && this->normals->getSize()!=this->inputPointCloud->getSize()) { cout<<"[ObjectModelCylinder::computeModelCoefficients] No input dataset containing normals was given!"; return (false); } Eigen::Vector4d p1 = Eigen::Vector4d ((*inputPointCloud->getPointCloud())[samples[0]].getX(), (*inputPointCloud->getPointCloud())[samples[0]].getY(), (*inputPointCloud->getPointCloud())[samples[0]].getZ(), 0); Eigen::Vector4d p2 = Eigen::Vector4d ((*inputPointCloud->getPointCloud())[samples[1]].getX(), (*inputPointCloud->getPointCloud())[samples[1]].getY(), (*inputPointCloud->getPointCloud())[samples[1]].getZ(), 0); Eigen::Vector4d n1 = Eigen::Vector4d (this->normals->getNormals()->data()[samples[0]].getX(), this->normals->getNormals()->data()[samples[0]].getY(), this->normals->getNormals()->data()[samples[0]].getZ(), 0); Eigen::Vector4d n2 = Eigen::Vector4d (this->normals->getNormals()->data()[samples[1]].getX(), this->normals->getNormals()->data()[samples[1]].getY(), this->normals->getNormals()->data()[samples[1]].getZ(), 0); Eigen::Vector4d w = n1 + p1 - p2; double a = n1.dot (n1); double b = n1.dot (n2); double c = n2.dot (n2); double d = n1.dot (w); double e = n2.dot (w); double denominator = a*c - b*b; double sc, tc; // Compute the line parameters of the two closest points if (denominator < 1e-8) // The lines are almost parallel { sc = 0.0; tc = (b > c ? d / b : e / c); // Use the largest denominator } else { sc = (b*e - c*d) / denominator; tc = (a*e - b*d) / denominator; } // point_on_axis, axis_direction Eigen::Vector4d line_pt = p1 + n1 + sc * n1; Eigen::Vector4d line_dir = p2 + tc * n2 - line_pt; line_dir.normalize (); model_coefficients.resize (7); #ifdef EIGEN3 model_coefficients.head<3> () = line_pt.head<3> (); model_coefficients.segment<3> (3) = line_dir.head<3> (); #else model_coefficients.start<3> () = line_pt.start<3> (); model_coefficients.segment<3> (3) = line_dir.start<3> (); #endif // cylinder radius model_coefficients[6] = pointToLineDistance (p1, line_pt, line_dir); if (model_coefficients[6] > this->radiusMax || model_coefficients[6] < this->radiusMin) return (false); return (true); }