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