void GeoXGLWidget3D::mousePressEvent(QMouseEvent *event)
{
  if (mode == PICK)
  {
    findClosestPoint(event->x(),event->y());
  }
  if (mode == CAM || pickedPointIndex == -1)
  {
    camControl->mouseDown(event->x(),event->y(),MouseButtons(event->button()==Qt::LeftButton,event->button()==Qt::RightButton, event->button()==Qt::MidButton),event->modifiers());
  }

  updateGL();
}
void extractFace(const PCRGB::Ptr& input_pc, PCRGB::Ptr& out_pc, int u_click, int v_click)
{
    double trim_radius, skin_thresh;
    ros::param::param<double>("~trim_radius", trim_radius, 0.12);
    ros::param::param<double>("~skin_thresh", skin_thresh, 0.8);

    PCRGB::Ptr trimmed_pc(new PCRGB());
    int32_t closest_ind = findClosestPoint(input_pc, u_click, v_click);
    if(closest_ind < 0)
        return;
    sphereTrim(input_pc, trimmed_pc, closest_ind, trim_radius);
    extractSkinPC(trimmed_pc, out_pc, skin_thresh);
}
void InteractiveMarkerControl::moveAxis( const Ogre::Ray& mouse_ray, const ViewportMouseEvent& event )
{
  // compute control-axis ray based on grab_point_, etc.
  Ogre::Ray control_ray;
  control_ray.setOrigin( grab_point_ );
  control_ray.setDirection( control_frame_node_->getOrientation() * control_orientation_.xAxis() );
  
  // project control-axis ray onto screen.
  Ogre::Vector2 control_ray_screen_start, control_ray_screen_end;
  worldToScreen( control_ray.getOrigin(), event.viewport, control_ray_screen_start );
  worldToScreen( control_ray.getPoint( 1 ), event.viewport, control_ray_screen_end );

  Ogre::Vector2 mouse_point( event.x, event.y );

  // Find closest point on projected ray to mouse point
  // Math: if P is the start of the ray, v is the direction vector of
  //       the ray (not normalized), and X is the test point, then the
  //       closest point on the line to X is given by:
  //
  //               (X-P).v
  //       P + v * -------
  //                 v.v
  //       where "." is the dot product.
  Ogre::Vector2 control_ray_screen_dir = control_ray_screen_end - control_ray_screen_start;
  double denominator = control_ray_screen_dir.dotProduct( control_ray_screen_dir );
  if( fabs( denominator ) > Ogre::Matrix3::EPSILON ) // If the control ray is not straight in line with the view.
  {
    double factor =
      ( mouse_point - control_ray_screen_start ).dotProduct( control_ray_screen_dir ) / denominator;
    
    Ogre::Vector2 closest_screen_point = control_ray_screen_start + control_ray_screen_dir * factor;

    // make a new "mouse ray" for the point on the projected ray
    int width = event.viewport->getActualWidth() - 1;
    int height = event.viewport->getActualHeight() - 1;
    Ogre::Ray new_mouse_ray = event.viewport->getCamera()->getCameraToViewportRay( (closest_screen_point.x+.5) / width,
                                                                                   (closest_screen_point.y+.5) / height );
    new_mouse_ray.setOrigin( reference_node_->convertWorldToLocalPosition( new_mouse_ray.getOrigin() ) );
    new_mouse_ray.setDirection( reference_node_->convertWorldToLocalOrientation( Ogre::Quaternion::IDENTITY ) * new_mouse_ray.getDirection() );

    // find closest point on control-axis ray to new mouse ray (should intersect actually)
    Ogre::Vector3 closest_point;
    if( findClosestPoint( control_ray, new_mouse_ray, closest_point ))
    {
      // set position of parent to closest_point - grab_point_ + parent_position_at_mouse_down_.
      parent_->setPose( closest_point - grab_point_ + parent_position_at_mouse_down_,
                        parent_->getOrientation(), name_ );
    }
  }
}
	void findGoodCorners2(const Mat &grayFrame, const SoccerPitchData &data, Mat &currToKeyTrans, Mat &keyToTopTrans) {
		Mat topToCurrTrans;
		invert(keyToTopTrans * currToKeyTrans, topToCurrTrans);
		vector<Point2f> imagePitchOuterContour;
		perspectiveTransform(data.pitchOuterPoints, imagePitchOuterContour, topToCurrTrans);

		vector<Point2f> hull;
		convexHull(imagePitchOuterContour, hull);

		Mat mask = Mat::zeros(frameSize, CV_8UC1);
		fillConvexPoly(mask, vector<Point>(hull.begin(), hull.end()), Scalar(1, 0, 0));

		dilate(mask, mask, getStructuringElement(MORPH_ELLIPSE, Size(3, 3)));

		Mat bin;
		adaptiveThreshold(grayFrame, bin, 255, ADAPTIVE_THRESH_MEAN_C , THRESH_BINARY, 5, -10);
		
		vector<Point2f> candidateCorners;
		goodFeaturesToTrack(bin, candidateCorners, 100, 0.01, 24, mask);

		cornerSubPix(bin, candidateCorners, Size(5, 5), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS & CV_TERMCRIT_ITER, 40, 0.001));

		vector<Point2f> goodPoints;
		for (Point2f corner : candidateCorners) {
			if (goodCornerCheck(corner, bin) && !closeToBoundary(corner))
				goodPoints.push_back(corner);
		}

		if (goodPoints.size() > 0) {
			vector<Point2f> reprojGoodPoints;
			perspectiveTransform(goodPoints, reprojGoodPoints, keyToTopTrans * currToKeyTrans);
			// try to add these new corners into the relocatedCorners
			for (int i = 0; i < reprojGoodPoints.size(); i++) {
				// if does not exists already and coincide with reproj of 28 points
				bool exists = hasSimilarPoint(relocatedPitchPoints, reprojGoodPoints[i], 10) ;
				int minId = findClosestPoint(data.pitchPoints, reprojGoodPoints[i]);
				double minDist = norm(reprojGoodPoints[i] - data.pitchPoints[minId]);
				if ((!exists ) && (minDist < 16) && (minDist < reprojErr[minId])) {
					relocatedCorners.push_back(goodPoints[i]);
					relocatedPitchPoints.push_back(data.pitchPoints[minId]);
					reprojErr[minId] = minDist;
				}
			}
		}

		cout<<relocatedCorners.size()<<" points relocated"<<endl;
	}
