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

/* ************************************************************************* */
Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2,
  boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2)
{
  // get cosines and sines from rotation matrices
  const Rot2& R1 = r1.r(), R2 = r2.r();
  double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();

  // Assert that R1 and R2 are normalized
  assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);

  // Calculate delta rotation = between(R1,R2)
  double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
  Rot2 R(Rot2::atan2(s,c)); // normalizes

  // Calculate delta translation = unrotate(R1, dt);
  Point2 dt = r2.t() - r1.t();
  double x = dt.x(), y = dt.y();
  Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);

  // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
  if (H1) {
    double dt1 = -s2 * x + c2 * y;
    double dt2 = -c2 * x - s2 * y;
    *H1 = Matrix3(); (*H1) <<
      -c,  -s,  dt1,
      s,  -c,  dt2,
      0.0, 0.0,-1.0;
  }
  if (H2) *H2 = Matrix3::Identity();

  return Pose2(R,t);
}

double VectorMeters2NorthHeading(const Point2& ref, const Point2& p, const Vector2& dir)
{
	double h = atan2(dir.dx, dir.dy) * RAD_TO_DEG;
	if (h < 0.0) h += 360.0;
	double lon_delta = p.x() - ref.x();
	return h + lon_delta * sin(p.y() * DEG_TO_RAD);
}

pscalar PointSolver::LineSegment2D::errorJacobian (Point2 &P, pscalar *jm)
{
	pscalar lsegmentsq = this->lengthSquared();

	pscalar dep1x, dep1y, dep2x, dep2y;
	pscalar p1x = A.coord.x(),
			p1y = A.coord.y(),
			p2x = B.coord.x(),
			p2y = B.coord.y(),
			px = P.x(), py = P.y();

	dep1x = -2*(p2y-p1y)*
		(p2x*py-p1x*py-p2y*px+p1y*px+p1x*p2y-p1y*p2x) *
		(p2y*py-p1y*py+p2x*px-p1x*px-p2y*p2y+p1y*p2y-p2x*p2x+p1x*p2x) /
		(lsegmentsq * lsegmentsq);
	dep1y = 2*(p2x-p1x)*
		(p2x*py-p1x*py-p2y*px+p1y*px+p1x*p2y-p1y*p2x) *
		(p2y*py-p1y*py+p2x*px-p1x*px-p2y*p2y+p1y*p2y-p2x*p2x+p1x*p2x) /
		(lsegmentsq * lsegmentsq);
	dep2x = 2*(p2y-p1y)*
		(p2x*py-p1x*py-p2y*px+p1y*px+p1x*p2y-p1y*p2x) *
		(p2y*py-p1y*py+p2x*px-p1x*px-p1y*p2y-p1x*p2x+p1y*p1y+p1x*p1x) /
		(lsegmentsq * lsegmentsq);
	dep2y = -2*(p2x-p1x)*
		(p2x*py-p1x*py-p2y*px+p1y*px+p1x*p2y-p1y*p2x)*
		(p2y*py-p1y*py+p2x*px-p1x*px-p1y*p2y-p1x*p2x+p1y*p1y+p1x*p1x) /
		(lsegmentsq * lsegmentsq);

	for (int i=0; i<7; i++) {
		jm[i] = dep1x*A.jacobian[i][0] + dep1y*A.jacobian[i][1] + dep2x*B.jacobian[i][0] + dep2y*B.jacobian[i][1];
	}
}

    Matrix H(const Point2& x_t1) const {
        // Update Jacobian
        Matrix H(1,2);
        H(0,0) =  4*x_t1.x() - x_t1.y() - 2.5;
        H(0,1) = -1*x_t1.x() + 7;

        return H;
    }

double ComputeRayAngle(Point2 p, Point2 q, const Camera& cam1, const Camera& cam2)
{
    const Point3 p3n = cam1.GetIntrinsicMatrix().inverse() * EuclideanToHomogenous(p);
    const Point3 q3n = cam2.GetIntrinsicMatrix().inverse() * EuclideanToHomogenous(q);

    const Point2 pn = p3n.head<2>() / p3n.z();
    const Point2 qn = q3n.head<2>() / q3n.z();

    const Point3 p_w = cam1.m_R.transpose() * Point3{pn.x(), pn.y(), -1.0};
    const Point3 q_w = cam2.m_R.transpose() * Point3{qn.x(), qn.y(), -1.0};

    // Compute the angle between the rays
    const double dot = p_w.dot(q_w);
    const double mag = p_w.norm() * q_w.norm();

    return acos(util::clamp((dot / mag), (-1.0 + 1.0e-8), (1.0 - 1.0e-8)));
}

void NorthHeading2VectorMeters(const Point2& ref, const Point2& p, double heading, Vector2& dir)
{
	double lon_delta = p.x() - ref.x();
	double real_heading = heading - lon_delta * sin(p.y() * DEG_TO_RAD);

	dir.dx = sin(real_heading * DEG_TO_RAD);
	dir.dy = cos(real_heading * DEG_TO_RAD);
}

 /// \brief Constructor
 ///
 /// @param a one end of the line defining the edge.
 /// @param b one end of the line defining the edge.
 Edge(const Point2& a, const Point2& b)
 {
     // horizontal segments should be discarded earlier
     assert(a.y() != b.y());
     
     if (a.y() < b.y()) {
         m_start = a;
         m_seg = b - a;
     } else {
         m_start = b;
         m_seg = a - b;
     }
     
     // normal gradient is y/x, here we use x/y. seg.y() will be != 0,
     // as we already asserted above.
     m_inverseGradient = m_seg.x() / m_seg.y();
 }

