/*!
 * \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;
}
Beispiel #2
0
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);
}
Beispiel #3
0
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);
}
Beispiel #4
0
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);
}
Beispiel #5
0
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);
  }
}
Beispiel #6
0
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);
	}
}
Beispiel #7
0
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);
}
Beispiel #8
0
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;
}
Beispiel #9
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);
}