Esempio n. 5
0
void								Landscape::smoothMap(void)
{
	sPoint							closestPoint;
	std::vector<sPoint>::iterator	it;

	for (it = _immovablePoints.begin(); it != _immovablePoints.end(); ++it)
	{
		findClosestPoint((*it), closestPoint);
		std::cout << "Closest point from:\t" 
				  << it->x << ", " 
				  << it->y << ", " 
				  << it->z << std::endl
				  << "found at:\t\t" 
				  << closestPoint.x << ", " 
				  << closestPoint.y << ", " 
				  << closestPoint.z << std::endl;
		smoothPoint(*it, closestPoint);
		std::cout << "Fill map:\tDONE" << std::endl;
	}
}
void extractFaceColorModel(const PCRGB::Ptr& input_pc, PCRGB::Ptr& out_pc, int u_click, int v_click)
{
    double trim_radius, model_radius, color_std_thresh;
    ros::param::param<double>("~trim_radius", trim_radius, 0.12);
    ros::param::param<double>("~model_radius", model_radius, 0.02);
    ros::param::param<double>("~color_std_thresh", color_std_thresh, 1.5);

    int32_t closest_ind = findClosestPoint(input_pc, u_click, v_click);
    if(closest_ind < 0)
        return;

    PCRGB::Ptr model_pc(new PCRGB());
    sphereTrim(input_pc, model_pc, closest_ind, model_radius);
    Eigen::Vector3d mean_color;
    Eigen::Matrix3d cov_color;
    generateColorModel(model_pc, mean_color, cov_color);

    PCRGB::Ptr trimmed_pc(new PCRGB());
    sphereTrim(input_pc, trimmed_pc, closest_ind, trim_radius);
    extractPCFromColorModel(trimmed_pc, out_pc, mean_color, cov_color, color_std_thresh);
}
Esempio n. 7
0
std::vector<Eigen::Vector3f> PathPlanner::getPath(Eigen::Vector3f start, Eigen::Vector3f end, cv::Mat3b * image)
{
    std::vector<Eigen::Vector3f> path;
    std::vector<Eigen::Vector2i> pathImg;

    //Objects are outside the configuration space, so get the closest point
    Eigen::Vector2i lastPoint;
    if(!validPoint(worldToImg(end)))
    {
        if(!findClosestPoint(start, end, bot_in_pixels_))
        {
            return path;
        }
    }

    //Check if we can just go straight there unobstructed
    if(traceLine(worldToImg(start), worldToImg(end)))
    {
        path.push_back(start);
        path.push_back(end);
    }
    else if(aStar(worldToImg(start), worldToImg(end), pathImg))
    {
        pathImg = smoothPath(pathImg);

        for(size_t i = 0; i < pathImg.size(); i++)
        {
            path.push_back(imgToWorld(pathImg.at(i)));
        }
    }

    if(image)
    {
        drawPath(path, *image);
    }

    return path;
}
Esempio n. 8
0
vector< pair< Point2i , Point2i > > FireflyOptimizator::findMatch(vector< Point3i > points1,vector< Point3i > points2) {

   // vector< Point3i > points1  = im1.getMarkers();
   // vector< Point3i > points2  = im2.getMarkers();

    sort(points1.begin(),points1.end(),sortByXAxis);
    sort(points1.begin(),points1.end(),sortByXAxis);

    vector< pair< Point2i, Point2i > > result;


    //cout<<points1.size()<<" "<<points2.size()<<endl;
    for( Point3i p1 : points1) {
        int winnerPos = findClosestPoint(p1,points2);
        //cout<<winnerPos<<endl;
        Point2i a(p1.x,p1.y),b(points2[winnerPos].x,points2[winnerPos].y);
        result.push_back(make_pair(a,b));
        points2.erase(points2.begin()+winnerPos);

    }

    return result;

}
Esempio n. 9
0
void ContourExtractor::findContours(frsmPoint * points, unsigned numValidPoints, std::vector<Contour*> &contours)
{

  priority_queue<Join*, std::vector<Join*>, JoinCompare> joins;

  std::vector<PointRecord> pointrecs;

  int range = searchRange;

  pointrecs.resize(numValidPoints);
  //  int i = 0;
  // create the point record table, one for every point.
  for (unsigned int parent = 0; parent < numValidPoints; parent++) {
    pointrecs[parent].point = points[parent];
  }

  // build the joins...
  // everybody gets their first pick to begin with.
  int numJoins = 0;
  for (unsigned int parent = 0; parent < pointrecs.size(); parent++) {
    Join *j;
    j = findClosestPoint(pointrecs, parent, parent - range, parent - 1);
    if (j != NULL) {
      joins.push(j);
      numJoins++;
    }

    j = findClosestPoint(pointrecs, parent, parent + 1, parent + range);
    if (j != NULL) {
      joins.push(j);
      numJoins++;
    }
  }

  // now we start plucking the best joins. If someone's best choice is
  // taken, we let them
  // pick a new partner and reinsert them into the queue.
  Join j;
  while (joins.size() > 0) {
    Join *jtmp = joins.top();
    joins.pop();
    j = *jtmp;
    delete jtmp;

    //    printf("JOIN cost=%f, parent=%d, victim=%d\n",j.cost,j.parent,j.victim);
    //    continue;

    // victim <--> parent
    if (j.parent > j.victim) {
      // if this parent has already been joined, we're done.
      if (pointrecs[j.parent].left >= 0)
        continue;

      // is the victim still available?
      if (pointrecs[j.victim].right < 0) {
        if (j.cost > alwaysOkayDistance) {
          if (pointrecs[j.victim].leftDistance > minDistForDistRatio && j.cost / pointrecs[j.victim].leftDistance
              > maxDistanceRatio)
            continue;
          else if (pointrecs[j.victim].leftDistance < minDistForDistRatio && j.cost > maxDistForSkippingDistRatio)
            continue;
          if (pointrecs[j.parent].rightDistance > minDistForDistRatio && j.cost / pointrecs[j.parent].rightDistance
              > maxDistanceRatio)
            continue;
          else if (pointrecs[j.parent].rightDistance < minDistForDistRatio && j.cost > maxDistForSkippingDistRatio)
            continue;

          if (pointrecs[j.victim].leftDistance < 0 && pointrecs[j.parent].rightDistance < 0 && j.cost
              > maxFirstDistance)
            continue;
        }

        // yes, join.
        pointrecs[j.victim].right = j.parent;
        pointrecs[j.parent].left = j.victim;
        pointrecs[j.parent].leftDistance = j.cost;
        pointrecs[j.victim].rightDistance = j.cost;
      }
      else {
        // search for a new point.
        jtmp = findClosestPoint(pointrecs, j.parent, j.parent - range, j.parent - 1);
        if (jtmp != NULL)
          joins.push(jtmp);
      }

      continue;
    }

    // parent <--> victim. Same as above, roles reversed.
    if (j.parent < j.victim) {
      if (pointrecs[j.parent].right >= 0)
        continue;

      if (pointrecs[j.victim].left < 0) {
        if (j.cost > alwaysOkayDistance) {

          if (pointrecs[j.parent].leftDistance > minDistForDistRatio && j.cost / pointrecs[j.parent].leftDistance
              > maxDistanceRatio)
            continue;
          else if (pointrecs[j.parent].leftDistance < minDistForDistRatio && j.cost > maxDistForSkippingDistRatio)
            continue;
          if (pointrecs[j.victim].rightDistance > minDistForDistRatio && j.cost / pointrecs[j.victim].rightDistance
              > maxDistanceRatio)
            continue;
          else if (pointrecs[j.victim].rightDistance < minDistForDistRatio && j.cost > maxDistForSkippingDistRatio)
            continue;

          if (pointrecs[j.parent].leftDistance < 0 && pointrecs[j.victim].rightDistance < 0 && j.cost
              > maxFirstDistance)
            continue;
        }

        pointrecs[j.victim].left = j.parent;
        pointrecs[j.parent].right = j.victim;
        pointrecs[j.parent].rightDistance = j.cost;
        pointrecs[j.victim].leftDistance = j.cost;

      }
      else {
        jtmp = findClosestPoint(pointrecs, j.parent, j.parent + 1, j.parent + range);
        if (jtmp != NULL)
          joins.push(jtmp);
      }
      continue;
    }

  }

  int contour = 0;

  // pull out the contours.
  for (unsigned int i = 0; i < pointrecs.size(); i++) {
    // we have a new contour.
    if (pointrecs[i].contour < 0) {
      //      if (debug)
      //        System.out.print("contour " + contour + " " + pointrecs[i].left + ": ");

      assert(pointrecs[i].left == -1);

      Contour * c = new Contour();
      int p = i;
      while (p >= 0) {
        //        if (debug)
        //          System.out.print(p + " ");
        c->points.push_back(pointrecs[p].point);
        pointrecs[p].contour = contour;
        p = pointrecs[p].right;
      }

      //      if (debug)
      //        System.out.println("");
      contour++;

      if (c->points.size() == 0) {
        printf("*Adding empty contour!?\n");
      }

      if (simplifyContourThresh > 0 && c->points.size() > 2) {
        c->simplify(simplifyContourThresh);
      }
      contours.push_back(c);

    }
  }

  pointrecs.clear();

}
Esempio n. 10
0
TennisFieldDelimiter *TennisFieldCalibrator::computeTennisFieldDelimiter(Mat calibrationFrame, vector<Point> intersPts, CalibrationWindow *calibWnd)
{
	TennisFieldDelimiter *retval = new TennisFieldDelimiter();
	Point2f bottomLeft, bottomRight;
	Point2f topLeft, topRight;

	double h = calibWnd->bottomLeft.y - calibWnd->topLeft.y;
	double centerX = calibWnd->topLeft.x + (calibWnd->topRight.x - calibWnd->topLeft.x)/2;

	///
	/// Top Left
	///
	// Define Calibration Window Quadrant
	// Line between topLeft and bottomLeft
	double m = (calibWnd->topLeft.y - calibWnd->bottomLeft.y) / (calibWnd->topLeft.x - calibWnd->bottomLeft.x);
	double q = calibWnd->topLeft.y - m * calibWnd->topLeft.x;

	// Find x for y = h/2
	// y = mx + q ---> x = (y - q)/m
	double y = calibWnd->topLeft.y + h/2;
	double x = (y - q)/m;
	topLeft = calibWnd->topLeft;
	topRight = Point2f(centerX, topLeft.y);
	bottomLeft = Point2f(x, y);
	bottomRight = Point2f(centerX, y);

	CalibrationWindow *w1 = new CalibrationWindow();

	w1->topLeft = topLeft;
	w1->topRight = topRight;
	w1->bottomLeft = bottomLeft;
	w1->bottomRight = bottomRight;

	// Filter points with current sub-window
	vector<Point> filtered = w1->filterPoints(intersPts);
	retval->topLeft = findClosestPoint(filtered, calibWnd->topLeft);

	///
	/// Bottom left
	///
	topLeft = bottomLeft;
	bottomLeft = calibWnd->bottomLeft;
	topRight = topRight = Point2f(centerX, topLeft.y);
	bottomRight = Point2f(centerX, bottomLeft.y);
	w1->topLeft = topLeft;
	w1->topRight = topRight;
	w1->bottomLeft = bottomLeft;
	w1->bottomRight = bottomRight;
	renderCalibrationWindow(calibrationFrame, w1);

	// Filter points with current sub-window
	filtered = w1->filterPoints(intersPts);
	retval->bottomLeft = findClosestPoint(filtered, calibWnd->bottomLeft);

	//
	// Bottom right
	//
	m = (calibWnd->topRight.y - calibWnd->bottomRight.y) / (calibWnd->topRight.x - calibWnd->bottomRight.x);
	q = calibWnd->topRight.y - m * calibWnd->topRight.x;
	y = calibWnd->topRight.y + h/2;
	x = (y - q)/m;
	topLeft = topRight;
	bottomLeft = bottomRight;
	bottomRight = calibWnd->bottomRight;
	topRight = Point2f(x, y);
	w1->topLeft = topLeft;
	w1->topRight = topRight;
	w1->bottomLeft = bottomLeft;
	w1->bottomRight = bottomRight;
	renderCalibrationWindow(calibrationFrame, w1);

	// Filter points with current sub-window
	filtered = w1->filterPoints(intersPts);
	retval->bottomRight = findClosestPoint(filtered, calibWnd->bottomRight);

	//
	// Top right
	//
	bottomLeft = topLeft;
	topLeft = Point2f(centerX, calibWnd->topLeft.y);
	bottomRight = topRight;
	topRight = calibWnd->topRight;
	w1->topLeft = topLeft;
	w1->topRight = topRight;
	w1->bottomLeft = bottomLeft;
	w1->bottomRight = bottomRight;
	renderCalibrationWindow(calibrationFrame, w1);

	// Filter points with current sub-window
	filtered = w1->filterPoints(intersPts);
	retval->topRight = findClosestPoint(filtered, calibWnd->topRight);

	delete w1;

	return retval;
}
Esempio n. 11
0
TennisFieldDelimiter *TennisFieldCalibrator::computeConeDelimitedStaticModel(vector<Point> intersPts)
{
	printf("computeConeDelimitedStaticModel(): Computing...\n");

	TennisFieldDelimiter *retval = new TennisFieldDelimiter();
	Point2f bottomLeft, bottomRight;
	Point2f topLeft, topRight;

	double h = cones.vertex_bottomLeft.y - cones.vertex_topLeft.y;
	double centerX = cones.vertex_topLeft.x + (cones.vertex_topRight.x - cones.vertex_topLeft.x)/2;

	///
	/// Top Left
	///
	// Define Calibration Window Quadrant
	// Line between topLeft and bottomLeft
	double m = (cones.vertex_topLeft.y - cones.vertex_bottomLeft.y) / (cones.vertex_topLeft.x - cones.vertex_bottomLeft.x);
	double q = cones.vertex_topLeft.y - m * cones.vertex_topLeft.x;

	// Find x for y = h/2
	// y = mx + q ---> x = (y - q)/m
	double y = cones.vertex_topLeft.y + h/2;
	double x = (y - q)/m;

	printf("TL x, y, q, m = %g %g %g %g\n", x, y, q, m);

	topLeft = cones.vertex_topLeft;
	topRight = Point2f(centerX, topLeft.y);
	bottomLeft = Point2f(x, y);
	bottomRight = Point2f(centerX, y);

	printf("computeConeDelimitedStaticModel(): Window TL\n");

	CalibrationWindow *w1 = new CalibrationWindow();

	w1->topLeft = topLeft;
	w1->topRight = topRight;
	w1->bottomLeft = bottomLeft;
	w1->bottomRight = bottomRight;

	// Filter points with current sub-window
	vector<Point> filtered = w1->filterPoints(intersPts);
	retval->topLeft = findClosestPoint(filtered, cones.vertex_topLeft);

	///
	/// Bottom left
	///
	printf("computeConeDelimitedStaticModel(): Window BL\n");

	topLeft = bottomLeft;
	bottomLeft = cones.vertex_bottomLeft;
	topRight = topRight = Point2f(centerX, topLeft.y);
	bottomRight = Point2f(centerX, bottomLeft.y);
	w1->topLeft = topLeft;
	w1->topRight = topRight;
	w1->bottomLeft = bottomLeft;
	w1->bottomRight = bottomRight;

	// Filter points with current sub-window
	filtered = w1->filterPoints(intersPts);
	retval->bottomLeft = findClosestPoint(filtered, cones.vertex_bottomLeft);

	//
	// Bottom right
	//
	printf("computeConeDelimitedStaticModel(): Window BR\n");
	m = (cones.vertex_topRight.y - cones.vertex_bottomRight.y) / (cones.vertex_topRight.x - cones.vertex_bottomRight.x);
	q = cones.vertex_topRight.y - m * cones.vertex_topRight.x;
	y = cones.vertex_topRight.y + h/2;
	x = (y - q)/m;

	printf("BR x, y, q, m = %g %g %g %g\n", x, y, q, m);

	topLeft = topRight;
	bottomLeft = bottomRight;
	bottomRight = cones.vertex_bottomRight;
	topRight = Point2f(x, y);
	w1->topLeft = topLeft;
	w1->topRight = topRight;
	w1->bottomLeft = bottomLeft;
	w1->bottomRight = bottomRight;

	// Filter points with current sub-window
	filtered = w1->filterPoints(intersPts);
	retval->bottomRight = findClosestPoint(filtered, cones.vertex_bottomRight);

	//
	// Top right
	//
	printf("computeConeDelimitedStaticModel(): Window TR\n");
	bottomLeft = topLeft;
	topLeft = Point2f(centerX, cones.vertex_topLeft.y);
	bottomRight = topRight;
	topRight = cones.vertex_topRight;
	w1->topLeft = topLeft;
	w1->topRight = topRight;
	w1->bottomLeft = bottomLeft;
	w1->bottomRight = bottomRight;

	// Filter points with current sub-window
	filtered = w1->filterPoints(intersPts);
	retval->topRight = findClosestPoint(filtered, cones.vertex_topRight);

	delete w1;

	return retval;
}