void
ShortestPathFinder::FindPathToEndPoint ( Point2<int> const& iEndPoint,
					 std::list<Point2<int> >& ioPoints ) {

  if( iEndPoint.x() < 0 || iEndPoint.x() >= mzX ||
      iEndPoint.y() < 0 || iEndPoint.y() >= mzY )
      throw runtime_error( "Invalid start point, out of bounds." );

  // Find the costs if we haven't already.
  if( !mbValidCosts )
    FindCosts();

  // Start at the end and follow the direction table back to the
  // beginning, adding points on the way. The next step in the
  // shortest path is the neighbor with the lowest cost.
  ioPoints.clear();
  Point2<int> current( iEndPoint );
  while ( current.x() != mStartPoint.x() ||
          current.y() != mStartPoint.y() ) {

    // Push the current point to the result path.
    ioPoints.push_front( current );

    // Search for the lowest cost of its neighbors.
    float lowestCost = numeric_limits<float>::max();
    int lowestDirection = 0;
    for ( int direction = 1; direction < 9; direction++ ) {
      Point2<int> neighbor( current.x() + GetDirectionOffset(direction)[0],
			    current.y() + GetDirectionOffset(direction)[1]);
      if ( neighbor.x() >= 0 && neighbor.x() < mzX &&
	   neighbor.y() >= 0 && neighbor.y() < mzY ) {
	float cost = maTotalCost->Get( neighbor );
	if( cost < lowestCost ) {
	  lowestCost = cost;
	  lowestDirection = direction;
	}
      }
    }

    // Update current to be the neighbor with the lowest cost.
    current.Set( current.x() + GetDirectionOffset(lowestDirection)[0],
		 current.y() + GetDirectionOffset(lowestDirection)[1]);
  }
}

void MetersToLLE(const Point2& ref, int count, Point2 * pts)
{
	while(count--)
	{
		pts->y_ = ref.y() + pts->y() * MTR_TO_DEG_LAT;
		pts->x_ = ref.x() + pts->x() * MTR_TO_DEG_LAT / cos(pts->y() * DEG_TO_RAD);

		++pts;
	}
}

bool			WED_GISBoundingBox::PtOnFrame		(GISLayer_t l,const Point2& p, double d) const
{
	Bbox2	me;
	GetBounds(l,me);

	if (p.x() < me.xmin() ||
		p.x() > me.xmax() ||
		p.y() < me.ymin() ||
		p.y() > me.ymax())
		return false;

	if (p.x() > me.xmin() &&
		p.x() < me.xmax() &&
		p.y() > me.ymin() &&
		p.y() < me.ymax())
		return false;

	return true;
}

    // Calculate the next state prediction using the nonlinear function f()
    Point2 f(const Point2& x_t0) const {

        // Calculate f(x)
        double x = x_t0.x() * x_t0.x();
        double y = x_t0.x() * x_t0.y();
        Point2 x_t1(x,y);

        // In this example, the noiseModel remains constant
        return x_t1;
    }

    // Calculate the Jacobian of the nonlinear function f()
    Matrix F(const Point2& x_t0) const {
        // Calculate Jacobian of f()
        Matrix F = Matrix(2,2);
        F(0,0) = 2*x_t0.x();
        F(0,1) = 0.0;
        F(1,0) = x_t0.y();
        F(1,1) = x_t0.x();

        return F;
    }

// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_from(const Point2& p,
                             boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
    const Point2 q = r_ * p;
    if (H1 || H2) {
        const Matrix R = r_.matrix();
        const Matrix Drotate1 = (Matrix(2, 1) <<  -q.y(), q.x());
        if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
        if (H2) *H2 = R;                         // R
    }
    return q + t_;
}

/* ************************************************************************* */
Matrix Cal3Bundler::D2d_calibration(const Point2& p) const {

	const double x = p.x(), y = p.y(), xx = x*x, yy = y*y ;
	const double r = xx + yy ;
	const double r2 = r*r ;
	const double f = f_ ;
	const double g = 1 + (k1_+k2_*r) * r ; // save one multiply
	//const double g = (1+k1_*r+k2_*r2) ;

	return Matrix_(2, 3,
	g*x, f*x*r , f*x*r2,
	g*y, f*y*r , f*y*r2);
}

/* ************************************************************************* */
Vector Pose2::Logmap(const Pose2& p) {
    const Rot2& R = p.r();
    const Point2& t = p.t();
    double w = R.theta();
    if (std::abs(w) < 1e-10)
        return (Vector(3) << t.x(), t.y(), w);
    else {
        double c_1 = R.c()-1.0, s = R.s();
        double det = c_1*c_1 + s*s;
        Point2 p = R_PI_2 * (R.unrotate(t) - t);
        Point2 v = (w/det) * p;
        return (Vector(3) << v.x(), v.y(), w);
    }
}