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