/*!
* \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);
}
