C++ Observations类代码示例

double
MultivariateModel
::ComputeLogLikelihood(const Observations &Obs) 
{
  /// It computes the likelihood of the model. For each subject i, it sums its likelihood, namely the distance,
  /// for each time t_ij, between the observation y_ij and the prediction f(t_ij) = ComputeParallelCurve
  
  ScalarType LogLikelihood = 0;

  /// For each subject
  for(size_t i = 0; i < m_NumberOfSubjects; ++i) 
  {
    ScalarType N = Obs.GetNumberOfTimePoints(i);
    
    /// For each timepoint
    for(size_t j = 0; j < N; ++j)
    {
      auto& it = Obs.GetSubjectCognitiveScore(i, j);
      VectorType PC = ComputeParallelCurve(i, j);
      LogLikelihood += (it - PC).squared_magnitude();
    }
  }
  
  LogLikelihood /= -2*m_Noise->GetVariance();
  LogLikelihood -= m_NbTotalOfObservations * log(sqrt(2 * m_Noise->GetVariance() * M_PI));
  
  return LogLikelihood;
}

///
/// \brief LogicInitiator::processObservations  main function which processes new observations
/// to initiation candidates. And compares already existing candidates and observation with use of
/// cascaded logic.
/// \param observations new observations
/// \return new tracks to be initated to tracker
///
InitiatorCandidates LogicInitiator::processObservations(const Observations observations)
{        
    // declare structure for new tracks which should be initiated by tracker
    InitiatorCandidates newTracks;
    if(observations.empty()) return newTracks;

    // declare structure for accepted candidates
    InitiatorCandidates acceptedCandidates;

    // Get delta time
    double timeDiff = observations.front()->createdAt - m_lastObservationTime;
    m_lastObservationTime = observations.front()->createdAt;

    // Get observations which could not be matched by the tracker
    Observations newObs;
    foreach(Observation::Ptr ob, observations)
    {
        if(!ob->matched)
        {
            newObs.push_back(ob);
        }
    }
    
    // Associate new observations with existing candidates for initiation
    foreach(InitiatorCandidate::Ptr candidate, m_candidates)
    {
        // initialize flag if pairing could be found for candidate
        bool pairingFound(false);
        
        // If selected number of scans will be reached tracks are created
        if(candidate->observations.size() == m_numberScans-1)
        {
            predictKalman(candidate->state, timeDiff);
            candidate->extrapolation = linearExtrapolation(*(candidate->observations.rbegin()+1),
                                                           candidate->observations.back(),timeDiff);
            foreach(Observation::Ptr observation,newObs)
            {
                if(compareWithExtrapolation(candidate,observation))
                {
                    if(compareWithCandidate(candidate, observation))
                    {
                        if(updateKalman(candidate->state,observation))
                        {
                            pairingFound = true;
                            candidate->missedObs = 0;
                            observation->matched = true;
                            candidate->observations.push_back(observation);
                            newTracks.push_back(candidate);
                        }
                    }
                }
            }
        }

    // Gather all Objects and setup ObjectRefs
    void PharmML::setupObjects() {
        std::unordered_set<std::string> allOids;

        TrialDesign *td = this->getTrialDesign();

        if (td) {
            Arms *arms = td->getArms();
            if (arms) {
                for (Arm *arm : arms->getArms()) {
                    this->checkAndAddOid(allOids, arm, arm);
                }
            }
            Observations *observations = td->getObservations();
            if (observations) {
                for (Observation *observation : observations->getObservations()) {
                    this->checkAndAddOid(allOids, observation, observation);
                }
                for (IndividualObservations *observation : observations->getIndividualObservations()) {
                    this->checkAndAddOid(allOids, observation, observation);
                }
            }
            Interventions *interventions = td->getInterventions();
            if (interventions) {
                for (Administration *admin : interventions->getAdministrations()) {
                    this->checkAndAddOid(allOids, admin, admin);
                }
                for (InterventionsCombination *comb : interventions->getInterventionsCombinations()) {
                    this->checkAndAddOid(allOids, comb, comb);
                }
            }
        }

        // Obtain a map from all oids to Objects. Will be used to populate ObjectRefs
        std::unordered_map<std::string, Object *> oidMap;
        for (Object *object : this->allObjects) {
            oidMap[object->getOid()] = object;
        }

        for (Object *object : this->allObjects) {
            object->gatherObjectRefs(oidMap);
        }

        // Populate ObjectReferer ObjectRefs
        if (td) {
            Interventions *interventions = td->getInterventions();
            if (interventions) {
                for (IndividualAdministration *ia : interventions->getIndividualAdministrations()) {
                    ia->gatherObjectRefs(oidMap);
                }
            }
        }
    }

Observations PositionTracker::makeObservations(DataPtr data) {

    Observations obsvs;
    for (int i = 0; i < data->position_size(); i++) {

        Observation obsv(OBSERVATION_DIMENSIONS);
        obsv[0] = data->position(i).position().x();
        obsv[1] = data->position(i).position().y();
        obsv[2] = data->position(i).position().z();

        obsvs.push_back(obsv);
    }
    return obsvs;
}

vector<double> ObjectiveFunc::get_residuals_vec(const Observations &sim_obs, const Parameters &pars, const vector<string> &obs_names) const
{
	vector<double> residuals_vec;
	residuals_vec.resize(obs_names.size(), 0.0);

	Observations::const_iterator found_obs;
	Observations::const_iterator not_found_obs=(*observations_ptr).end();
	PriorInformation::const_iterator found_prior_info;
	PriorInformation::const_iterator not_found_prior_info = prior_info_ptr->end();

	int i=0;
	for(vector<string>::const_iterator b=obs_names.begin(), e=obs_names.end(); b != e; ++b, ++i)
	{
		found_obs = observations_ptr->find(*b);
		found_prior_info = prior_info_ptr->find(*b);

		if (found_obs != not_found_obs)
		{
			residuals_vec[i] = sim_obs.get_rec(*b) - (*found_obs).second;
		}
		else if (found_prior_info != not_found_prior_info)
		{
			residuals_vec[i] = (*found_prior_info).second.calc_residual(pars);
		}
	}
	return residuals_vec;
}

void RunStorage::update_run(int run_id, const Parameters &pars, const Observations &obs)
{
	//set run status flage to complete
	std::int8_t r_status = 1;
	check_rec_id(run_id);
	vector<double> par_data(pars.get_data_vec(par_names));
	vector<double> obs_data(obs.get_data_vec(obs_names));
	//write data to buffer at end of file and set buffer flag to 1
	std::int8_t buf_status = 0;
	std::int32_t buf_run_id = run_id;
	int end_of_runs = get_nruns();
	buf_stream.seekp(get_stream_pos(end_of_runs), ios_base::beg);
	buf_stream.write(reinterpret_cast<char*>(&buf_status), sizeof(buf_status));
	buf_stream.write(reinterpret_cast<char*>(&buf_run_id), sizeof(buf_run_id));
	buf_stream.write(reinterpret_cast<char*>(&r_status), sizeof(r_status));
	buf_stream.write(reinterpret_cast<char*>(par_data.data()), par_data.size() * sizeof(double));
	buf_stream.write(reinterpret_cast<char*>(obs_data.data()), obs_data.size() * sizeof(double));
	buf_status = 1;
	buf_stream.seekp(get_stream_pos(end_of_runs), ios_base::beg);
	buf_stream.write(reinterpret_cast<char*>(&buf_status), sizeof(buf_status));
	buf_stream.flush();
	//write data
	buf_stream.seekp(get_stream_pos(run_id), ios_base::beg);
	buf_stream.write(reinterpret_cast<char*>(&r_status), sizeof(r_status));
	//skip over info_txt and info_value fields
	buf_stream.seekp(sizeof(char)*info_txt_length+sizeof(double), ios_base::cur);
	buf_stream.write(reinterpret_cast<char*>(par_data.data()), par_data.size() * sizeof(double));
	buf_stream.write(reinterpret_cast<char*>(obs_data.data()), obs_data.size() * sizeof(double));
	buf_stream.flush();
	//reset flag for buffer at end of file to 0 to signal it is no longer relavent
	buf_status = 0;
	buf_stream.seekp(get_stream_pos(end_of_runs), ios_base::beg);
	buf_stream.write(reinterpret_cast<char*>(&buf_status), sizeof(buf_status));
	buf_stream.flush();
}

AbstractModel::SufficientStatisticsVector
MultivariateModel
::GetSufficientStatistics(const Realizations &R, const Observations& Obs) 
{
  /// Computation of the suffisient statistics of the model
  /// Basically, it is a vector (named S1 to S9) of vectors
  
  /// S1 <- y_ij * eta_ij    &    S2 <- eta_ij * eta_ij
  VectorType S1(m_NbTotalOfObservations), S2(m_NbTotalOfObservations);
  auto itS1 = S1.begin(), itS2 = S2.begin();
  for(size_t i = 0; i < m_NumberOfSubjects; ++i)
  {
      for(size_t j = 0; j < Obs.GetNumberOfTimePoints(i); ++j)
      {
          VectorType PC = ComputeParallelCurve(i, j);
          *itS1 = dot_product(PC, Obs.GetSubjectCognitiveScore(i, j));
          *itS2 = PC.squared_magnitude();
          ++itS1, ++itS2;
      }
  }
  
  /// S3 <- Ksi_i   &    S4 <- Ksi_i * Ksi_i
  VectorType S3 = R.at("Ksi");
  VectorType S4 = R.at("Ksi") % R.at("Ksi");
  
  /// S5 <- Tau_i   &    S6 <- Tau_i * Tau_i
  VectorType S5 = R.at("Tau");
  VectorType S6 = R.at("Tau") % R.at("Tau");
  
  /// S7 <- G 
  VectorType S7(1, R.at("G", 0));
  
  /// S8 <- beta_k
  VectorType S8((m_ManifoldDimension-1) * m_NbIndependentSources);
  ScalarType * itS8 = S8.memptr();
  for(size_t i = 0; i < S8.size(); ++i)
      itS8[i] = R.at("Beta#" + std::to_string(i), 0);
  
  /// S8 <- delta_k
  VectorType S9(m_ManifoldDimension - 1);
  ScalarType * itS9 = S9.memptr();
  for(size_t i = 1; i < S9.size(); ++i)
      itS9[i] = R.at("Delta#" + std::to_string(i), 0);

  return {S1, S2, S3, S4, S5, S6, S7, S8, S9};
  
}

int RunStorage::get_run(int run_id, Parameters &pars, Observations &obs, string &info_txt, double &info_value, bool clear_old)
{
	vector<double> par_data;
	vector<double> obs_data;

	int status = get_run(run_id, par_data, obs_data, info_txt, info_value);
	if (clear_old)
	{
	  pars.update(par_names, par_data);
	  obs.update(obs_names, obs_data);
	}
	else
	{
	  pars.update_without_clear(par_names, par_data);
	  obs.update_without_clear(obs_names, obs_data);
	}
	return status;
}

TEST(SFM_DATA_FILTERS, LargestViewTrackCC)
{
  // Init a scene with 5 Views & poses
  SfM_Data sfm_data;
  init_scene(sfm_data, 5);

  // Fill with some tracks
  //
  //- View 0,1,2 are sharing 3 tracks
  // TrackId 0 -> 0-1-2
  // TrackId 1 -> 0-1-2
  // TrackId 2 -> 0-1-2
  //
  // While view 3-4 are sharing 1 track
  // TrackId 3 -> 3-4

  Observations obs;
  obs[0] = Observation( Vec2(10,20), 0);
  obs[1] = Observation( Vec2(30,10), 1);
  obs[2] = Observation( Vec2(30,10), 1);
  sfm_data.structure[0].obs = obs;
  sfm_data.structure[0].X = Vec3::Random();

  sfm_data.structure[1].obs = obs;
  sfm_data.structure[1].X = Vec3::Random();
  sfm_data.structure[2].obs = obs;
  sfm_data.structure[2].X = Vec3::Random();

  obs.clear();
  obs[3] = Observation( Vec2(10,20), 0);
  obs[4] = Observation( Vec2(10,20), 0);
  sfm_data.structure[3].obs = obs;
  sfm_data.structure[3].X = Vec3::Random();

  // Track of the SfM_Data scene contains two connected component
  // One for view (0,1,2)
  // One for the view (3,4)
  // Test that function to keep the largest one is working
  EXPECT_EQ (4, sfm_data.GetLandmarks().size());
  EXPECT_FALSE (IsTracksOneCC(sfm_data));
  KeepLargestViewCCTracks(sfm_data);
  EXPECT_EQ (3, sfm_data.GetLandmarks().size());
}

void Simulator::perturbProjections(Observations& obs, double sigma){

  for (Observations::iterator it = obs.begin(); it != obs.end(); ++it){

    double dx = Simulator::getGaussianSample(0,sigma);
    double dy = Simulator::getGaussianSample(0,sigma);

    ROS_WARN("observation moved by %f %f", dx,dy);

    CvPoint2D32f p = it->second;

    p.x += dx; // zero centered normal
    p.y += dy;

    obs[it->first] = p;

  }

}

vector<char> Serialization::serialize(const Parameters &pars, const vector<string> &par_names_vec, const Observations &obs, const vector<string> &obs_names_vec)
{

	assert(pars.size() == par_names_vec.size());
	assert(obs.size() == obs_names_vec.size());
	vector<char> serial_data;
	size_t npar = par_names_vec.size();
	size_t nobs = obs_names_vec.size();
	size_t par_buf_sz = npar * sizeof(double);
	size_t obs_buf_sz = nobs * sizeof(double);
	serial_data.resize(par_buf_sz + obs_buf_sz, Parameters::no_data);

	char *buf = &serial_data[0];
	vector<double> par_data = pars.get_data_vec(par_names_vec);
	w_memcpy_s(buf, par_buf_sz, &par_data[0], par_data.size() * sizeof(double));

	vector<double> obs_data = obs.get_data_vec(obs_names_vec);
	w_memcpy_s(buf+par_buf_sz, obs_buf_sz, &obs_data[0], obs_data.size() * sizeof(double));
	return serial_data;
}

 void SensorArray::getObservations(Observations& observations)
 {
     std::vector<SensorPtr>::iterator sensIter;
     size_t i = 0;
     for (sensIter = sensors.begin(); sensIter != sensors.end(); ++sensIter) 
     {
         AssertMsg(i < observations.size(), "There are more built-in sensors than observations in AgentInitInfo");
         SimEntitySet::const_iterator entIter;
         const SimEntitySet entSet = Kernel::instance().GetSimContext()->getSimulation()->GetEntities((*sensIter)->getTypes());
         for (entIter = entSet.begin(); entIter != entSet.end(); ++entIter) 
         {
             (*sensIter)->process(GetEntity(), (*entIter));
         }
         observations[i] = (*sensIter)->getObservation(GetEntity());
         i++;
     }
 }

Point3 Triangulate(const Observations& observations, double* error, bool optimize)
{
    const int num_points = static_cast<int>(observations.size());

    Mat A{2 * num_points, 3};
    Vec b{2 * num_points};
    Vec x{3};

    for (int i = 0; i < num_points; i++)
    {
        A.row(2 * i + 0) = observations[i].m_R.row(0) - observations[i].m_point.x() * observations[i].m_R.row(2);
        A.row(2 * i + 1) = observations[i].m_R.row(1) - observations[i].m_point.y() * observations[i].m_R.row(2);

        b(2 * i + 0) = observations[i].m_t(2) * observations[i].m_point.x() - observations[i].m_t(0);
        b(2 * i + 1) = observations[i].m_t(2) * observations[i].m_point.y() - observations[i].m_t(1);
    }

    // Find the least squares result
    x = A.colPivHouseholderQr().solve(b);

    TriangulationResidual functor{observations};

    // Run a non-linear optimization to refine the result
    if (optimize)
    {
        Eigen::NumericalDiff<TriangulationResidual> numDiff{functor};
        Eigen::LevenbergMarquardt<Eigen::NumericalDiff<TriangulationResidual>> lm{numDiff};

        lm.parameters.ftol   = 1.0e-5;
        lm.parameters.xtol   = 1.0e-5;
        lm.parameters.maxfev = 200;

        const auto status = lm.minimize(x);
    }

    if (error != nullptr)
    {
        Vec residuals{2 * num_points};
        functor(x, residuals);
        *error = residuals.squaredNorm();
    }

    return x;
}

 /// Triangulate a given track from a selection of observations
 Vec3 track_sample_triangulation(
   const SfM_Data & sfm_data,
   const Observations & obs,
   const std::set<IndexT> & samples) const
 {
   Triangulation trianObj;
   for (auto& it : samples)
   {
     const IndexT & idx = it;
     Observations::const_iterator itObs = obs.begin();
     std::advance(itObs, idx);
     const View * view = sfm_data.views.at(itObs->first).get();
     const IntrinsicBase * cam = sfm_data.getIntrinsics().at(view->id_intrinsic).get();
     const Pose3 & pose = sfm_data.poses.at(view->id_pose);
     trianObj.add(
       cam->get_projective_equivalent(pose),
       cam->get_ud_pixel(itObs->second.x));
   }
   return trianObj.compute();
 }

int  RunStorage::get_observations(int run_id, Observations &obs)
{
	std::int8_t r_status;
	vector<char> info_txt_buf;
	info_txt_buf.resize(info_txt_length, '\0');
	double info_value;

	check_rec_id(run_id);

	size_t n_par = par_names.size();
	size_t n_obs = obs_names.size();
	vector<double> obs_data;
	obs_data.resize(n_obs);
	buf_stream.seekg(get_stream_pos(run_id), ios_base::beg);
	buf_stream.read(reinterpret_cast<char*>(&r_status), sizeof(r_status));
	buf_stream.read(reinterpret_cast<char*>(&info_txt_buf[0]), sizeof(char)*info_txt_length);
	buf_stream.read(reinterpret_cast<char*>(&info_value), sizeof(double));
	buf_stream.seekg(n_par*sizeof(double), ios_base::cur);
	buf_stream.read(reinterpret_cast<char*>(obs_data.data()), n_obs*sizeof(double));
	int status = r_status;
	obs.update(obs_names, obs_data);
	return status;
}