Ejemplo n.º 1
0
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);
}
Ejemplo n.º 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);
}
Ejemplo n.º 4
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);
	}
}