C++ Point2::x方法代码示例

Point2 Exact_adaptive_kernel::circumcenter(Point2 const& p1, Point2 const& p2, Point2 const& p3)
{
    Point2 p2p1(p2.x()-p1.x(), p2.y()-p1.y());
    Point2 p3p1(p3.x()-p1.x(), p3.y()-p1.y());
    Point2 p2p3(p2.x()-p3.x(), p2.y()-p3.y());
    double p2p1dist = p2p1.x()*p2p1.x() + p2p1.y()*p2p1.y();
    double p3p1dist = p3p1.x()*p3p1.x() + p3p1.y()*p3p1.y();
    double denominator = 0.5/(2.0*signed_area(p1, p2, p3));
    BOOST_ASSERT(denominator > 0.0);
    double dx = (p3p1.y() * p2p1dist - p2p1.y() * p3p1dist) * denominator;
    double dy = (p2p1.x() * p3p1dist - p3p1.x() * p2p1dist) * denominator;
    return Point2(p1.x()+dx, p1.y()+dy);
}

	bool IsInsideTriangle(const ISectTriangle* triangle, MPMesh* pMesh, MPMesh::FaceHandle coTri, const Point2 &bc, Relation &rel)
	{
		Vec3d *v0, *v1, *v2;
		GetCorners(pMesh, coTri, v0, v1, v2);
		
		OpenMesh::Vec2d v[3];
		v[0][0] = (*v0)[triangle->xi]; v[0][1] = (*v0)[triangle->yi];
		v[1][0] = (*v1)[triangle->xi]; v[1][1] = (*v1)[triangle->yi];
		v[2][0] = (*v2)[triangle->xi]; v[2][1] = (*v2)[triangle->yi];

		if (IsPointInTriangle(OpenMesh::Vec2d(bc.x(), bc.y()), v, rel))
		{
			double d = OpenMesh::dot(triangle->pMesh->normal(triangle->face), pMesh->normal(coTri));
			if (pMesh->bInverse ^ triangle->pMesh->bInverse) d = -d;
			if (d > 0.0) rel = REL_SAME;
			else rel = REL_OPPOSITE;
			return true;
		}
		else return false;
	}

void Object::getChildrenDimensions(Point2& r_pos, Point2& r_size) {
	Point2 min(0, 0);
	Point2 max = size;
	if(hasChildren()) {
		BOOST_FOREACH(const PTR(Object)& a, children) {
			Point2 p, s;
			a->getChildrenDimensions(p, s);
			float x1 = p.x();
			float x2 = p.x() + s.x();
			float y1 = p.y();
			float y2 = p.y() + s.y();
			Danvilifmin(min.x(), x1);
			Danvilifmax(max.x(), x2);
			Danvilifmin(min.y(), y1);
			Danvilifmax(max.y(), y2);
		}
	}
	r_pos = min + pos;
	r_size = max - min;
}

/* ************************************************************************* */
Point2 CalibratedCamera::project(const Point3& point,
		boost::optional<Matrix&> H1,
		boost::optional<Matrix&> H2) const {

#ifdef CALIBRATEDCAMERA_CHAIN_RULE
	Point3 q = pose_.transform_to(point, H1, H2);
#else
	Point3 q = pose_.transform_to(point);
#endif
	Point2 intrinsic = project_to_camera(q);

	// Check if point is in front of camera
	if(q.z() <= 0)
	  throw CheiralityException();

	if (H1 || H2) {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
		// just implement chain rule
		Matrix H;
		project_to_camera(q,H);
		if (H1) *H1 = H * (*H1);
		if (H2) *H2 = H * (*H2);
#else
		// optimized version, see CalibratedCamera.nb
		const double z = q.z(), d = 1.0/z;
		const double u = intrinsic.x(), v = intrinsic.y(), uv = u*v;
		if (H1) *H1 = Matrix_(2,6,
				      uv,-(1.+u*u), v, -d , 0., d*u,
				(1.+v*v),      -uv,-u,  0.,-d , d*v
		    );
		if (H2) {
			const Matrix R(pose_.rotation().matrix());
			*H2 = d * Matrix_(2,3,
					R(0,0) - u*R(0,2), R(1,0) - u*R(1,2), R(2,0) - u*R(2,2),
					R(0,1) - v*R(0,2), R(1,1) - v*R(1,2), R(2,1) - v*R(2,2)
		    );
		}
#endif
	}
	return intrinsic;
}

