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

bool
vpPolygon3D::getClippedPointsDistance(const vpPoint &p1, const vpPoint &p2,
                               vpPoint &p1Clipped, vpPoint &p2Clipped,
                               unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
                               const unsigned int &flag, const double &distance)
{
    // Since p1 and p1Clipped can be the same object as well as p2 and p2Clipped
    // to avoid a valgrind "Source and destination overlap in memcpy" error,
    // we introduce a two temporary points.
    vpPoint p1Clipped_, p2Clipped_;
    p1Clipped_ = p1;
    p2Clipped_ = p2;

    p1Clipped = p1Clipped_;
    p2Clipped = p2Clipped_;


    bool test1 = (p1Clipped.get_Z() < distance && p2Clipped.get_Z() < distance);
    if(flag == vpPolygon3D::FAR_CLIPPING)
        test1 = (p1Clipped.get_Z() > distance && p2Clipped.get_Z() > distance);

    bool test2 = (p1Clipped.get_Z() < distance || p2Clipped.get_Z() < distance);
    if(flag == vpPolygon3D::FAR_CLIPPING)
        test2 = (p1Clipped.get_Z() > distance || p2Clipped.get_Z() > distance);

    bool test3 = (p1Clipped.get_Z() < distance);
    if(flag == vpPolygon3D::FAR_CLIPPING)
        test3 = (p1Clipped.get_Z() > distance);

    if(test1)
      return false;

    else if(test2){
      vpPoint pClippedNear;
      double t;
      t = (p2Clipped.get_Z() - p1Clipped.get_Z());
      t = (distance - p1Clipped.get_Z()) / t;

      pClippedNear.set_X((p2Clipped.get_X() - p1Clipped.get_X())*t + p1Clipped.get_X());
      pClippedNear.set_Y((p2Clipped.get_Y() - p1Clipped.get_Y())*t + p1Clipped.get_Y());
      pClippedNear.set_Z(distance);

      if(test3){
        p1Clipped = pClippedNear;
        if(flag == vpPolygon3D::FAR_CLIPPING)
            p1ClippedInfo = p1ClippedInfo | vpPolygon3D::FAR_CLIPPING;
        else
            p1ClippedInfo = p1ClippedInfo | vpPolygon3D::NEAR_CLIPPING;
      }
      else{
        p2Clipped = pClippedNear;
        if(flag == vpPolygon3D::FAR_CLIPPING)
            p2ClippedInfo = p2ClippedInfo | vpPolygon3D::FAR_CLIPPING;
        else
            p2ClippedInfo = p2ClippedInfo | vpPolygon3D::NEAR_CLIPPING;
      }
    }

    return true;
}

/*!
  Create a vpMbScanLineEdge from two points while ordering them.

  \param a : First point of the line.
  \param b : Second point of the line.

  \return Resulting vpMbScanLineEdge.
*/
vpMbScanLine::vpMbScanLineEdge
vpMbScanLine::makeMbScanLineEdge(const vpPoint &a, const vpPoint &b)
{
  vpColVector _a(3);
  vpColVector _b(3);

  _a[0] = std::ceil((a.get_X() * 1e8) * 1e-6);
  _a[1] = std::ceil((a.get_Y() * 1e8) * 1e-6);
  _a[2] = std::ceil((a.get_Z() * 1e8) * 1e-6);

  _b[0] = std::ceil((b.get_X() * 1e8) * 1e-6);
  _b[1] = std::ceil((b.get_Y() * 1e8) * 1e-6);
  _b[2] = std::ceil((b.get_Z() * 1e8) * 1e-6);

  bool b_comp = false;
  for(unsigned int i = 0 ; i < 3 ; ++i)
    if (_a[i] < _b[i])
    {
      b_comp = true;
      break;
    }
    else if(_a[i] > _b[i])
      break;

  if (b_comp)
    return std::make_pair(_a, _b);

  return std::make_pair(_b, _a);
}

/*!
  Interpolate two vpPoints.

  \param a : first point.
  \param b : second point.
  \param alpha : interpolation factor.

  \return Interpolated vpPoint.
*/
vpPoint
vpMbScanLine::mix(const vpPoint &a, const vpPoint &b, double alpha)
{
  vpPoint res;
  res.set_X(a.get_X() + ( b.get_X() - a.get_X() ) * alpha);
  res.set_Y(a.get_Y() + ( b.get_Y() - a.get_Y() ) * alpha);
  res.set_Z(a.get_Z() + ( b.get_Z() - a.get_Z() ) * alpha);

  return res;
}

/*!
  Create a vpColVector of a projected point.

  \param p : Point to project.
  \param v : Resulting vector.
  \param K : Camera parameters.
*/
void
vpMbScanLine::createVectorFromPoint(const vpPoint &p, vpColVector &v, const vpCameraParameters &K)
{
    v = vpColVector(3);

    v[0] = p.get_X() * K.get_px() + K.get_u0() * p.get_Z();
    v[1] = p.get_Y() * K.get_py() + K.get_v0() * p.get_Z();
    v[2] = p.get_Z();
}

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

   \return A cv::Point3d with the 3D coordinates stored in vpPoint in the camera frame.
 */
cv::Point3d vpConvert::vpCamPointToPoint3d(const vpPoint &point) {
    return cv::Point3d(point.get_X(), point.get_Y(), point.get_Z());
}

/*!
  Compute the norm of two vpPoints.

  \param a : first point.
  \param b : second point.

  \return Resulting norm.
*/
double
vpMbScanLine::norm(const vpPoint &a, const vpPoint &b)
{
  return sqrt(vpMath::sqr(a.get_X()-b.get_X()) + vpMath::sqr(a.get_Y() - b.get_Y()) + vpMath::sqr(a.get_Z() - b.get_Z()));
}