C++ eigen::Vector3f类代码示例

size_t closest_point_index_rayOMP(const pcl::PointCloud<PointT>& cloud,
                                  const Eigen::Vector3f& direction_pre,
                                  const Eigen::Vector3f& line_pt,
                                  double& distance_to_ray) {
    Eigen::Vector3f direction = direction_pre / direction_pre.norm();
    PointT point;
    std::vector<double> distances(cloud.points.size(), 10);  // Initialize to value 10
// Generate a vector of distances
    #pragma omp parallel for
    for (size_t index = 0; index < cloud.points.size(); index++) {
        point = cloud.points[index];

        Eigen::Vector3f cloud_pt(point.x, point.y, point.z);
        Eigen::Vector3f difference = (line_pt - cloud_pt);
        // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Vector_formulation
        double distance = (difference - (difference.dot(direction) * direction)).norm();
        distances[index] = distance;
    }

    double min_distance = std::numeric_limits<double>::infinity();
    size_t min_index = 0;
    // Find index of maximum (TODO: Figure out how to OMP this)
    for (size_t index = 0; index < cloud.points.size(); index++) {
        const double distance = distances[index];
        if (distance < min_distance) {
            min_distance = distance;
            min_index = index;
        }
    }

    distance_to_ray = min_distance;

    return (min_index);
}

bool CommandSubscriber::getForcedTfFrame(Eigen::Affine3f & result) const
  {
  tf::StampedTransform transform;
  try
    {
    m_tf_listener.lookupTransform(m_forced_tf_frame_reference,m_forced_tf_frame_name,ros::Time(0),transform);
    }
    catch (tf::TransformException ex)
      {
      ROS_ERROR("kinfu: hint was forced by TF, but retrieval failed because: %s",ex.what());
      return false;
      }

  Eigen::Vector3f vec;
  vec.x() = transform.getOrigin().x();
  vec.y() = transform.getOrigin().y();
  vec.z() = transform.getOrigin().z();
  Eigen::Quaternionf quat;
  quat.x() = transform.getRotation().x();
  quat.y() = transform.getRotation().y();
  quat.z() = transform.getRotation().z();
  quat.w() = transform.getRotation().w();

  result.linear() = Eigen::AngleAxisf(quat).matrix();
  result.translation() = vec;
  return true;
  }

