Example #1
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);
}
Example #2
0
    inline Eigen::Affine3f tf_from_plane_model(float a, float b, float c, float d) {
        Eigen::Vector3f new_normal(a, b, c);
        Eigen::Vector3f old_normal(0, 0, 1);

        Eigen::Vector3f v = old_normal.cross(new_normal);
        float s2 = v.squaredNorm();
        float cc = old_normal.dot(new_normal);
        Eigen::Matrix3f v_cross;
        v_cross << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;

        Eigen::Matrix3f rot = Eigen::Matrix3f::Identity() + v_cross + v_cross * v_cross * (1 - cc) / s2;

        Eigen::Affine3f arot(rot);

        // Create a transform where the rotation component is given by the rotation axis as the normal vector (a, b, c)
        // and some arbitrary angle about that axis and the translation component is -d in the z direction after
        // that rotation (not the original z direction, which is how transforms are usually defined).
        auto result = Eigen::Translation3f(0, 0, d) * arot.inverse();

        return result;
    }
Example #3
0
float mrpt::pbmap::dist3D_Segment_to_Segment2(Segment S1, Segment S2)
{
	Eigen::Vector3f u = diffPoints(S1.P1, S1.P0);
	Eigen::Vector3f v = diffPoints(S2.P1, S2.P0);
	Eigen::Vector3f w = diffPoints(S1.P0, S2.P0);
	float a = u.dot(u);  // always >= 0
	float b = u.dot(v);
	float c = v.dot(v);  // always >= 0
	float d = u.dot(w);
	float e = v.dot(w);
	float D = a * c - b * b;  // always >= 0
	float sc, sN, sD = D;  // sc = sN / sD, default sD = D >= 0
	float tc, tN, tD = D;  // tc = tN / tD, default tD = D >= 0

	// compute the line parameters of the two closest points
	if (D < SMALL_NUM)
	{  // the lines are almost parallel
		sN = 0.0;  // force using point P0 on segment S1
		sD = 1.0;  // to prevent possible division by 0.0 later
		tN = e;
		tD = c;
	}
	else
	{  // get the closest points on the infinite lines
		sN = (b * e - c * d);
		tN = (a * e - b * d);
		if (sN < 0.0)
		{  // sc < 0 => the s=0 edge is visible
			sN = 0.0;
			tN = e;
			tD = c;
		}
		else if (sN > sD)
		{  // sc > 1 => the s=1 edge is visible
			sN = sD;
			tN = e + b;
			tD = c;
		}
	}

	if (tN < 0.0)
	{  // tc < 0 => the t=0 edge is visible
		tN = 0.0;
		// recompute sc for this edge
		if (-d < 0.0)
			sN = 0.0;
		else if (-d > a)
			sN = sD;
		else
		{
			sN = -d;
			sD = a;
		}
	}
	else if (tN > tD)
	{  // tc > 1 => the t=1 edge is visible
		tN = tD;
		// recompute sc for this edge
		if ((-d + b) < 0.0)
			sN = 0;
		else if ((-d + b) > a)
			sN = sD;
		else
		{
			sN = (-d + b);
			sD = a;
		}
	}
	// finally do the division to get sc and tc
	sc = (fabs(sN) < SMALL_NUM ? 0.0 : sN / sD);
	tc = (fabs(tN) < SMALL_NUM ? 0.0 : tN / tD);

	// get the difference of the two closest points
	Eigen::Vector3f dP = w + (sc * u) - (tc * v);  // = S1(sc) - S2(tc)

	return dP.squaredNorm();  // return the closest distance
}
void Streamer::performPlayerUpdate(Player &player, bool automatic)
{
	Eigen::Vector3f delta = Eigen::Vector3f::Zero(), position = player.position;
	int state = GetPlayerState(player.playerID);
	bool update = true;
	if (automatic)
	{
		player.interiorID = GetPlayerInterior(player.playerID);
		player.worldID = GetPlayerVirtualWorld(player.playerID);
		GetPlayerPos(player.playerID, &player.position[0], &player.position[1], &player.position[2]);
		if (state != PLAYER_STATE_NONE && state != PLAYER_STATE_WASTED)
		{
			if (player.position != position)
			{
				position = player.position;
				Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
				if (state == PLAYER_STATE_ONFOOT)
				{
					GetPlayerVelocity(player.playerID, &velocity[0], &velocity[1], &velocity[2]);
				}
				else if (state == PLAYER_STATE_DRIVER || state == PLAYER_STATE_PASSENGER)
				{
					GetVehicleVelocity(GetPlayerVehicleID(player.playerID), &velocity[0], &velocity[1], &velocity[2]);
				}
				float velocityNorm = velocity.squaredNorm();
				if (velocityNorm >= velocityBoundaries.get<0>() && velocityNorm <= velocityBoundaries.get<1>())
				{
					delta = velocity * averageUpdateTime;
					player.position += delta;
				}
			}
			else
			{
				update = player.updateWhenIdle;
			}
		}
		else
		{
			update = false;
		}
	}
	std::vector<SharedCell> cells;
	if (update)
	{
		core->getGrid()->findAllCells(player, cells);
		if (!cells.empty())
		{
			if (!core->getData()->objects.empty() && player.enabledItems[STREAMER_TYPE_OBJECT] && !IsPlayerNPC(player.playerID))
			{
				processObjects(player, cells);
			}
			if (!core->getData()->checkpoints.empty() && player.enabledItems[STREAMER_TYPE_CP])
			{
				processCheckpoints(player, cells);
			}
			if (!core->getData()->raceCheckpoints.empty() && player.enabledItems[STREAMER_TYPE_RACE_CP])
			{
				processRaceCheckpoints(player, cells);
			}
			if (!core->getData()->mapIcons.empty() && player.enabledItems[STREAMER_TYPE_MAP_ICON] && !IsPlayerNPC(player.playerID))
			{
				processMapIcons(player, cells);
			}
			if (!core->getData()->textLabels.empty() && player.enabledItems[STREAMER_TYPE_3D_TEXT_LABEL] && !IsPlayerNPC(player.playerID))
			{
				processTextLabels(player, cells);
			}
			if (!core->getData()->areas.empty() && player.enabledItems[STREAMER_TYPE_AREA])
			{
				if (!delta.isZero())
				{
					player.position = position;
				}
				processAreas(player, cells);
				if (!delta.isZero())
				{
					player.position += delta;
				}
			}
		}
	}
	if (automatic)
	{
		if (!core->getData()->pickups.empty())
		{
			if (!update)
			{
				core->getGrid()->findMinimalCells(player, cells);
			}
			processPickups(player, cells);
		}
		if (!delta.isZero())
		{
			player.position = position;
		}
		executeCallbacks();
	}
}
Example #5
0
template <typename PointT> void
pcl16::approximatePolygon2D (const typename pcl16::PointCloud<PointT>::VectorType &polygon, 
                           typename pcl16::PointCloud<PointT>::VectorType &approx_polygon, 
                           float threshold, bool refine, bool closed)
{
  approx_polygon.clear ();
  if (polygon.size () < 3)
    return;
  
  std::vector<std::pair<unsigned, unsigned> > intervals;
  std::pair<unsigned,unsigned> interval (0, 0);
  
  if (closed)
  {
    float max_distance = .0f;
    for (unsigned idx = 1; idx < polygon.size (); ++idx)
    {
      float distance = (polygon [0].x - polygon [idx].x) * (polygon [0].x - polygon [idx].x) + 
                       (polygon [0].y - polygon [idx].y) * (polygon [0].y - polygon [idx].y);

      if (distance > max_distance)
      {
        max_distance = distance;
        interval.second = idx;
      }
    }

    for (unsigned idx = 1; idx < polygon.size (); ++idx)
    {
      float distance = (polygon [interval.second].x - polygon [idx].x) * (polygon [interval.second].x - polygon [idx].x) + 
                       (polygon [interval.second].y - polygon [idx].y) * (polygon [interval.second].y - polygon [idx].y);

      if (distance > max_distance)
      {
        max_distance = distance;
        interval.first = idx;
      }
    }

    if (max_distance < threshold * threshold)
      return;

    intervals.push_back (interval);
    std::swap (interval.first, interval.second);
    intervals.push_back (interval);
  }
  else
  {
    interval.first = 0;
    interval.second = static_cast<unsigned int> (polygon.size ()) - 1;
    intervals.push_back (interval);
  }
  
  std::vector<unsigned> result;
  // recursively refine
  while (!intervals.empty ())
  {
    std::pair<unsigned, unsigned>& currentInterval = intervals.back ();
    float line_x = polygon [currentInterval.first].y - polygon [currentInterval.second].y;
    float line_y = polygon [currentInterval.second].x - polygon [currentInterval.first].x;
    float line_d = polygon [currentInterval.first].x * polygon [currentInterval.second].y - polygon [currentInterval.first].y * polygon [currentInterval.second].x;
    
    float linelen = 1.0f / sqrt (line_x * line_x + line_y * line_y);
    
    line_x *= linelen;
    line_y *= linelen;
    line_d *= linelen;
    
    float max_distance = 0.0;
    unsigned first_index = currentInterval.first + 1;
    unsigned max_index = 0;

    // => 0-crossing
    if (currentInterval.first > currentInterval.second)
    {
      for (unsigned idx = first_index; idx < polygon.size(); idx++)
      {
        float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
        if (distance > max_distance)
        {
          max_distance = distance;
          max_index  = idx;
        }
      }
      first_index = 0;
    }

    for (unsigned int idx = first_index; idx < currentInterval.second; idx++)
    {
      float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
      if (distance > max_distance)
      {
        max_distance = distance;
        max_index  = idx;
      }
    }

    if (max_distance > threshold)
    {
      std::pair<unsigned, unsigned> interval (max_index, currentInterval.second);
      currentInterval.second = max_index;
      intervals.push_back (interval);
    }
    else
    {
      result.push_back (currentInterval.second);
      intervals.pop_back ();
    }
  }
  
  approx_polygon.reserve (result.size ());
  if (refine)
  {
    std::vector<Eigen::Vector3f> lines (result.size ());
    std::reverse (result.begin (), result.end ());
    for (unsigned rIdx = 0; rIdx < result.size (); ++rIdx)
    {
      unsigned nIdx = rIdx + 1;
      if (nIdx == result.size ())
        nIdx = 0;
      
      Eigen::Vector2f centroid = Eigen::Vector2f::Zero ();
      Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero ();
      unsigned pIdx = result[rIdx];
      unsigned num_points = 0;
      if (pIdx > result[nIdx])
      {
        num_points = static_cast<unsigned> (polygon.size ()) - pIdx;
        for (; pIdx < polygon.size (); ++pIdx)
        {
          covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
          covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
          covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
          centroid [0] += polygon [pIdx].x;
          centroid [1] += polygon [pIdx].y;
        }
        pIdx = 0;
      }
      
      num_points += result[nIdx] - pIdx;
      for (; pIdx < result[nIdx]; ++pIdx)
      {
        covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
        covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
        covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
        centroid [0] += polygon [pIdx].x;
        centroid [1] += polygon [pIdx].y;
      }
      
      covariance.coeffRef (2) = covariance.coeff (1);
      
      float norm = 1.0f / float (num_points);
      centroid *= norm;
      covariance *= norm;
      covariance.coeffRef (0) -= centroid [0] * centroid [0];
      covariance.coeffRef (1) -= centroid [0] * centroid [1];
      covariance.coeffRef (3) -= centroid [1] * centroid [1];
      
      float eval;
      Eigen::Vector2f normal;
      eigen22 (covariance, eval, normal);

      // select the one which is more "parallel" to the original line
      Eigen::Vector2f direction;
      direction [0] = polygon[result[nIdx]].x - polygon[result[rIdx]].x;
      direction [1] = polygon[result[nIdx]].y - polygon[result[rIdx]].y;
      direction.normalize ();
      
      if (fabs (direction.dot (normal)) > float(M_SQRT1_2))
      {
        std::swap (normal [0], normal [1]);
        normal [0] = -normal [0];
      }
      
      // needs to be on the left side of the edge
      if (direction [0] * normal [1] < direction [1] * normal [0])
        normal *= -1.0;
      
      lines [rIdx].head<2> () = normal;
      lines [rIdx] [2] = -normal.dot (centroid);
    }
    
    float threshold2 = threshold * threshold;
    for (unsigned rIdx = 0; rIdx < lines.size (); ++rIdx)
    {
      unsigned nIdx = rIdx + 1;
      if (nIdx == result.size ())
        nIdx = 0;      
      
      Eigen::Vector3f vertex = lines [rIdx].cross (lines [nIdx]);
      vertex /= vertex [2];
      vertex [2] = 0.0;

      PointT point;      
      // test whether we need another edge since the intersection point is too far away from the original vertex
      Eigen::Vector3f pq = polygon [result[nIdx]].getVector3fMap () - vertex;
      pq [2] = 0.0;
      
      float distance = pq.squaredNorm ();
      if (distance > threshold2)
      {
        // test whether the old point is inside the new polygon or outside
        if ((pq [0] * lines [rIdx] [0] + pq [1] * lines [rIdx] [1] < 0.0) &&
            (pq [0] * lines [nIdx] [0] + pq [1] * lines [nIdx] [1] < 0.0) )
        {
          float distance1 = lines [rIdx] [0] * polygon[result[nIdx]].x + lines [rIdx] [1] * polygon[result[nIdx]].y + lines [rIdx] [2];
          float distance2 = lines [nIdx] [0] * polygon[result[nIdx]].x + lines [nIdx] [1] * polygon[result[nIdx]].y + lines [nIdx] [2];

          point.x = polygon[result[nIdx]].x - distance1 * lines [rIdx] [0];
          point.y = polygon[result[nIdx]].y - distance1 * lines [rIdx] [1];

          approx_polygon.push_back (point);

          vertex [0] = polygon[result[nIdx]].x - distance2 * lines [nIdx] [0];
          vertex [1] = polygon[result[nIdx]].y - distance2 * lines [nIdx] [1];
        }
      }
      point.getVector3fMap () = vertex;
      approx_polygon.push_back (point);
    }
  }
  else
  {
    // we have a new polygon in results, but inverted (clockwise <-> counter-clockwise)    
    for (std::vector<unsigned>::reverse_iterator it = result.rbegin (); it != result.rend (); ++it)
      approx_polygon.push_back (polygon [*it]);
  }
}