C++ Observations::size方法代码示例

 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;
}

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;
}

  /// Robustly try to estimate the best 3D point using a ransac Scheme
  /// Return true for a successful triangulation
  bool robust_triangulation(
    const SfM_Data & sfm_data,
    const Observations & obs,
    Vec3 & X,
    const IndexT min_required_inliers = 3,
    const IndexT min_sample_index = 3) const
  {
    const double dThresholdPixel = 4.0; // TODO: make this parameter customizable

    const IndexT nbIter = obs.size(); // TODO: automatic computation of the number of iterations?

    // - Ransac variables
    Vec3 best_model;
    std::set<IndexT> best_inlier_set;
    double best_error = std::numeric_limits<double>::max();

    // - Ransac loop
    for (IndexT i = 0; i < nbIter; ++i)
    {
      std::vector<size_t> vec_samples;
      robust::UniformSample(min_sample_index, obs.size(), &vec_samples);
      const std::set<IndexT> samples(vec_samples.begin(), vec_samples.end());

      // Hypothesis generation.
      const Vec3 current_model = track_sample_triangulation(sfm_data, obs, samples);

      // Test validity of the hypothesis
      // - chierality (for the samples)
      // - residual error

      // Chierality (Check the point is in front of the sampled cameras)
      bool bChierality = true;
      for (auto& it : samples){
        Observations::const_iterator itObs = obs.begin();
        std::advance(itObs, it);
      	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);
        const double z = pose.depth(current_model); // TODO: cam->depth(pose(X));
        bChierality &= z > 0;
      }

      if (!bChierality)
        continue;

      std::set<IndexT> inlier_set;
      double current_error = 0.0;
      // Classification as inlier/outlier according pixel residual errors.
      for (Observations::const_iterator itObs = obs.begin();
          itObs != obs.end(); ++itObs)
      {
        const View * view = sfm_data.views.at(itObs->first).get();
        const IntrinsicBase * intrinsic = sfm_data.getIntrinsics().at(view->id_intrinsic).get();
        const Pose3 & pose = sfm_data.poses.at(view->id_pose);
        const Vec2 residual = intrinsic->residual(pose, current_model, itObs->second.x);
        const double residual_d = residual.norm();
        if (residual_d < dThresholdPixel)
        {
          inlier_set.insert(itObs->first);
          current_error += residual_d;
        }
        else
        {
          current_error += dThresholdPixel;
        }
      }
      // Does the hypothesis is the best one we have seen and have sufficient inliers.
      if (current_error < best_error && inlier_set.size() >= min_required_inliers)
      {
        X = best_model = current_model;
        best_inlier_set = inlier_set;
        best_error = current_error;
      }
    }
    return !best_inlier_set.empty();
  }