void NodeRendererEvent::render( const Eigen::Matrix4f& view, const Eigen::Matrix4f& model )
{
    glPushMatrix();
    glMultMatrixf( ( model * view ).array() );

    glColor3f( 1.0f, 1.0f, 0.0f );

    // render a filled elipse
    glBegin( GL_POLYGON );
    for ( float step = 0.0f; step < 2.0f * M_PI; step += 0.1f )
    {
        float x = GEOM_WIDTH * sinf( step );
        float y = GEOM_HEIGHT * cosf( step );
        glVertex3f( x, y, 0.0f );
    }
    glEnd();

    // render the border
    if ( _isHighlighted )
    {
        Eigen::Vector3f col = RenderManager::get()->getHeighlightColour();
        glColor3f( col.x(), col.y(), col.z() );
    }
    else
    {
        glColor3f( 0.3f, 0.3f, 0.0f );
    }

    glLineWidth( 8.0f );
    glBegin( GL_LINE_LOOP );
    for ( float step = 0.0f; step < 2.0f * M_PI; step += 0.05f )
    {
        float x = ( GEOM_WIDTH + 0.9f ) * sinf( step );
        float y = ( GEOM_HEIGHT + 0.9f ) * cosf( step );
        glVertex3f( x, y, 0.0f );
    }
    glEnd();

    // render the node text
    glColor3f( 0.0, 0.0, 0.0 );
    std::string text( getText() );
    if ( text.length() )
    {
        Eigen::Vector3f tmin, tmax;
        RenderManager::get()->fontGetDims( text.c_str(), tmin, tmax );

        float xpos = tmax.x() - tmin.x();
        float ypos = tmax.y() - tmin.y();
        if ( getScale().x() )
            xpos /= getScale().x();

        if ( getScale().y() )
            ypos /= getScale().y();

        Eigen::Vector2f pos( -xpos / 2.0f, -ypos / 2.0f );
        RenderManager::get()->fontRender( text.c_str(), pos );
    }

    glPopMatrix();
}

	void EXPORT_API InitShapeMatching(float* restPositions, float* invMasses, bool* posLocks, int pointsCount, float* restCMs, float* invRestMat)
	{

		Eigen::Vector3f* X_0 = new Eigen::Vector3f[pointsCount];
		float* w = new float[pointsCount];

		for (size_t i = 0; i < pointsCount; i++)
		{
			X_0[i] = Eigen::Map<Eigen::Vector3f>(restPositions + (i * 4));
			w[i] = posLocks[i] ? 0.0f : invMasses[i];
		}

		Eigen::Vector3f restCM;
		Eigen::Matrix3f A;


		PBD::PositionBasedDynamics::initShapeMatchingRestInfo(X_0, w, pointsCount, restCM, A);

		restCMs[0] = restCM.x();
		restCMs[1] = restCM.y();
		restCMs[2] = restCM.z();
		restCMs[3] = 0.0f;

		invRestMat[0] = A(0, 0); invRestMat[1] = A(0, 1); invRestMat[2] = A(0, 2);
		invRestMat[4] = A(1, 0); invRestMat[5] = A(1, 1); invRestMat[6] = A(1, 2);
		invRestMat[8] = A(2, 0); invRestMat[9] = A(2, 1); invRestMat[10]= A(2, 2);

		delete[] X_0;
		delete[] w;
		
	}

	void EXPORT_API InitTetrasShapeMatchingJB(btVector3* initialPositions,float* invMasses, bool* posLocks, Tetrahedron* tetras, btVector3* restCMs, float* invRestMat, int tetrasCount)
	{
		for (int i = 0; i < tetrasCount; i++)
		{
			Tetrahedron tetra = tetras[i];

			Eigen::Vector3f x1_0(initialPositions[tetra.idA].x(), initialPositions[tetra.idA].y(), initialPositions[tetra.idA].z());
			Eigen::Vector3f x2_0(initialPositions[tetra.idB].x(), initialPositions[tetra.idB].y(), initialPositions[tetra.idB].z());
			Eigen::Vector3f x3_0(initialPositions[tetra.idC].x(), initialPositions[tetra.idC].y(), initialPositions[tetra.idC].z());
			Eigen::Vector3f x4_0(initialPositions[tetra.idD].x(), initialPositions[tetra.idD].y(), initialPositions[tetra.idD].z());


			float w1 = posLocks[tetra.idA] ? 0.0f : invMasses[tetra.idA];
			float w2 = posLocks[tetra.idB] ? 0.0f : invMasses[tetra.idB];
			float w3 = posLocks[tetra.idC] ? 0.0f : invMasses[tetra.idC];
			float w4 = posLocks[tetra.idD] ? 0.0f : invMasses[tetra.idD];

			Eigen::Vector3f restCM;
			Eigen::Matrix3f A;


			Eigen::Vector3f x0[4] = { x1_0, x2_0, x3_0, x4_0 };
			float w[4] = { w1, w2, w3, w4 };
			Eigen::Vector3f corr[4];

			PBD::PositionBasedDynamics::initShapeMatchingRestInfo(x0, w, 4, restCM, A);

			restCMs[i] = btVector3(restCM.x(), restCM.y(), restCM.z());

			invRestMat[16 * i + 0] = A(0, 0); invRestMat[16 * i + 1] = A(0, 1); invRestMat[16 * i + 2] = A(0, 2);
			invRestMat[16 * i + 4] = A(1, 0); invRestMat[16 * i + 5] = A(1, 1); invRestMat[16 * i + 6] = A(1, 2);
			invRestMat[16 * i + 8] = A(2, 0); invRestMat[16 * i + 9] = A(2, 1); invRestMat[16 * i +10] = A(2, 2);
		}
	}

/**
 * Verifica si un plano corta a una nube en dos
 * @param  pc    nube de puntos a evaluar
 * @param  coefs Coeficientes del plano
 * @return       True si lo corta, false en caso contrario.
 */
