C++ Vector2d::squaredNorm方法代码示例

double
FittingCurve2d::findClosestElementMidPoint (const ON_NurbsCurve &nurbs, const Eigen::Vector2d &pt, double hint)
{
  // evaluate hint
  double param = hint;
  double points[2];
  nurbs.Evaluate (param, 0, 2, points);
  Eigen::Vector2d p (points[0], points[1]);
  Eigen::Vector2d r = p - pt;

  double d_shortest_hint = r.squaredNorm ();
  double d_shortest_elem (DBL_MAX);

  // evaluate elements
  std::vector<double> elements = pcl::on_nurbs::FittingCurve2d::getElementVector (nurbs);
  double seg = 1.0 / (nurbs.Order () - 1);

  for (unsigned i = 0; i < elements.size () - 1; i++)
  {
    double &xi0 = elements[i];
    double &xi1 = elements[i + 1];
    double dxi = xi1 - xi0;

    for (unsigned j = 0; j < nurbs.Order (); j++)
    {
      double xi = xi0 + (seg * j) * dxi;

      nurbs.Evaluate (xi, 0, 2, points);
      p (0) = points[0];
      p (1) = points[1];

      r = p - pt;

      double d = r.squaredNorm ();

      if (d < d_shortest_elem)
      {
        d_shortest_elem = d;
        param = xi;
      }
    }
  }

  if(d_shortest_hint < d_shortest_elem)
    return hint;
  else
    return param;
}

ON_NurbsCurve
FittingCurve2d::initNurbsCurve2D (int order, const vector_vec2d &data, int ncps, double radiusF)
{
  if (data.empty ())
    printf ("[FittingCurve2d::initNurbsCurve2D] Warning, no boundary parameters available\n");

  Eigen::Vector2d mean = NurbsTools::computeMean (data);

  unsigned s = data.size ();

  double r (0.0);
  for (unsigned i = 0; i < s; i++)
  {
    Eigen::Vector2d d = data[i] - mean;
    double sn = d.squaredNorm ();
    if (sn > r)
      r = sn;
  }
  r = radiusF * sqrt (r);

  if (ncps < 2 * order)
    ncps = 2 * order;

  ON_NurbsCurve nurbs = ON_NurbsCurve (2, false, order, ncps);
  nurbs.MakePeriodicUniformKnotVector (1.0 / (ncps - order + 1));

  double dcv = (2.0 * M_PI) / (ncps - order + 1);
  Eigen::Vector2d cv;
  for (int j = 0; j < ncps; j++)
  {
    cv (0) = r * sin (dcv * j);
    cv (1) = r * cos (dcv * j);
    cv = cv + mean;
    nurbs.SetCV (j, ON_3dPoint (cv (0), cv (1), 0.0));
  }

  return nurbs;
}

unsigned
NurbsTools::getClosestPoint (const Eigen::Vector2d &p, const Eigen::Vector2d &dir, const vector_vec2d &data,
                             unsigned &idxcp)
{
  if (data.empty ())
    throw std::runtime_error ("[NurbsTools::getClosestPoint(2d)] Data empty.\n");

  size_t idx = 0;
  idxcp = 0;
  double dist2 (0.0);
  double dist2cp (DBL_MAX);
  for (size_t i = 0; i < data.size (); i++)
  {
    Eigen::Vector2d v = (data[i] - p);
    double d2 = v.squaredNorm ();

    if (d2 < dist2cp)
    {
      idxcp = i;
      dist2cp = d2;
    }

    if (d2 == 0.0)
      return i;

    v.normalize ();

    double d1 = dir.dot (v);
    if (d1 / d2 > dist2)
    {
      idx = i;
      dist2 = d1 / d2;
    }
  }
  return idx;
}

	double distance_squared () const { return translation.squaredNorm(); }

//.........这里部分代码省略.........

#if 0
            if (led.getMeasurement().knowBoundingBox) {
                /// @todo For right now, if we don't have a bounding box, we're
                /// assuming it's square enough (and only testing for
                /// non-squareness on those who actually do have bounding
                /// boxes). This is very much a temporary situation.
                auto boundingBoxRatio =
                    led.getMeasurement().boundingBox.height /
                    led.getMeasurement().boundingBox.width;
                if (boundingBoxRatio < minBoxRatio ||
                    boundingBoxRatio > maxBoxRatio) {
                    /// skip non-circular blobs.
                    numBad++;
                    continue;
                }
            }
