C++ Observation类代码示例

void coap_handle_notification(NetworkAddress * sourceAddress, coap_packet_t * message)
{
    if (message->token_len == sizeof(int))
    {
        int token;
        memcpy(&token, message->token, sizeof(int));
        Observation * observation = NULL;
        int index;
        for (index = 0; index < MAX_COAP_OBSERVATIONS; index++)
        {
            if ((Observations[index].Token == token) && (NetworkAddress_Compare(Observations[index].Address, sourceAddress) == 0))
            {
                observation = &Observations[index];
                break;
            }
        }
        if (observation && observation->Callback)
        {
            AddressType address;
            int ContentType = 0;
            char * payload = NULL;

            NetworkAddress_SetAddressType(sourceAddress, &address);
            coap_get_header_content_format(message, &ContentType);
            int payloadLen = coap_get_payload(message, (const uint8_t **) &payload);

            observation->Callback(observation->Context, &address, observation->Path, COAP_OPTION_TO_RESPONSE_CODE(message->code),
                    ContentType, payload, payloadLen);
        }
    }

}

void ProfilerIOInterposeObserver::Observe(Observation& aObservation)
{
  const char* str = nullptr;

  switch (aObservation.ObservedOperation()) {
    case IOInterposeObserver::OpCreateOrOpen:
      str = "create/open";
      break;
    case IOInterposeObserver::OpRead:
      str = "read";
      break;
    case IOInterposeObserver::OpWrite:
      str = "write";
      break;
    case IOInterposeObserver::OpFSync:
      str = "fsync";
      break;
    case IOInterposeObserver::OpStat:
      str = "stat";
      break;
    case IOInterposeObserver::OpClose:
      str = "close";
      break;
    default:
      return;
  }
  ProfilerBacktrace* stack = profiler_get_backtrace();
  IOMarkerPayload* markerPayload = new IOMarkerPayload(aObservation.Reference(),
                                                       aObservation.Start(),
                                                       aObservation.End(),
                                                       stack);
  PROFILER_MARKER_PAYLOAD(str, markerPayload);
}

void ValueIterationPredictor::update(const Transition &transition)
{
  Predictor::update(transition);
  
  double v=-std::numeric_limits<double>::infinity();
  
  for (Discretizer::iterator it = discretizer_->begin(transition.prev_obs); it != discretizer_->end(); ++it)
  {
    Observation next;
    double reward;
    int terminal;
    model_->step(transition.prev_obs, *it, &next, &reward, &terminal);
    
    if (next.size())
    {
      if (!terminal)
      {
        Vector value;
        reward += gamma_*representation_->read(projector_->project(next), &value);
      }
      
      v = fmax(v, reward);
    }
  }

  if (std::isfinite(v))
    representation_->write(projector_->project(transition.prev_obs), VectorConstructor(v));
}

void ToyExperiments::setObservation(const Observation &obs) {
  assert( obs.nBins() >= Parameters::nBins() );

  std::vector<unsigned int>::iterator iOld;
  std::vector<unsigned int>::const_iterator iNew;

  // Set observed yields
  iOld = observedYields_.begin();
  iNew = obs.yields();
  for(; iOld != observedYields_.end(); ++iOld, ++iNew) {
    *iOld = *iNew;
  }
}

/**
 * Determine the simalirty between the observed and expected of a line.
 *
 * @param z    the observation to determine the weight of
 * @param x_t  the a priori estimate of the robot pose.
 * @param l    the landmark to be used as basis for the observation
 * @return     the probability of the observation
 */