bool isPointCloudCutByPlane(PointCloud<PointXYZ>::Ptr pc, ModelCoefficients::Ptr coefs, PointXYZ p_plane){
	/*
	Algoritmo:
		- Obtener vector normal a partir de coeficientes
		- Iterar sobre puntos de la nube
			- Calcular vector delta entre punto entregado (dentro del plano) y punto iterado
			- Calcular ángulo entre vector delta y normal
			- Angulos menores a PI/2 son de un lado, mayores son de otro
			- Si aparecen puntos de ambos lados, retornar True, else, false.
	*/
	const double PI = 3.1416;
	Eigen::Vector3f normal = Eigen::Vector3f(coefs->values[0], coefs->values[1], coefs->values[2]);
	normal.normalize();
	// cout << "Normal: " << normal << endl;
	bool side;
	// Iterar sobre los puntos
	for (int i=0; i<pc->points.size(); i++){
		// Calcular ángulo entre punto y normal
		Eigen::Vector3f delta = Eigen::Vector3f (p_plane.x, p_plane.y, p_plane.z) - Eigen::Vector3f(pc->points[i].x, pc->points[i].y, pc->points[i].z);
		delta.normalize();
		double alpha = acos(normal.dot(delta));
		// printf ("Alpha: %f°\n", (alpha*180/PI));
		if (i==0){
			side = (alpha < PI/2.0);
			// printf("Lado escogido: %s", side ? "true": "false");
			continue;
		}
		if (side != (alpha < PI/2.0)){
			// printf("Nube es cortada por plano\n");
			return true;
		}
	}
	// printf("Nube NO es cortada por plano\n");
	return false;
}

void WorldDownloadManager::findExtraCubesForBoundingBox(const Eigen::Vector3f& current_cube_min,
  const Eigen::Vector3f& current_cube_max,const Eigen::Vector3f& bbox_min,const Eigen::Vector3f& bbox_max,Vector3fVector& cubes_centers,
  bool& extract_current)
{
  const Eigen::Vector3f & cube_size = current_cube_max - current_cube_min;
  cubes_centers.clear();
  extract_current = false;

  const Eigen::Vector3f relative_act_bbox_min = bbox_min - current_cube_min;
  const Eigen::Vector3f relative_act_bbox_max = bbox_max - current_cube_min;
  const Eigen::Vector3i num_cubes_plus = Eigen::Vector3f(floor3f(Eigen::Vector3f(relative_act_bbox_max.array() / cube_size.array())
    - (Eigen::Vector3f::Ones() * 0.0001))).cast<int>();
  const Eigen::Vector3i num_cubes_minus = Eigen::Vector3f(floor3f(Eigen::Vector3f(relative_act_bbox_min.array() / cube_size.array())
    + (Eigen::Vector3f::Ones() * 0.0001))).cast<int>();
  for (int z = num_cubes_minus.z(); z <= num_cubes_plus.z(); z++)
    for (int y = num_cubes_minus.y(); y <= num_cubes_plus.y(); y++)
      for (int x = num_cubes_minus.x(); x <= num_cubes_plus.x(); x++)
      {
        const Eigen::Vector3i cube_index(x,y,z);
        if ((cube_index.array() == Eigen::Vector3i::Zero().array()).all())
        {
          extract_current = true;
          continue;
        }

        const Eigen::Vector3f relative_cube_origin = cube_index.cast<float>().array() * cube_size.array();
        const Eigen::Vector3f cube_center = relative_cube_origin + current_cube_min + (cube_size * 0.5);
        cubes_centers.push_back(cube_center);
      }
}