#endif

            auto localVarianceFactor = varianceFactor;
            auto newIdentificationVariancePenalty =
                std::pow(2.0, led.novelty());

            /// Stick a little bit of process model uncertainty in the beacon,
            /// if it's meant to have some
            if (m_beaconFixed[id]) {
                beaconProcess.setNoiseAutocorrelation(0);
            } else {
                beaconProcess.setNoiseAutocorrelation(
                    m_params.beaconProcessNoise);
                kalman::predict(*(m_beacons[id]), beaconProcess, dt);
            }

            meas.setMeasurement(
                Eigen::Vector2d(led.getLocation().x, led.getLocation().y));
            led.markAsUsed();
            auto state = kalman::makeAugmentedState(m_state, *(m_beacons[id]));
            meas.updateFromState(state);
            Eigen::Vector2d residual = meas.getResidual(state);
            if (residual.squaredNorm() > maxSquaredResidual) {
                // probably bad
                numBad++;
                localVarianceFactor *= m_params.highResidualVariancePenalty;
            } else {
                numGood++;
            }
            debug.residual.x = residual.x();
            debug.residual.y = residual.y();
            auto effectiveVariance =
                localVarianceFactor * m_params.measurementVarianceScaleFactor *
                newIdentificationVariancePenalty *
                (led.isBright() ? BRIGHT_PENALTY : 1.) *
                m_beaconMeasurementVariance[id] / led.getMeasurement().area;
            debug.variance = effectiveVariance;
            meas.setVariance(effectiveVariance);

            /// Now, do the correction.
            auto model =
                kalman::makeAugmentedProcessModel(m_model, beaconProcess);
            kalman::correct(state, model, meas);
            m_gotMeasurement = true;
        }

        /// Probation: Dealing with ratios of bad to good residuals
        bool incrementProbation = false;
        if (0 == m_framesInProbation) {
            // Let's try to keep a 3:2 ratio of good to bad when not "in
            // probation"
            incrementProbation = (numBad * 3 > numGood * 2);

        } else {
            // Already in trouble, add a bit of hysteresis and raising the bar
            // so we don't hop out easily.
            incrementProbation = numBad * 2 > numGood;
            if (!incrementProbation) {
                // OK, we're good again
                m_framesInProbation = 0;
            }
        }
        if (incrementProbation) {
            m_framesInProbation++;
        }

        /// Frames without measurements: dealing with getting in a bad state
        if (m_gotMeasurement) {
            m_framesWithoutUtilizedMeasurements = 0;
        } else {
            if (inBoundsID > 0) {
                /// We had a measurement, we rejected it. The problem may be the
                /// plank in our own eye, not the speck in our beacon's eye.
                m_framesWithoutUtilizedMeasurements++;
            }
        }

        /// Output to the OpenCV state types so we can see the reprojection
        /// debug view.
        m_rvec = eiQuatToRotVec(m_state.getQuaternion());
        cv::eigen2cv(m_state.position().eval(), m_tvec);
        return true;
    }

const CPoint3DWORLD CLandmark::_getOptimizedLandmarkIDWA( )
{
    //ds return vector
    CPoint3DWORLD vecPointXYZWORLD( Eigen::Vector3d::Zero( ) );

    //ds total accumulated depth
    double dInverseDepthAccumulated = 0.0;

    //ds loop over all measurements
    for( const CMeasurementLandmark* pMeasurement: m_vecMeasurements )
    {
        //ds current inverse depth
        const double dInverseDepth = 1.0/pMeasurement->vecPointXYZLEFT.z( );

        //ds add current measurement with depth weight
        vecPointXYZWORLD += dInverseDepth*pMeasurement->vecPointXYZWORLD;

        //std::cout << "in camera frame: " << pMeasurement->vecPointXYZ.transpose( ) << std::endl;
        //std::cout << "adding: " << dInverseDepth << " x " << pMeasurement->vecPointXYZWORLD.transpose( ) << std::endl;

        //ds accumulate depth
        dInverseDepthAccumulated += dInverseDepth;
    }

    //ds compute average point
    vecPointXYZWORLD /= dInverseDepthAccumulated;

    //std::cout << "from: " << vecPointXYZCalibrated.transpose( ) << " to: " << vecPointXYZWORLD.transpose( ) << std::endl;

    ++uOptimizationsSuccessful;

    double dSumSquaredErrors = 0.0;

    //ds loop over all previous measurements again
    for( const CMeasurementLandmark* pMeasurement: m_vecMeasurements )
    {
        //ds get point into camera (cast to avoid eclipse error..)
        const CPoint3DHomogenized vecPointXYZLEFT( CMiniVisionToolbox::getHomogeneous( static_cast< CPoint3DCAMERA >( pMeasurement->matTransformationWORLDtoLEFT*vecPointXYZWORLD ) ) );

        //ds get projected point
        const CPoint2DHomogenized vecProjectionHomogeneous( m_matProjectionLEFT*vecPointXYZLEFT );

        //ds compute pixel coordinates TODO remove cast
        const Eigen::Vector2d vecUV = CWrapperOpenCV::getInterDistance( static_cast< Eigen::Vector2d >( vecProjectionHomogeneous.head< 2 >( )/vecProjectionHomogeneous(2) ), pMeasurement->ptUVLEFT );

        //ds compute squared error
        const double dSquaredError( vecUV.squaredNorm( ) );

        //ds add up
        dSumSquaredErrors += dSquaredError;
    }

    //ds average the measurement
    dCurrentAverageSquaredError = dSumSquaredErrors/m_vecMeasurements.size( );

    //ds if optimal
    if( 5.0 > dCurrentAverageSquaredError )
    {
        bIsOptimal = false;
    }

    //ds return
    return vecPointXYZWORLD;
}