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