void SensorFusion::reset(const Eigen::Vector3f& accel, const Eigen::Vector3f& magnetom)
{
	G_Dt = 0;

	Eigen::Vector3f temp1;
	Eigen::Vector3f temp2;
	Eigen::Vector3f xAxis;
	xAxis << 1.0f, 0.0f, 0.0f;

	timestamp = highResClock.now();

	// GET PITCH
	// Using y-z-plane-component/x-component of gravity vector
	pitch = -atan2f(accel(0), sqrtf(accel(1) * accel(1) + accel(2) * accel(2)));

	// GET ROLL
	// Compensate pitch of gravity vector
	temp1 = accel.cross(xAxis);
	temp2 = xAxis.cross(temp1);

	// Normally using x-z-plane-component/y-component of compensated gravity vector
	// roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
	// Since we compensated for pitch, x-z-plane-component equals z-component:
	roll = atan2f(temp2(1), temp2(2));

	// GET YAW
	compassHeading(magnetom);
	yaw = magHeading;

	// Init rotation matrix
	init_rotation_matrix(dcmMatrix, yaw, pitch, roll);
}

void adjustNormalsToViewPoints(
		const pcl::PointCloud<pcl::PointXYZ>::Ptr & viewpoints,
		pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud)
{
	if(viewpoints->size() && cloud.size())
	{
		pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
		tree->setInputCloud (viewpoints);

		for(unsigned int i=0; i<cloud.size(); ++i)
		{
			std::vector<int> indices;
			std::vector<float> dist;
			tree->nearestKSearch(pcl::PointXYZ(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z), 1, indices, dist);
			UASSERT(indices.size() == 1);

			Eigen::Vector3f v = viewpoints->at(indices[0]).getVector3fMap() - cloud.points[i].getVector3fMap();
			Eigen::Vector3f n(cloud.points[i].normal_x, cloud.points[i].normal_y, cloud.points[i].normal_z);

			float result = v.dot(n);
			if(result < 0)
			{
				//reverse normal
				cloud.points[i].normal_x *= -1.0f;
				cloud.points[i].normal_y *= -1.0f;
				cloud.points[i].normal_z *= -1.0f;
			}
		}
	}
}

float robotDepth()
{
	// Two interesting points
	float alpha1 = (x + (sx-height)/2)/(float)height*2*tan(VFOV/2.);
	float alpha2 = (x - (sx+height)/2)/(float)height*2*tan(VFOV/2.);
	float beta = (y - width/2)/(float)width*2*tan(HFOV/2.);

	// Vectors
	Eigen::Vector3f v1(1,-beta,-alpha1);
	Eigen::Vector3f v2(1,-beta,-alpha2);

	// Normalization
	v1 = v1/v1.norm();
	v2 = v2/v2.norm();

	// Center
	Eigen::Vector3f c = (v1+v2)/2.;
	float c_norm = c.norm();

	// Projection
	Eigen::MatrixXf proj_mat = c.transpose()*v1;
	float proj = proj_mat(0,0);

	// Orthogonal part in v1
	Eigen::Vector3f orth = v1 - proj/c_norm*c;

	// Norm
	float orth_norm = orth.norm();

	// Approximate depth
	float d = H/2.*proj/orth_norm;
	return d;
}

png::image<png::rgb_pixel_16> bumpier::model::calculate_normals(const png::image<png::gray_pixel_16>& input, double phi, space_type space) const
{
    int width = input.get_width(), height = input.get_height();
    png::image<png::rgb_pixel_16> output(width, height);

    for (int y = 0; y < height; y++)
        for (int x = 0; x < width; x++)
        {
            int xmin = (x + width  - 1) % width,
                ymin = (y + height - 1) % height,
                xmax = (x + 1) % width,
                ymax = (y + 1) % height;

            Eigen::Vector3f normal = (position(input, x, ymax) - position(input, x, ymin)).cross(
                                      position(input, xmax, y) - position(input, xmin, y));

            if(space == tangent_space)
				normal = tangentspace(Eigen::Vector2f((float)x / width, (float)y / height)) * normal;

			normal.normalize();

            normal = (normal.array() * 0.5 + 0.5) * 0xFFFF;
            output.set_pixel(x, y, png::rgb_pixel_16(normal[0], normal[1], normal[2]));
        }

    return output;
}