float MCL::determineLineWeight(Observation z, PoseEst x_t, LineLandmark line)
{
    // Distance and bearing for expected point
    float d_hat;
    float a_hat;

    // Residuals of distance and bearing observations
    float r_d;
    float r_a;

    // Determine nearest expected point on the line
    // NOTE:  This is currently too naive, we need to extrapolate the entire
    // line from the vision inforamtion and find the closest expected distance
    PointLandmark pt_hat;
    if (line.x1 == line.x2) { // Line is vertical
        pt_hat.x = line.x1;
        pt_hat.y = x_t.y;

        // Make sure we aren't outside the line
        if ( pt_hat.y < line.y1) {
            pt_hat.y = line.y1;
        } else if ( pt_hat.y > line.y2) {
            pt_hat.y = line.y2;
        }
    } else { // Line is horizontal
        pt_hat.x = x_t.x;
        pt_hat.y = line.y1;

        // Make sure we aren't outside the line
        if ( pt_hat.x < line.x1) {
            pt_hat.x = line.x1;
        } else if ( pt_hat.x > line.x2) {
            pt_hat.x = line.x2;
        }
    }

    // Get dist and bearing to expected point
    d_hat = sqrt( pow(pt_hat.x - x_t.x, 2.0f) +
                  pow(pt_hat.y - x_t.y, 2.0f));

    // Expected bearing
    a_hat = atan2(pt_hat.y - x_t.y, pt_hat.x - x_t.x) - x_t.h;

    // Calculate residuals
    r_d = fabs(z.getVisDist() - d_hat);
    r_a = fabs(z.getVisBearing() - a_hat);

    return getSimilarity(r_d, r_a, z);
}

/**
 * Method to deal with incorporating a loc measurement into the EKF
 *
 * @param z the measurement to be incorporated
 * @param H_k the jacobian associated with the measurement, to be filled out
 * @param R_k the covariance matrix of the measurement, to be filled out
 * @param V_k the measurement invariance
 *
 * @return the measurement invariance
 */
void LocEKF::incorporateMeasurement(Observation z,
                                    StateMeasurementMatrix &H_k,
                                    MeasurementMatrix &R_k,
                                    MeasurementVector &V_k)
{
#ifdef DEBUG_LOC_EKF_INPUTS
    cout << "\t\t\tIncorporating measurement " << z << endl;
#endif
    int obsIndex;
    // Get the best fit for ambigious data
    // NOTE: this is only done, if useAmbiguous is turned to true
    if (z.getNumPossibilities() > 1) {
        obsIndex = findBestLandmark(&z);
        // No landmark is close enough, don't attempt to use one
        if (obsIndex < 0) {
            R_k(0,0) = DONT_PROCESS_KEY;
            return;
        }
    } else {
        obsIndex = 0;
    }

	if ( z.isLine() ){
		incorporatePolarMeasurement( obsIndex, z, H_k, R_k, V_k);
	}

    else if (z.getVisDistance() < USE_CARTESIAN_DIST) {
		incorporateCartesianMeasurement( obsIndex, z, H_k, R_k, V_k);
    } else {
		incorporatePolarMeasurement( obsIndex, z, H_k, R_k, V_k);
    }

    // Calculate the standard error of the measurement
    StateMeasurementMatrix newP = prod(P_k, trans(H_k));
    MeasurementMatrix se = prod(H_k, newP) + R_k;
    se(0,0) = sqrt(se(0,0));
    se(1,1) = sqrt(se(1,1));

    // Ignore observations based on standard error
    if ( se(0,0)*6.0f < abs(V_k(0))) {
#ifdef DEBUG_STANDARD_ERROR
        cout << "\t Ignoring measurement " << endl;
        cout << "\t Standard error is " << se << endl;
        cout << "\t Invariance is " << abs(V_k(0))*5 << endl;
#endif
        R_k(0,0) = DONT_PROCESS_KEY;
    }

}

/**
 * Determine the similarity of an observation to a landmark location
 *
 * @param r_d     The difference between the expected and observed distance
 * @param r_a     The difference between the expected and observed bearing
 * @param sigma_d The standard deviation of the distance measurement
 * @param sigma_a The standard deviation of the bearing measurement
 * @return        The combined similarity of the landmark observation
 */
