C++ PointVector类代码示例

PointVector Track::getTrackTriangleStripPoints() const
{
	PointVector vStripPoints;

	if(m_vPathPoints.size() < 2)
	{
		return vStripPoints;
	}

	// First 2 points
	NMVector2f v = getNormalizedPerpendicularVector(m_vPathPoints[1] - m_vPathPoints[0]) * m_fTrackWidth;
	vStripPoints.push_back(m_vPathPoints[0] + v);
	vStripPoints.push_back(m_vPathPoints[0] - v);

	// All points in the middle
	for(NLib::NSize_t i = 1; i < m_vPathPoints.size() - 1; ++i)
	{
		NMVector2f v1 = m_vPathPoints[i] - m_vPathPoints[i - 1];
		NMVector2f v2 = m_vPathPoints[i + 1] - m_vPathPoints[i];
		NMVector2f n = getNormalizedPerpendicularVector(v1 + v2) * m_fTrackWidth;
		
		vStripPoints.push_back(m_vPathPoints[i] + n);
		vStripPoints.push_back(m_vPathPoints[i] - n);
	}

	//  Last 2 points
	NLib::NSize_t uLastIndex = m_vPathPoints.size() - 1;
	v = getNormalizedPerpendicularVector(m_vPathPoints[uLastIndex] - m_vPathPoints[uLastIndex - 1]) * m_fTrackWidth;
	vStripPoints.push_back(m_vPathPoints[uLastIndex] + v);
	vStripPoints.push_back(m_vPathPoints[uLastIndex] - v);

	return vStripPoints;
}

  void SphericalPointProjector::unProject(PointVector &points, 
					  IntImage &indexImage,
					  const DepthImage &depthImage) const {
    if(_imageRows == 0 ||  _imageCols == 0) { 
      throw "SphericalPointProjector: Depth image has zero dimensions";
    }
    points.resize(depthImage.rows * depthImage.cols);
    int count = 0;
    indexImage.create(depthImage.rows, depthImage.cols);
    Point *point = &points[0];
    for(int r = 0; r < depthImage.rows; r++) {
      const float *f = &depthImage(r, 0);
      int *i = &indexImage(r, 0);
      for(int c = 0; c < depthImage.cols; c++, f++, i++) {
	if(!_unProject(*point, c, r, *f)) {
	  *i = -1;
	  continue;
	}
	point++;
	*i = count;
	count++;
      }
    }
    points.resize(count);
  }

PointPairVector Space::pairsInZone(const PointVector& zone, unsigned int d)
{
    PointPairVector result;
    PointVector::const_iterator iter = zone.begin();
    PointVector::const_iterator endIter = zone.end();
    for(;iter!=endIter;++iter)
    {
        for(unsigned int i = 1;(iter+i)!=endIter;++i)
        {
            // optimization
            if(iter->distanceYTo(*(iter+i))>d) // if distance by Y is higher than d
            {
                if(iter->y < (iter+i)->y) // if current point is before second one
                    break; // there won't be more interesting points in d-neighborhood
                else
                    continue; // we could not reach interesting points yet
            }
            if(iter->distanceTo(*(iter+i))<=d) // if distance is lower than d
            {
                result.insert(PointPair(*iter,*(iter+i))); // add pair of d-neighbors
            }
        }
    }
    return result;
}

    void CylindricalPointProjector::unProject(PointVector &points, 
					      Gaussian3fVector &gaussians,
					      IntImage &indexImage,
					      const DepthImage &depthImage) const {
      assert(depthImage.rows > 0 && depthImage.cols > 0 && "CylindricalPointProjector: Depth image has zero dimensions");
      points.resize(depthImage.rows * depthImage.cols);
      gaussians.resize(depthImage.rows * depthImage.cols);
      indexImage.create(depthImage.rows, depthImage.cols);
      int count = 0;
      Point *point = &points[0];
      Gaussian3f *gaussian = &gaussians[0];
      for(int r = 0; r < depthImage.rows; r++) {
	const float *f = &depthImage(r, 0);
	int *i = &indexImage(r, 0);
	for(int c = 0; c < depthImage.cols; c++, f++, i++) {      
	  if(!_unProject(*point, c, r, *f)) {
	    *i = -1;
	    continue;
	  }
	  Eigen::Matrix3f cov = Eigen::Matrix3f::Identity();
	  *gaussian = Gaussian3f(point->head<3>(), cov);
	  gaussian++;
	  point++;
	  *i = count;
	  count++;
	}
      }
      points.resize(count);
      gaussians.resize(count);
    }

