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

boost::optional<Intersection> Triangle::intersect(Ray ray) {
	Eigen::Vector3f s1 = ray.dir.cross(d2);
	const float div = s1.dot(d1);
	if(div <= 0) {  // parallel or back
		return boost::optional<Intersection>();
	}

	const float div_inv = 1 / div;
		
	Eigen::Vector3f s  = ray.org - p0;
	const float a = s.dot(s1) * div_inv;
	if(a < 0 || a > 1) {
		return boost::optional<Intersection>();
	}
				
	Eigen::Vector3f s2 = s.cross(d1);
	const float b = ray.dir.dot(s2) * div_inv;
		
	if(b < 0 || a + b > 1) {
		return boost::optional<Intersection>();
	}

	const float t = d2.dot(s2) * div_inv;
	if(t < 0) {
		return boost::optional<Intersection>();
	}

	return boost::optional<Intersection>(Intersection(
		t,
		ray.at(t),
		normal,
		(1 - a - b) * uv0 + a * uv1 + b * uv2,
		(1 - a - b) * ir0 + a * ir1 + b * ir2,
		attribute));
}

// This algorithm comes from Sebastian O.H. Madgwick's 2010 paper:
// "An efficient orientation filter for inertial and inertial/magnetic sensor arrays"
// https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf
static void _psmove_orientation_fusion_imu_update(
	PSMoveOrientation *orientation_state,
	float delta_t,
	const Eigen::Vector3f &current_omega,
	const Eigen::Vector3f &current_g)
{
	// Current orientation from earth frame to sensor frame
	Eigen::Quaternionf SEq = orientation_state->quaternion;
	Eigen::Quaternionf SEq_new = SEq;

	// Compute the quaternion derivative measured by gyroscopes
	// Eqn 12) q_dot = 0.5*q*omega
	Eigen::Quaternionf omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z());
	Eigen::Quaternionf SEqDot_omega = Eigen::Quaternionf(SEq.coeffs() * 0.5f) *omega;

	if (current_g.isApprox(Eigen::Vector3f::Zero(), k_normal_epsilon))
	{
		// Get the direction of the gravitational fields in the identity pose		
		PSMove_3AxisVector identity_g= psmove_orientation_get_gravity_calibration_direction(orientation_state);
		Eigen::Vector3f k_identity_g_direction = Eigen::Vector3f(identity_g.x, identity_g.y, identity_g.z);

		// Eqn 15) Applied to the gravity vector
		// Fill in the 3x1 objective function matrix f(SEq, Sa) =|f_g|
		Eigen::Matrix<float, 3, 1> f_g;
		psmove_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL);

		// Eqn 21) Applied to the gravity vector
		// Fill in the 4x3 objective function Jacobian matrix: J_gb(SEq)= [J_g]
		Eigen::Matrix<float, 4, 3> J_g;
		psmove_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g);

		// Eqn 34) gradient_F= J_g(SEq)*f(SEq, Sa)
		// Compute the gradient of the objective function
		Eigen::Matrix<float, 4, 1> gradient_f = J_g * f_g;
		Eigen::Quaternionf SEqHatDot =
			Eigen::Quaternionf(gradient_f(0, 0), gradient_f(1, 0), gradient_f(2, 0), gradient_f(3, 0));

		// normalize the gradient
		psmove_quaternion_normalize_with_default(SEqHatDot, *k_psmove_quaternion_zero);

		// Compute the estimated quaternion rate of change
		// Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot
		Eigen::Quaternionf SEqDot_est = Eigen::Quaternionf(SEqDot_omega.coeffs() - SEqHatDot.coeffs()*beta);

		// Compute then integrate the estimated quaternion rate
		// Eqn 42) SEq_new = SEq + SEqDot_est*delta_t
		SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_est.coeffs()*delta_t);
	}
	else
	{
		SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_omega.coeffs()*delta_t);
	}

	// Make sure the net quaternion is a pure rotation quaternion
	SEq_new.normalize();

	// Save the new quaternion back into the orientation state
	orientation_state->quaternion = SEq_new;
}

