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