bool testIntegerNorms()
{
  unsigned int nbok = 0;
  unsigned int nb = 0;

  DGtal::int32_t t[]= {2,1,3,4};
  PointVector<4,DGtal::int32_t> p1(t);
  DGtal::int32_t t2[]= {4,5,3,2};
  PointVector<4,DGtal::int32_t> p2(t2);
  PointVector<4,DGtal::int32_t> p = p2 - p1;
  
  trace.beginBlock ( "Checking Integer norm1" );
  trace.info() << "p1: "<<p1 <<", "<<"p2: "<<p2 <<std::endl;
  nbok += p.norm1() == 8 ? 1 : 0; 
  nb++;
  trace.info() << "(" << nbok << "/" << nb << ") "
	       << "L1(p2-p1): "<< p.norm1() << "( == 8 ?)" << std::endl;
  nbok += p.normInfinity() == 4 ? 1 : 0; 
  nb++;
  trace.info() << "(" << nbok << "/" << nb << ") "
	       << "Linfty(p2-p1): "<< p.normInfinity()  << "( == 4 ?)"
	       << std::endl;
  trace.endBlock();

  return nbok == nb;
}

void MouseMovementManager::mouseMove(QPointF const & pnt)
{
	PointVector path = mPath.back();
	mPath.pop_back();
	path.push_back(pnt);
	mPath.push_back(path);
	recountCentre();
}

  void SparseSurfaceAdjustmentT<EdgeType1, EdgeType2, EdgeType3>::optimizeColorsIncidenteWeight()
  {
    for(int i = 0; i < (int) graph()->_verticies_points.size(); ++i){
      PointVertex* point = graph()->_verticies_points[i];
      std::vector<PointVertex* > neighborhood;
      double totalWeight = 0;
      double r = 0; //point->cr;
      double g = 0; //point->cg;
      double b = 0; //point->cb;

      //find neighborhood
      for (g2o::OptimizableGraph::EdgeSet::iterator it=point->edges().begin(); it!=point->edges().end(); it++){
        EdgeType3* edge=dynamic_cast<EdgeType3*>(*it);
        if(edge){
          PointVertex* vp1=dynamic_cast<PointVertex*>(edge->vertices()[0]);
          PointVertex* vp2=dynamic_cast<PointVertex*>(edge->vertices()[1]);
          if(point->id() != vp1->id()){
            neighborhood.push_back(vp1);
          }
          if(point->id() != vp2->id()){
            neighborhood.push_back(vp2);
          }
        }
      }

      ///Add weighted color values
      for(size_t j = 0; j < neighborhood.size(); ++j){
        PointVertex*& n = neighborhood[j];
        ///find sensor edge and calculate incidence angle
        PoseVertex* nParent =  n->parentVertex();
        PointVector beam = nParent->estimate().translation() - n->estimate();
        double incidenceAngle = fabs(acos(beam.dot(n->normal())));
        double weight = 1.0; // - (incidenceAngle / (M_PI * 0.5));
//         weight = min(weight, 0.0);
//         weight *= weight;
//         if(RAD2DEG(incidenceAngle) > 60.0)
//           weight = 0.0;
        r += n->cr * weight;
        g += n->cg * weight;
        b += n->cb * weight;
        totalWeight += weight;
      }

      //set colors
      if(neighborhood.size() > 1){
        point->cr = r / totalWeight;
        point->cg = g / totalWeight;
        point->cb = b / totalWeight;
        for(size_t j = 0; j < neighborhood.size(); ++j){
          PointVertex*& n = neighborhood[j];
          n->cr = point->cr;
          n->cg = point->cg;
          n->cb = point->cb;
        }
      }
    }
  }

 double PathDistance(PointVector& pts1, PointVector& pts2)
 {
     double d = 0.0;
     for (size_t i = 0; i < pts1.size(); ++i) // assumes pts1.length == pts2.length
     {
         d += Distance(pts1[i], pts2[i]);
     }
     return d / pts1.size();
 }

void util::printPointVector(const PointVector pVect, int fieldWidth)
{
    PointVector::const_iterator point;
    for (point = pVect.begin(); point != pVect.end(); ++point) {
        std::cout << "["
            << std::setw(fieldWidth) << (*point).x << " , "
            << std::setw(fieldWidth) << (*point).y << "]  ";
    }
    std::cout << std::endl;
}