float MCL::getSimilarity(float r_d, float r_a, Observation &z)
{
    // Similarity of observation and expectation
    float s_d_a;
    float sigma_d = z.getDistSD();
    float sigma_a = z.getBearingSD();
    // Calculate the similarity of the observation and expectation
    s_d_a = exp((-(r_d*r_d) / (sigma_d*sigma_d))
                -((r_a*r_a) / (sigma_a*sigma_a)));

    if (s_d_a < MIN_SIMILARITY) {
        s_d_a = MIN_SIMILARITY;
    }
    return s_d_a;
}

void
OutputPredictionSet::pushBack(Observation _prediction, Observation _correct) {
  /* if a correct value is specified, factor trial into accuracy stats */
  if (_correct != Observation::kInvalid) {
    /* increment test count for class */
    ++tested_[_correct.value()];

    /* increment correct count for class */
    if (_prediction == _correct) {
      /* the value of expected identifies the true outcome
         hence, use as index in our prediction statistics model */
      ++correct_[_correct.value()];
    }
  }

  prediction_.pushBack(_prediction);
}

void DynaAgent::runModel()
{
  Observation obs;
  Action action;
  int terminal=1;
  size_t steps=0;

  for (size_t ii=0; ii < planning_steps_; ++ii)
  {
    if (terminal)
    {
      steps = 0;
      obs = start_obs_.draw();
      state_->set(obs.v);
      model_agent_->start(obs, &action);
    }
      
    Observation next;
    double reward;
  
    double tau = model_->step(obs, action, &next, &reward, &terminal);
    
    obs = next;
    state_->set(obs.v);
        
    // Guard against failed model prediction    
    if (obs.size())
    {
      if (terminal == 2)
        model_agent_->end(tau, obs, reward);
      else
        model_agent_->step(tau, obs, reward, &action);
        
      planning_reward_ += reward;
      planned_steps_++;
      total_planned_steps_++;
    }
    else
      terminal = 1;
      
    // Break episodes after a while
    if (planning_horizon_ && steps++ == planning_horizon_)
      terminal = 1;
  }
}

void ProfilerIOInterposeObserver::Observe(Observation& aObservation)
{
  // TODO: The profile might want to take notice of non-main-thread IO, as
  // well as noting what files or references causes the IO.
  if (NS_IsMainThread()) {
    const char* ops[] = {"none", "read", "write", "invalid", "fsync"};
    PROFILER_MARKER(ops[aObservation.ObservedOperation()]);
  }
}

/**
 * Determine the weight to compound the particle weight by for a single
 * observation of an landmark point
 *
 * @param z    the observation to determine the weight of
 * @param x_t  the a priori estimate of the robot pose.
 * @param l    the landmark to be used as basis for the observation
 * @return     the probability of the observation
 */
float MCL::determinePointWeight(Observation z, PoseEst x_t, PointLandmark pt)
{
    // Expected dist and bearing
    float d_hat;
    float a_hat;
    // Residuals of distance and bearing observations
    float r_d;
    float r_a;

    // Determine expected distance to the landmark
    d_hat = sqrt( (pt.x - x_t.x)*(pt.x - x_t.x) +
                  (pt.y - x_t.y)*(pt.y - x_t.y));
    // Expected bearing
    a_hat = atan2(pt.y - x_t.y, pt.x - x_t.x) - x_t.h;
    // Calculate residuals
    r_d = z.getVisDist() - d_hat;
    r_a = z.getVisBearing() - a_hat;

    return getSimilarity(r_d, r_a, z);
}

//! Set the device to be used to plot/log the digitizer statistics
void dsp::LevelMonitor::set_input (IOManager* _input)
{
  input = _input;

  if (!input)
    return;

  if (data)
    input->set_output (data);

  input->prepare();

  input->set_block_size (block_size);

  Observation* info = input->get_info();

  nchan = info->get_nchan();
  npol = info->get_npol();
  ndim = info->get_ndim();

  unpacker = dynamic_cast<HistUnpacker*>(input->get_unpacker());

  if (!unpacker)
    throw Error (InvalidState, "dsp::LevelMonitor::set_input",
                 "unpacker is not a HistUnpacker; optimal_variance unknown");

  if (history)
    history->set_unpacker( unpacker );

  if (unpacker->get_ndim_per_digitizer() == 2)
    ndim = 1;

  optimal_variance = unpacker->get_optimal_variance();

  cerr << "dsp::LevelMonitor optimal_variance=" << optimal_variance << endl;

  ndig = nchan * npol * ndim;
}