/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p,
		   boost::optional<Matrix&> H1,
		   boost::optional<Matrix&> H2) const {
//	r = x^2 + y^2 ;
//	g = (1 + k(1)*r + k(2)*r^2) ;
//	pi(:,i) = g * pn(:,i)
	const double x = p.x(), y = p.y() ;
	const double r = x*x + y*y ;
	const double r2 = r*r ;
	const double g = 1 + k1_ * r + k2_*r2 ; // save one multiply
	const double fg = f_*g ;

	// semantic meaningful version
	//if (H1) *H1 = D2d_calibration(p) ;
	//if (H2) *H2 = D2d_intrinsic(p) ;

	// unrolled version, much faster
	if ( H1 || H2 ) {

		const double fx = f_*x, fy = f_*y ;
		if ( H1 ) {
			*H1 = Matrix_(2, 3,
				g*x, fx*r , fx*r2,
				g*y, fy*r , fy*r2) ;
		}

		if ( H2 ) {
			const double dr_dx = 2*x ;
			const double dr_dy = 2*y ;
			const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ;
			const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ;
			*H2 = Matrix_(2, 2,
					fg + fx*dg_dx, fx*dg_dy,
					fy*dg_dx , fg + fy*dg_dy) ;
		}
	}

	return Point2(fg * x, fg * y) ;
}

/* ************************************************************************* */
bool SimPolygon2D::contains(const Point2& c) const {
  vector<SimWall2D> edges = walls();
  bool initialized = false;
  bool lastSide = false;
  BOOST_FOREACH(const SimWall2D& ab, edges) {
    // compute cross product of ab and ac
    Point2 dab = ab.b() - ab.a();
    Point2 dac = c - ab.a();
    double cross = dab.x() * dac.y() - dab.y() * dac.x();
    if (fabs(cross) < 1e-6) // check for on one of the edges
      return true;
    bool side = cross > 0;
    // save the first side found
    if (!initialized) {
      lastSide = side;
      initialized = true;
      continue;
    }

    // to be inside the polygon, point must be on the same side of all lines
    if (lastSide != side)
      return false;
  }

Point2 Exact_adaptive_kernel::offcenter(Point2 const& p1, Point2 const& p2, Point2 const& p3, double offconstant)
{
    Point2 p2p1(p2.x()-p1.x(), p2.y()-p1.y());
    Point2 p3p1(p3.x()-p1.x(), p3.y()-p1.y());
    Point2 p2p3(p2.x()-p3.x(), p2.y()-p3.y());
    double p2p1dist = p2p1.x()*p2p1.x() + p2p1.y()*p2p1.y();
    double p3p1dist = p3p1.x()*p3p1.x() + p3p1.y()*p3p1.y();
    double p2p3dist = p2p3.x()*p2p3.x() + p2p3.y()*p2p3.y();
    double denominator = 0.5/(2.0*Exact_adaptive_kernel::signed_area(p1, p2, p3));
    BOOST_ASSERT(denominator > 0.0);
    double dx = (p3p1.y() * p2p1dist - p2p1.y() * p3p1dist) * denominator;
    double dy = (p2p1.x() * p3p1dist - p3p1.x() * p2p1dist) * denominator;
    double dxoff, dyoff;
    
    if ((p2p1dist < p3p1dist) && (p2p1dist < p2p3dist)) {
        dxoff = 0.5 * p2p1.x() - offconstant * p2p1.y();
        dyoff = 0.5 * p2p1.y() + offconstant * p2p1.x();
        if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy) {
            dx = dxoff;
            dy = dyoff;
        }
    } else if (p3p1dist < p2p3dist) {
        dxoff = 0.5 * p3p1.x() + offconstant * p3p1.y();
        dyoff = 0.5 * p3p1.y() - offconstant * p3p1.x();
        if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy) {
            dx = dxoff;
            dy = dyoff;
        }
    } else {
        dxoff = 0.5 * p2p3.x() - offconstant * p2p3.y();
        dyoff = 0.5 * p2p3.y() + offconstant * p2p3.x();
        if (dxoff * dxoff + dyoff * dyoff < (dx - p2p1.x()) * (dx - p2p1.x()) + (dy - p2p1.y()) * (dy - p2p1.y())) {
            dx = p2p1.x() + dxoff;
            dy = p2p1.y() + dyoff;
        }
    }
    
    return Point2(p1.x()+dx, p1.y()+dy);
}