PointVector Space::leaveFromZone(const PointVector& vec, const Point& splitPoint, unsigned int d)
{
    PointVector result;
    PointVector::const_iterator iter = vec.begin();
    PointVector::const_iterator endIter = vec.end();
    for(;iter!=endIter;++iter)
    {
        if(iter->distanceXTo(splitPoint)<=d)
            result.push_back(*iter);
    }
    return result;
}

 PointPtr Centroid(PointVector& points)
 {
     double x = 0;
     double y = 0;
     for (size_t i = 0; i < points.size(); ++i)
     {
         x += points[i]->X;
         y += points[i]->Y;
     }
     x /= points.size();
     y /= points.size();
     return PointPtr(new Point(x, y));
 }

void fourier_broken(T &m, feature_t* buf) {
    int dftCount = FDLENGTH;

	typename ImageFactory<T>::view_type *tmp = simple_image_copy(m);
    // get contour points for each CC
    ImageList* ccs = cc_analysis(*tmp);
    PointVector p;

    for(ImageList::iterator cc_it = ccs->begin(); cc_it != ccs->end();
            cc_it++) {
        Cc* cc = static_cast<Cc*>(*cc_it);
        Point orig = cc->origin();
        PointVector* cc_p = contour_pavlidis(*cc);

        for(PointVector::iterator p_it = cc_p->begin();
                p_it != cc_p->end(); p_it++) {

            p.push_back(*p_it + orig);
        }
        delete *cc_it;
        delete cc_p;
    } 
    delete ccs;
	delete tmp->data();
	delete tmp;

    if (p.size() == 0) {
      for (int k = 0; k < dftCount; k++)
        buf[k] = 0.0;
      return;
    }
    else if (p.size() == 1) {
      buf[0] = 1.0;
      for (int k = 1; k < dftCount; k++)
        buf[k] = 0.0;
      return;
    }

    //  calculate convex hull and interpolate points
    PointVector* hullPoints = convex_hull_from_points(&p);
    FloatPointVector* interpolatedHullPoints =
        interpolatePolygonPoints(hullPoints);

    FloatVector* distances = minimumContourHullDistances(interpolatedHullPoints, &p);

    floatFourierDescriptorBrokenA(interpolatedHullPoints, &p, distances, dftCount, buf);

    delete hullPoints;
    delete interpolatedHullPoints;
    delete distances;
}

void PeakDetect::findAll(float delta) {
	
	clear();
	
	Point max(-1, -1);
	Point min(-1, 65536);
	bool findMax = false;
	
	for (index = 0; index < length; index++) {
		// check if max/min should be updated
		if (y[index] > max.getY()) max = Point(index, y[index]);
		if (minima->size()) {
			if (y[index] < min.getY()) min = Point(index, y[index]);
		} else {
			if (y[index] <= min.getY()) min = Point(index, y[index]);
		}
		// check if max/min are larger than delta
		if (findMax) {
			if (y[index] < max.getY() - delta) {
				maxima->add(max.getX(), max.getY());
				min = Point(index, y[index]);
				findMax = false;
			}
		} else {
			if (y[index] > min.getY() + delta) {
				minima->add(min.getX(), min.getY());
				max = Point(index, y[index]);
				findMax = true;
			}
		}
	}
	// add last minimum
	minima->add(min.getX(), min.getY());
	
	if (x != NULL) {
		
		PointVector *maxtemp = new PointVector();
		PointVector *mintemp = new PointVector();
		
		for (int i = 0; i < maxima->size(); i++)
			maxtemp->add(x[(maxima->get(i))->getX()], (maxima->get(i))->getY());
		for (int i = 0; i < minima->size(); i++)
			mintemp->add(x[(minima->get(i))->getX()], (minima->get(i))->getY());
		
		delete maxima;
		delete minima;
		maxima = maxtemp;
		minima = mintemp;
		
	}
}

PointVector rectangle(const QRectF &rect)
{
	const QPointF p1 = rect.topLeft();
	const QPointF p2 = rect.bottomRight();

	PointVector pv;
	pv.reserve(5);
	pv << Point(p1, 1);
	pv << Point(p1.x(), p2.y(), 1);
	pv << Point(p2, 1);
	pv << Point(p2.x(), p1.y(), 1);
	pv << Point(p1.x() + 1, p1.y(), 1);
	return pv;
}

PointVector ImageRegistrator::removeBadPoints(PointVector points, std::vector<uchar> status)
{
    std::vector<uchar>::iterator statusIter = status.begin();
    PointVector::iterator pointIter = points.begin();
    PointVector goodPoints(points.size() - std::count(status.begin(), status.end(), 0));

    for (int i = 0; statusIter != status.end(); ++statusIter, ++pointIter) {
        if (*statusIter == 1) {
            goodPoints[i] = *pointIter;
            ++i;
        }
    }

    return goodPoints;
}