void ClassifySvmSharedCommand::readSharedRAbundVectors(vector<SharedRAbundVector*>& lookup, GroupMap& designMap, LabeledObservationVector& labeledObservationVector, FeatureVector& featureVector) {
    for ( int j = 0; j < lookup.size(); j++ ) {
        //i++;
        vector<individual> data = lookup[j]->getData();
        Observation* observation = new Observation(data.size(), 0.0);
        string sharedGroupName = lookup[j]->getGroup();
        string treatmentName = designMap.getGroup(sharedGroupName);
        //std::cout << "shared group name: " << sharedGroupName << " treatment name: " << treatmentName << std::endl;
        //labeledObservationVector.push_back(std::make_pair(treatmentName, observation));
        labeledObservationVector.push_back(LabeledObservation(j, treatmentName, observation));
        //std::cout << " j=" << j << " label : " << lookup[j]->getLabel() << " group: " << lookup[j]->getGroup();
        for (int k = 0; k < data.size(); k++) {
            //std::cout << " abundance " << data[k].abundance;
            observation->at(k) = double(data[k].abundance);
            if ( j == 0) {
                featureVector.push_back(Feature(k, m->currentSharedBinLabels[k]));
            }
        }
        //std::cout << std::endl;
        // let this happen later?
        //delete lookup[j];
    }
}

void FeatureExtractor::calcObservedActions(Observation prevObs, Observation obs, std::vector<Action::Type> &actions) {
  actions.resize(prevObs.positions.size());
  TIC(historyuncenter);
  prevObs.uncenterPrey(dims);
  obs.uncenterPrey(dims);
  TOC(historyuncenter);
  //std::cout << prevObs << " " << obs << std::endl << std::flush;
  bool prevCapture = obs.didPreyMoveIllegally(dims,prevObs.absPrey);
  for (unsigned int i = 0; i < prevObs.positions.size(); i++) {
    // skip if the prey was captured last step
    if (prevCapture && ((int)i == obs.preyInd)) {
      actions[i] = Action::NUM_ACTIONS;
      continue;
    }
    TIC(historydiff);
    Point2D diff = getDifferenceToPoint(dims,prevObs.positions[i],obs.positions[i]);
    TOC(historydiff);
    TIC(historyaction);
    //actions.push_back(getAction(diff));
    actions[i] = getAction(diff);
    TOC(historyaction);
  }
}

void RBFInputData::init(const Observation& obs)
{
    if (discNumOptionsData)
    {
        delete [] discNumOptionsData;
    }

    if (discInputData)
    {
        delete [] discInputData;
    }

    if (contCircularData)
    {
        delete [] contCircularData;
    }

    if (contInputData)
    {
        delete [] contInputData;
    }

    numDiscInputs = obs.getNumDiscreteInputs();
    discNumOptionsData = new unsigned int[numDiscInputs];
    discInputData = new unsigned int[numDiscInputs];
    for (unsigned int i = 0; i < numDiscInputs; ++i)
    {
        discNumOptionsData[i] = obs.getDiscreteInputNumOptions(i);
        discInputData[i] = obs.getDiscreteValue(i);
    }

    numContInputs = obs.getNumContinuousInputs();
    contResolution = obs.getContinuousResolution();
    contCircularData = new bool[numContInputs];
    contInputData = new real[numContInputs];
    for (unsigned int i = 0; i < numContInputs; ++i)
    {
        contCircularData[i] = obs.getContinuousInputIsCircular(i);
        contInputData[i] = obs.getContinuousValue(i);
    }
}