void m2tq(Eigen::Vector3f& t, Eigen::Quaternionf& q, const Eigen::Matrix4f& m){
  t.x()=m(0,3);
  t.y()=m(1,3);
  t.z()=m(2,3);
  Eigen::Matrix3f R=m.block<3,3>(0,0);
  q=Eigen::Quaternionf(R);
}

template <typename PointNT> bool
pcl::GridProjection<PointNT>::isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
                                             std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
                                             std::vector <int> &pt_union_indices)
{
  assert (end_pts.size () == 2);
  assert (vect_at_end_pts.size () == 2);

  double length[2];
  for (size_t i = 0; i < 2; ++i)
  {
    length[i] = vect_at_end_pts[i].norm ();
    vect_at_end_pts[i].normalize ();
  }
  double dot_prod = vect_at_end_pts[0].dot (vect_at_end_pts[1]);
  if (dot_prod < 0)
  {
    double ratio = length[0] / (length[0] + length[1]);
    Eigen::Vector4f start_pt = end_pts[0] + (end_pts[1] - end_pts[0]) * ratio;
    Eigen::Vector4f intersection_pt = Eigen::Vector4f::Zero ();
    findIntersection (0, end_pts, vect_at_end_pts, start_pt, pt_union_indices, intersection_pt);

    Eigen::Vector3f vec;
    getVectorAtPoint (intersection_pt, pt_union_indices, vec);
    vec.normalize ();

    double d2 = getD2AtPoint (intersection_pt, vec, pt_union_indices);
    if (d2 < 0)
      return (true);
  }
  return (false);
}

template <typename PointNT> void
pcl::GridProjection<PointNT>::getProjectionWithPlaneFit (const Eigen::Vector4f &p,
                                                         std::vector<int> (&pt_union_indices),
                                                         Eigen::Vector4f &projection)
{
  // Compute the plane coefficients
  Eigen::Vector4f model_coefficients;
  /// @remark iterative weighted least squares or sac might give better results
  Eigen::Matrix3f covariance_matrix;
  Eigen::Vector4f xyz_centroid;

  computeMeanAndCovarianceMatrix (*data_, pt_union_indices, covariance_matrix, xyz_centroid);

  // Get the plane normal
  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value = -1;
  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);

  // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
  model_coefficients[0] = eigen_vector [0];
  model_coefficients[1] = eigen_vector [1];
  model_coefficients[2] = eigen_vector [2];
  model_coefficients[3] = 0;
  // Hessian form (D = nc . p_plane (centroid here) + p)
  model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);

  // Projected point
  Eigen::Vector3f point (p.x (), p.y (), p.z ());     //= Eigen::Vector3f::MapAligned (&output.points[cp].x, 3);
  float distance = point.dot (model_coefficients.head <3> ()) + model_coefficients[3];
  point -= distance * model_coefficients.head < 3 > ();

  projection = Eigen::Vector4f (point[0], point[1], point[2], 0);
}

template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::selectBaseTriangle (std::vector <int> &base_indices)
{
  int nr_points = static_cast <int> (target_indices_->size ());
  float best_t = 0.f;

  // choose random first point
  base_indices[0] = (*target_indices_)[rand () % nr_points];
  int *index1 = &base_indices[0];

  // random search for 2 other points (as far away as overlap allows)
  for (int i = 0; i < ransac_iterations_; i++)
  {
    int *index2 = &(*target_indices_)[rand () % nr_points];
    int *index3 = &(*target_indices_)[rand () % nr_points];

    Eigen::Vector3f u = target_->points[*index2].getVector3fMap () - target_->points[*index1].getVector3fMap ();
    Eigen::Vector3f v = target_->points[*index3].getVector3fMap () - target_->points[*index1].getVector3fMap ();
    float t = u.cross (v).squaredNorm (); // triangle area (0.5 * sqrt(t)) should be maximal

    // check for most suitable point triple
    if (t > best_t && u.squaredNorm () < max_base_diameter_sqr_ && v.squaredNorm () < max_base_diameter_sqr_)
    {
      best_t = t;
      base_indices[1] = *index2;
      base_indices[2] = *index3;
    }
  }

  // return if a triplet could be selected
  return (best_t == 0.f ? -1 : 0);
}