// main
int main(int argc, char** argv) {

  // load Plaza1 data
  list<TimedOdometry> odometry = readOdometry();
//  size_t M = odometry.size();

  vector<RangeTriple> triples = readTriples();
  size_t K = triples.size();

  // parameters
  size_t start = 220, end=3000;
  size_t minK = 100; // first batch of smart factors
  size_t incK = 50; // minimum number of range measurements to process after
  bool robust = true;
  bool smart = true;

  // Set Noise parameters
  Vector priorSigmas = Vector3(1, 1, M_PI);
  Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
  double sigmaR = 100; // range standard deviation
  const NM::Base::shared_ptr // all same type
  priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
  odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
  rangeNoise = robust ? tukey : gaussian;

  // Initialize iSAM
  ISAM2 isam;

  // Add prior on first pose
  Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
      M_PI - 2.02108900000000);
  NonlinearFactorGraph newFactors;
  newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));

  ofstream os2(
      "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
  ofstream os3(
      "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultSR.txt");

  //  initialize points (Gaussian)
  Values initial;
  if (!smart) {
    initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
    initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
    initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
    initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
  }
  Values landmarkEstimates = initial; // copy landmarks
  initial.insert(0, pose0);

  //  initialize smart range factors
  size_t ids[] = { 1, 6, 0, 5 };
  typedef boost::shared_ptr<SmartRangeFactor> SmartPtr;
  map<size_t, SmartPtr> smartFactors;
  if (smart) {
    for(size_t jj: ids) {
      smartFactors[jj] = SmartPtr(new SmartRangeFactor(sigmaR));
      newFactors.push_back(smartFactors[jj]);
    }
  }

  // set some loop variables
  size_t i = 1; // step counter
  size_t k = 0; // range measurement counter
  Pose2 lastPose = pose0;
  size_t countK = 0, totalCount=0;

  // Loop over odometry
  gttic_(iSAM);
  for(const TimedOdometry& timedOdometry: odometry) {
    //--------------------------------- odometry loop -----------------------------------------
    double t;
    Pose2 odometry;
    boost::tie(t, odometry) = timedOdometry;

    // add odometry factor
    newFactors.push_back(
        BetweenFactor<Pose2>(i - 1, i, odometry,
            NM::Diagonal::Sigmas(odoSigmas)));

    // predict pose and add as initial estimate
    Pose2 predictedPose = lastPose.compose(odometry);
    lastPose = predictedPose;
    initial.insert(i, predictedPose);
    landmarkEstimates.insert(i, predictedPose);

    // Check if there are range factors to be added
    while (k < K && t >= boost::get<0>(triples[k])) {
      size_t j = boost::get<1>(triples[k]);
      double range = boost::get<2>(triples[k]);
      if (i > start) {
        if (smart && totalCount < minK) {
          smartFactors[j]->addRange(i, range);
          printf("adding range %g for %d on %d",range,(int)j,(int)i);cout << endl;
        }
        else {
          RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range,
//.........这里部分代码省略.........

/* ************************************************************************* */
Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) {
	return Point3(p.x() * scale, p.y() * scale, scale);
}

/******************************************************************************
* Renders the text string at the given location given in normalized
* viewport coordinates ([-1,+1] range).
******************************************************************************/
void DefaultTextPrimitive::renderViewport(SceneRenderer* renderer, const Point2& pos, int alignment)
{
	QSize imageSize = renderer->outputSize();
	Point2 windowPos((pos.x() + 1.0f) * imageSize.width() / 2, (-pos.y() + 1.0f) * imageSize.height() / 2);
	renderWindow(renderer, windowPos, alignment);
}

 /// \brief Check a point is outside this clip.
 ///
 /// @param p point to be checked.
 /// @return true if p is outside the clip.
 bool inside(const Point2& p) const
 {
     return p.x() < rightX;
 }

inline cv::Point2f tocv (Point2 &p)
{ return cv::Point2f (p.x(), p.y()); }

 BOOST_FOREACH(const Point2Pair& pair, pairs) {
     Point2 dq = pair.first  - cp;
     Point2 dp = pair.second - cq;
     c +=  dp.x() * dq.x() + dp.y() * dq.y();
     s +=  dp.y() * dq.x() - dp.x() * dq.y(); // this works but is negative from formula above !! :-(
 }