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