C++ vpPoint::get_oZ方法代码示例

void
vpMbtDistanceKltCylinder::buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
{
  p1Ext = p1;
  p2Ext = p2;

  vpColVector ABC(3);
  vpColVector V1(3);
  vpColVector V2(3);

  V1[0] = p1.get_oX();
  V1[1] = p1.get_oY();
  V1[2] = p1.get_oZ();
  V2[0] = p2.get_oX();
  V2[1] = p2.get_oY();
  V2[2] = p2.get_oZ();

  // Get the axis of the cylinder
  ABC = V1-V2;

  // Build our extremity circles
  circle1.setWorldCoordinates(ABC[0],ABC[1],ABC[2],p1.get_oX(),p1.get_oY(),p1.get_oZ(),r);
  circle2.setWorldCoordinates(ABC[0],ABC[1],ABC[2],p2.get_oX(),p2.get_oY(),p2.get_oZ(),r);

  // Build our cylinder
  cylinder.setWorldCoordinates(ABC[0],ABC[1],ABC[2],(p1.get_oX()+p2.get_oX())/2.0,(p1.get_oY()+p2.get_oY())/2.0,(p1.get_oZ()+p2.get_oZ())/2.0,r);
}

/*!
  Build a 3D plane thanks to 3 points and stores it in \f$ plane \f$.
  
  \param P : The first point to define the plane
  \param Q : The second point to define the plane
  \param R : The third point to define the plane
  \param plane : The vpPlane instance used to store the computed plane equation.
*/
void
buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane)
{
  vpColVector a(3);
  vpColVector b(3);
  vpColVector n(3);
  //Calculate vector corresponding to PQ
  a[0]=P.get_oX()-Q.get_oX();
  a[1]=P.get_oY()-Q.get_oY();
  a[2]=P.get_oZ()-Q.get_oZ();

  //Calculate vector corresponding to PR
  b[0]=P.get_oX()-R.get_oX();
  b[1]=P.get_oY()-R.get_oY();
  b[2]=P.get_oZ()-R.get_oZ();

  //Calculate normal vector to plane PQ x PR
  n=vpColVector::cross(a,b);

  //Equation of the plane is given by:
  double A = n[0];
  double B = n[1];
  double C = n[2];
  double D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());

  double norm =  sqrt(A*A+B*B+C*C) ;
  plane.setA(A/norm) ;
  plane.setB(B/norm) ;
  plane.setC(C/norm) ;
  plane.setD(D/norm) ;
}

/*!
  Build a vpMbtDistanceSphere thanks to its center, 3 points (including the center) with
  coordinates expressed in the object frame and defining the plane that contain
  the circle and its radius.

  \param _p1 : Center of the circle.
  \param _p2,_p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
  defining the plane that contains the circle.
  \param r : Radius of the circle.
*/
void
vpMbtDistanceSphere::buildFrom(const vpPoint &_p1, const double r)
{
    sphere = new vpSphere ;
    p1 = new vpPoint ;

    // Get the points
    *p1 = _p1;

    // Get the radius
    radius = r;

    // Build our sphere
    sphere->setWorldCoordinates(_p1.get_oX(), _p1.get_oY(), _p1.get_oZ(), r);
}

void 
kltFbTracker::genFeaturesForTrackOnFace(std::vector<vpPoint>& features, int faceID, int numOfPtsPerFace)
{
	const vpPoint& 	vp1 = pyg[faceID].p[0],
			 		vp2 = pyg[faceID].p[1],
			 		vp3 = pyg[faceID].p[2],
			 		vp4 = pyg[faceID].p[3];

	float step = 1.0 / numOfPtsPerFace;

	// avoid the edge
	for (int i = 1; i < numOfPtsPerFace; i++)
		for (int j = 1; j < numOfPtsPerFace; j++)
		{
			vpPoint fp;
			float alpha = i * step;
			float beta  = j * step;
			fp.set_oX((alpha + beta - 1) * vp2.get_oX() + (1 - alpha) * vp1.get_oX() + (1 - beta) * vp3.get_oX());
			fp.set_oY((alpha + beta - 1) * vp2.get_oY() + (1 - alpha) * vp1.get_oY() + (1 - beta) * vp3.get_oY());
			fp.set_oZ((alpha + beta - 1) * vp2.get_oZ() + (1 - alpha) * vp1.get_oZ() + (1 - beta) * vp3.get_oZ());

			features.push_back(fp);
		}
}

/*!
   Unary function to convert the 3D coordinates in the object frame to a cv::Point3d.
   \param point : Point to convert.

   \return A cv::Point3d with the 3D coordinates stored in vpPoint in the object frame.
 */
cv::Point3d vpConvert::vpObjectPointToPoint3d(const vpPoint &point) {
    return cv::Point3d(point.get_oX(), point.get_oY(), point.get_oZ());
}