void randomUnitQuaternion(Eigen::Vector4d &quat) {
static boost::random::mt19937 rng(time(NULL));
static boost::random::normal_distribution<> normal;
do {
quat(0) = normal(rng);
quat(1) = normal(rng);
quat(2) = normal(rng);
quat(3) = normal(rng);
} while (quat.norm() < 0.00001);
quat.normalize();
}
mathtools::geometry::euclidian::Line<4> skeleton::model::Perspective::toObj(
const Eigen::Matrix<double,skeleton::model::meta<skeleton::model::Projective>::stordim,1> &vec,
const mathtools::geometry::euclidian::Line<4> &) const
{
Eigen::Vector4d origin;
origin.block<3,1>(0,0) = m_frame3->getOrigin();
origin(3) = 0.0;
Eigen::Vector4d vecdir;
vecdir.block<3,1>(0,0) = m_frame3->getBasis()->getMatrix()*Eigen::Vector3d(vec(0),vec(1),1.0);
vecdir(3) = vec(2);
vecdir.normalize();
return mathtools::geometry::euclidian::Line<4>(origin,vecdir);
}
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);
}
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);
}
}