void
pd_solver_advance(struct PdSolver *solver, uint32_t const n_iterations)
{
        /* set external force */
        Eigen::VectorXf ext_accel = Eigen::VectorXf::Zero(solver->positions.size());
#pragma omp parallel for
        for (uint32_t i = 0; i < solver->positions.size()/3; ++i)
                for (int j = 0; j < 3; ++j)
                        ext_accel[3*i + j] = solver->ext_force[j];
        Eigen::VectorXf const ext_force = solver->mass_mat*ext_accel;
        Eigen::VectorXf const mass_y = solver->mass_mat*(2.0f*solver->positions - solver->positions_last);

        solver->positions_last = solver->positions;

        for (uint32_t iter = 0; iter < n_iterations; ++iter) {
                /* LOCAL STEP */
                /* LOCAL STEP (we account everything except the global solve */
                struct timespec local_start;
                clock_gettime(CLOCK_MONOTONIC, &local_start);
                uint32_t const n_constraints = solver->n_attachments + solver->n_springs;
                Eigen::VectorXf d(3*n_constraints);
                for (uint32_t i = 0; i < solver->n_attachments; ++i) {
                        struct PdConstraintAttachment c = solver->attachments[i];
                        d.block<3, 1>(3*i, 0) = Eigen::Map<Eigen::Vector3f>(c.position);
                }

                uint32_t const offset = solver->n_attachments;
                for (uint32_t i = 0; i < solver->n_springs; ++i) {
                        struct PdConstraintSpring c = solver->springs[i];
                        Eigen::Vector3f const v = solver->positions.block<3, 1>(3*c.i[1], 0)
                                - solver->positions.block<3, 1>(3*c.i[0], 0);
                        d.block<3, 1>(3*(offset + i), 0) = v.normalized()*c.rest_length;
                }

                Eigen::VectorXf const b = mass_y + solver->t2*(solver->j_mat*d + ext_force);
                struct timespec local_end;
                clock_gettime(CLOCK_MONOTONIC, &local_end);

                /* GLOBAL STEP */
                struct timespec global_start;
                clock_gettime(CLOCK_MONOTONIC, &global_start);
                solve(solver, b);
                struct timespec global_end;
                clock_gettime(CLOCK_MONOTONIC, &global_end);

                solver->global_time = pd_time_diff_ms(&global_start, &global_end);
                solver->local_time = pd_time_diff_ms(&local_start, &local_end);

                solver->global_cma = (solver->global_time + solver->n_iters*solver->global_cma)/(solver->n_iters + 1);
                solver->local_cma = (solver->local_time + solver->n_iters*solver->local_cma)/(solver->n_iters + 1);

                if (!(solver->n_iters % 1000)) {
                        printf("Local CMA: %f ms\n", solver->local_cma);
                        printf("Global CMA: %f ms\n\n", solver->global_cma);
                }

                ++solver->n_iters;
        }
}

template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> int
pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getApproxIntersectedVoxelCentersBySegment (
    const Eigen::Vector3f& origin,
    const Eigen::Vector3f& end,
    AlignedPointTVector &voxel_center_list,
    float precision)
{
  Eigen::Vector3f direction = end - origin;
  float norm = direction.norm ();
  direction.normalize ();

  const float step_size = static_cast<const float> (resolution_) * precision;
  // Ensure we get at least one step for the first voxel.
  const int nsteps = std::max (1, static_cast<int> (norm / step_size));

  OctreeKey prev_key;

  bool bkeyDefined = false;

  // Walk along the line segment with small steps.
  for (int i = 0; i < nsteps; ++i)
  {
    Eigen::Vector3f p = origin + (direction * step_size * static_cast<const float> (i));

    PointT octree_p;
    octree_p.x = p.x ();
    octree_p.y = p.y ();
    octree_p.z = p.z ();

    OctreeKey key;
    this->genOctreeKeyforPoint (octree_p, key);

    // Not a new key, still the same voxel.
    if ((key == prev_key) && (bkeyDefined) )
      continue;

    prev_key = key;
    bkeyDefined = true;

    PointT center;
    genLeafNodeCenterFromOctreeKey (key, center);
    voxel_center_list.push_back (center);
  }

  OctreeKey end_key;
  PointT end_p;
  end_p.x = end.x ();
  end_p.y = end.y ();
  end_p.z = end.z ();
  this->genOctreeKeyforPoint (end_p, end_key);
  if (!(end_key == prev_key))
  {
    PointT center;
    genLeafNodeCenterFromOctreeKey (end_key, center);
    voxel_center_list.push_back (center);
  }

  return (static_cast<int> (voxel_center_list.size ()));
}

void Camera::setRotate3D(const Eigen::Vector3f& rot) {
    Eigen::AngleAxis<float> aaZ(rot.z(), Eigen::Vector3f::UnitZ());
    Eigen::AngleAxis<float> aaY(rot.y(), Eigen::Vector3f::UnitY());
    Eigen::AngleAxis<float> aaX(rot.x(), Eigen::Vector3f::UnitX());

    rotation_future_ = aaZ * aaY * aaX;
    emit modified();
}