void
pcl::WorldModel<PointT>::setSliceAsNans (const double origin_x, const double origin_y, const double origin_z, const double offset_x, const double offset_y, const double offset_z, const int size_x, const int size_y, const int size_z)
{
PCL_DEBUG ("IN SETSLICE AS NANS\n");
PointCloudPtr slice (new pcl::PointCloud<PointT>);
// prepare filter limits on all dimensions
double previous_origin_x = origin_x;
double previous_limit_x = origin_x + size_x - 1;
double new_origin_x = origin_x + offset_x;
double new_limit_x = previous_limit_x + offset_x;
double previous_origin_y = origin_y;
double previous_limit_y = origin_y + size_y - 1;
double new_origin_y = origin_y + offset_y;
double new_limit_y = previous_limit_y + offset_y;
double previous_origin_z = origin_z;
double previous_limit_z = origin_z + size_z - 1;
double new_origin_z = origin_z + offset_z;
double new_limit_z = previous_limit_z + offset_z;
// get points of slice on X (we actually set a negative filter and set the ouliers (so, our slice points) to nan)
double lower_limit_x, upper_limit_x;
if(offset_x >=0)
{
lower_limit_x = previous_origin_x;
upper_limit_x = new_origin_x;
}
else
{
lower_limit_x = new_limit_x;
upper_limit_x = previous_limit_x;
}
PCL_DEBUG ("Limit X: [%f - %f]\n", lower_limit_x, upper_limit_x);
ConditionOrPtr range_cond_OR_x (new pcl::ConditionOr<PointT> ());
range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, upper_limit_x ))); // filtered dimension
range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, lower_limit_x ))); // filtered dimension
range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_limit_y)));
range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y )));
range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_limit_z)));
range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z )));
pcl::ConditionalRemoval<PointT> condrem_x (range_cond_OR_x, true);
condrem_x.setInputCloud (world_);
condrem_x.setKeepOrganized (false);
// apply filter
condrem_x.filter (*slice);
IndicesConstPtr indices_x = condrem_x.getRemovedIndices ();
//set outliers (so our slice points) to nan
setIndicesAsNans(world_, indices_x);
PCL_DEBUG("%d points set to nan on X\n", indices_x->size ());
// get points of slice on Y (we actually set a negative filter and set the ouliers (so, our slice points) to nan)
double lower_limit_y, upper_limit_y;
if(offset_y >=0)
{
lower_limit_y = previous_origin_y;
upper_limit_y = new_origin_y;
}
else
{
lower_limit_y = new_limit_y;
upper_limit_y = previous_limit_y;
}
PCL_DEBUG ("Limit Y: [%f - %f]\n", lower_limit_y, upper_limit_y);
ConditionOrPtr range_cond_OR_y (new pcl::ConditionOr<PointT> ());
range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_limit_x )));
range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x )));
range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, upper_limit_y))); // filtered dimension
range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, lower_limit_y ))); // filtered dimension
range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_limit_z)));
range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z )));
pcl::ConditionalRemoval<PointT> condrem_y (range_cond_OR_y, true);
condrem_y.setInputCloud (world_);
condrem_y.setKeepOrganized (false);
// apply filter
condrem_y.filter (*slice);
IndicesConstPtr indices_y = condrem_y.getRemovedIndices ();
//set outliers (so our slice points) to nan
setIndicesAsNans(world_, indices_y);
PCL_DEBUG ("%d points set to nan on Y\n", indices_y->size ());
// get points of slice on Z (we actually set a negative filter and set the ouliers (so, our slice points) to nan)
double lower_limit_z, upper_limit_z;
if(offset_z >=0)
{
lower_limit_z = previous_origin_z;
upper_limit_z = new_origin_z;
}
else
{
lower_limit_z = new_limit_z;
upper_limit_z = previous_limit_z;
}
PCL_DEBUG ("Limit Z: [%f - %f]\n", lower_limit_z, upper_limit_z);
ConditionOrPtr range_cond_OR_z (new pcl::ConditionOr<PointT> ());
range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_limit_x )));
range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x )));
range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_limit_y)));
range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y )));
range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, upper_limit_z))); // filtered dimension
range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, lower_limit_z ))); // filtered dimension
pcl::ConditionalRemoval<PointT> condrem_z (range_cond_OR_z, true);
condrem_z.setInputCloud (world_);
condrem_z.setKeepOrganized (false);
// apply filter
condrem_z.filter (*slice);
IndicesConstPtr indices_z = condrem_z.getRemovedIndices ();
//set outliers (so our slice points) to nan
setIndicesAsNans(world_, indices_z);
PCL_DEBUG("%d points set to nan on Z\n", indices_z->size ());
}
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
PointCloudSource &output, const Matrix4 &guess)
{
// Point cloud containing the correspondences of each point in <input, indices>
PointCloudSourcePtr input_transformed (new PointCloudSource);
nr_iterations_ = 0;
converged_ = false;
// Initialise final transformation to the guessed one
final_transformation_ = guess;
// If the guessed transformation is non identity
if (guess != Matrix4::Identity ())
{
input_transformed->resize (input_->size ());
// Apply guessed transformation prior to search for neighbours
transformCloud (*input_, *input_transformed, guess);
}
else
*input_transformed = *input_;
transformation_ = Matrix4::Identity ();
// Make blobs if necessary
determineRequiredBlobData ();
PCLPointCloud2::Ptr target_blob (new PCLPointCloud2);
if (need_target_blob_)
pcl::toPCLPointCloud2 (*target_, *target_blob);
// Pass in the default target for the Correspondence Estimation/Rejection code
correspondence_estimation_->setInputTarget (target_);
if (correspondence_estimation_->requiresTargetNormals ())
correspondence_estimation_->setTargetNormals (target_blob);
// Correspondence Rejectors need a binary blob
for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
{
registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
if (rej->requiresTargetPoints ())
rej->setTargetPoints (target_blob);
if (rej->requiresTargetNormals () && target_has_normals_)
rej->setTargetNormals (target_blob);
}
convergence_criteria_->setMaximumIterations (max_iterations_);
convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
// Repeat until convergence
do
{
// Get blob data if needed
PCLPointCloud2::Ptr input_transformed_blob;
if (need_source_blob_)
{
input_transformed_blob.reset (new PCLPointCloud2);
toPCLPointCloud2 (*input_transformed, *input_transformed_blob);
}
// Save the previously estimated transformation
previous_transformation_ = transformation_;
// Set the source each iteration, to ensure the dirty flag is updated
correspondence_estimation_->setInputSource (input_transformed);
if (correspondence_estimation_->requiresSourceNormals ())
correspondence_estimation_->setSourceNormals (input_transformed_blob);
// Estimate correspondences
if (use_reciprocal_correspondence_)
correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
else
correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);
//if (correspondence_rejectors_.empty ())
CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_));
for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
{
registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ());
if (rej->requiresSourcePoints ())
rej->setSourcePoints (input_transformed_blob);
if (rej->requiresSourceNormals () && source_has_normals_)
rej->setSourceNormals (input_transformed_blob);
rej->setInputCorrespondences (temp_correspondences);
rej->getCorrespondences (*correspondences_);
// Modify input for the next iteration
if (i < correspondence_rejectors_.size () - 1)
*temp_correspondences = *correspondences_;
}
size_t cnt = correspondences_->size ();
// Check whether we have enough correspondences
if (static_cast<int> (cnt) < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria<Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
converged_ = false;
break;
}
// Estimate the transform
transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);
// Tranform the data
transformCloud (*input_transformed, *input_transformed, transformation_);
// Obtain the final transformation
final_transformation_ = transformation_ * final_transformation_;
++nr_iterations_;
// Update the vizualization of icp convergence
//if (update_visualizer_ != 0)
// update_visualizer_(output, source_indices_good, *target_, target_indices_good );
converged_ = static_cast<bool> ((*convergence_criteria_));
}
while (!converged_);
// Transform the input cloud using the final transformation
PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n",
final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
// Copy all the values
output = *input_;
// Transform the XYZ + normals
transformCloud (*input_, output, final_transformation_);
}
template <typename T> bool
pcl::visualization::ImageViewer::addRectangle (
const typename pcl::PointCloud<T>::ConstPtr &image,
const T &min_pt,
const T &max_pt,
double r, double g, double b,
const std::string &layer_id, double opacity)
{
// We assume that the data passed into image is organized, otherwise this doesn't make sense
if (!image->isOrganized ())
return (false);
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
if (am_it == layer_map_.end ())
{
PCL_DEBUG ("[pcl::visualization::ImageViewer::addRectangle] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, true);
}
// Construct a search object to get the camera parameters
pcl::search::OrganizedNeighbor<T> search;
search.setInputCloud (image);
// Project the 8 corners
T p1, p2, p3, p4, p5, p6, p7, p8;
p1.x = min_pt.x; p1.y = min_pt.y; p1.z = min_pt.z;
p2.x = min_pt.x; p2.y = min_pt.y; p2.z = max_pt.z;
p3.x = min_pt.x; p3.y = max_pt.y; p3.z = min_pt.z;
p4.x = min_pt.x; p4.y = max_pt.y; p4.z = max_pt.z;
p5.x = max_pt.x; p5.y = min_pt.y; p5.z = min_pt.z;
p6.x = max_pt.x; p6.y = min_pt.y; p6.z = max_pt.z;
p7.x = max_pt.x; p7.y = max_pt.y; p7.z = min_pt.z;
p8.x = max_pt.x; p8.y = max_pt.y; p8.z = max_pt.z;
std::vector<pcl::PointXY> pp_2d (8);
search.projectPoint (p1, pp_2d[0]);
search.projectPoint (p2, pp_2d[1]);
search.projectPoint (p3, pp_2d[2]);
search.projectPoint (p4, pp_2d[3]);
search.projectPoint (p5, pp_2d[4]);
search.projectPoint (p6, pp_2d[5]);
search.projectPoint (p7, pp_2d[6]);
search.projectPoint (p8, pp_2d[7]);
pcl::PointXY min_pt_2d, max_pt_2d;
min_pt_2d.x = min_pt_2d.y = std::numeric_limits<float>::max ();
max_pt_2d.x = max_pt_2d.y = std::numeric_limits<float>::min ();
// Search for the two extrema
for (size_t i = 0; i < pp_2d.size (); ++i)
{
if (pp_2d[i].x < min_pt_2d.x) min_pt_2d.x = pp_2d[i].x;
if (pp_2d[i].y < min_pt_2d.y) min_pt_2d.y = pp_2d[i].y;
if (pp_2d[i].x > max_pt_2d.x) max_pt_2d.x = pp_2d[i].x;
if (pp_2d[i].y > max_pt_2d.y) max_pt_2d.y = pp_2d[i].y;
}
#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION < 10))
min_pt_2d.y = float (image->height) - min_pt_2d.y;
max_pt_2d.y = float (image->height) - max_pt_2d.y;
#endif
vtkSmartPointer<context_items::Rectangle> rect = vtkSmartPointer<context_items::Rectangle>::New ();
rect->setColors (static_cast<unsigned char> (255.0 * r),
static_cast<unsigned char> (255.0 * g),
static_cast<unsigned char> (255.0 * b));
rect->setOpacity (opacity);
rect->set (min_pt_2d.x, min_pt_2d.y, (max_pt_2d.x - min_pt_2d.x), (max_pt_2d.y - min_pt_2d.y));
am_it->actor->GetScene ()->AddItem (rect);
return (true);
}
template <typename PointFeature> float
pcl::PyramidFeatureHistogram<PointFeature>::comparePyramidFeatureHistograms (const PyramidFeatureHistogramPtr &pyramid_a,
const PyramidFeatureHistogramPtr &pyramid_b)
{
// do a few consistency checks before and during the computation
if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions)
{
PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of dimensions: %u vs %u\n", pyramid_a->nr_dimensions, pyramid_b->nr_dimensions);
return -1;
}
if (pyramid_a->nr_levels != pyramid_b->nr_levels)
{
PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of levels: %u vs %u\n", pyramid_a->nr_levels, pyramid_b->nr_levels);
return -1;
}
// calculate for level 0 first
if (pyramid_a->hist_levels[0].hist.size () != pyramid_b->hist_levels[0].hist.size ())
{
PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of bins on level 0: %u vs %u\n", pyramid_a->hist_levels[0].hist.size (), pyramid_b->hist_levels[0].hist.size ());
return -1;
}
float match_count_level = 0.0f, match_count_prev_level = 0.0f;
for (size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size (); ++bin_i)
{
if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i])
match_count_level += pyramid_a->hist_levels[0].hist[bin_i];
else
match_count_level += pyramid_b->hist_levels[0].hist[bin_i];
}
float match_count = match_count_level;
for (size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i)
{
if (pyramid_a->hist_levels[level_i].hist.size () != pyramid_b->hist_levels[level_i].hist.size ())
{
PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of bins on level %u: %u vs %u\n", level_i, pyramid_a->hist_levels[level_i].hist.size (), pyramid_b->hist_levels[level_i].hist.size ());
return -1;
}
match_count_prev_level = match_count_level;
match_count_level = 0.0f;
for (size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size (); ++bin_i)
{
if (pyramid_a->hist_levels[level_i].hist[bin_i] < pyramid_b->hist_levels[level_i].hist[bin_i])
match_count_level += pyramid_a->hist_levels[level_i].hist[bin_i];
else
match_count_level += pyramid_b->hist_levels[level_i].hist[bin_i];
}
float level_normalization_factor = pow(2.0f, (int) level_i);
match_count += (match_count_level - match_count_prev_level) / level_normalization_factor;
}
// include self-similarity factors
float self_similarity_a = pyramid_a->nr_features,
self_similarity_b = pyramid_b->nr_features;
PCL_DEBUG ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self similarity measures: %f, %f\n", self_similarity_a, self_similarity_b);
match_count /= sqrt (self_similarity_a * self_similarity_b);
return match_count;
}
template <typename PointT> bool
pcl::RandomizedMEstimatorSampleConsensus<PointT>::computeModel (int debug_verbosity_level)
{
// Warn and exit if no threshold was set
if (threshold_ == DBL_MAX)
{
PCL_ERROR ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] No threshold set!\n");
return (false);
}
iterations_ = 0;
double d_best_penalty = DBL_MAX;
double k = 1.0;
std::vector<int> best_model;
std::vector<int> selection;
Eigen::VectorXf model_coefficients;
std::vector<double> distances;
std::set<int> indices_subset;
int n_inliers_count = 0;
// Number of samples to try randomly
size_t fraction_nr_points = pcl_lrint (sac_model_->getIndices ()->size () * fraction_nr_pretest_ / 100.0);
// Iterate
while (iterations_ < k)
{
// Get X samples which satisfy the model criteria
sac_model_->getSamples (iterations_, selection);
if (selection.empty ()) break;
// Search for inliers in the point cloud for the current plane model M
if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
{
//iterations_++;
continue;
}
// RMSAC addon: verify a random fraction of the data
// Get X random samples which satisfy the model criterion
this->getRandomSamples (sac_model_->getIndices (), fraction_nr_points, indices_subset);
if (!sac_model_->doSamplesVerifyModel (indices_subset, model_coefficients, threshold_))
{
// Unfortunately we cannot "continue" after the first iteration, because k might not be set, while iterations gets incremented
if (k != 1.0)
{
++iterations_;
continue;
}
}
double d_cur_penalty = 0;
// Iterate through the 3d points and calculate the distances from them to the model
sac_model_->getDistancesToModel (model_coefficients, distances);
if (distances.empty () && k > 1.0)
continue;
for (size_t i = 0; i < distances.size (); ++i)
d_cur_penalty += (std::min) (distances[i], threshold_);
// Better match ?
if (d_cur_penalty < d_best_penalty)
{
d_best_penalty = d_cur_penalty;
// Save the current model/coefficients selection as being the best so far
model_ = selection;
model_coefficients_ = model_coefficients;
n_inliers_count = 0;
// Need to compute the number of inliers for this model to adapt k
for (size_t i = 0; i < distances.size (); ++i)
if (distances[i] <= threshold_)
n_inliers_count++;
// Compute the k parameter (k=log(z)/log(1-w^n))
double w = (double)((double)n_inliers_count / (double)sac_model_->getIndices ()->size ());
double p_no_outliers = 1 - pow (w, (double)selection.size ());
p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by -Inf
p_no_outliers = (std::min) (1 - std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by 0.
k = log (1 - probability_) / log (p_no_outliers);
}
++iterations_;
if (debug_verbosity_level > 1)
PCL_DEBUG ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_, (int)ceil (k), d_best_penalty);
if (iterations_ > max_iterations_)
{
if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] MSAC reached the maximum number of trials.\n");
break;
}
}
if (model_.empty ())
{
if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Unable to find a solution!\n");
return (false);
}
// Iterate through the 3d points and calculate the distances from them to the model again
sac_model_->getDistancesToModel (model_coefficients_, distances);
std::vector<int> &indices = *sac_model_->getIndices ();
if (distances.size () != indices.size ())
{
PCL_ERROR ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", (unsigned long)distances.size (), (unsigned long)indices.size ());
return (false);
}
inliers_.resize (distances.size ());
// Get the inliers for the best model found
n_inliers_count = 0;
for (size_t i = 0; i < distances.size (); ++i)
if (distances[i] <= threshold_)
inliers_[n_inliers_count++] = indices[i];
// Resize the inliers vector
inliers_.resize (n_inliers_count);
if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", (unsigned long)model_.size (), n_inliers_count);
return (true);
}
template <typename PointT> void
pcl::SACSegmentation<PointT>::initSAC (const int method_type)
{
if (sac_)
sac_.reset ();
// Build the sample consensus method
switch (method_type)
{
case SAC_RANSAC:
default:
{
PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
sac_.reset (new RandomSampleConsensus<PointT> (model_, threshold_));
break;
}
case SAC_LMEDS:
{
PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_LMEDS with a model threshold of %f\n", getClassName ().c_str (), threshold_);
sac_.reset (new LeastMedianSquares<PointT> (model_, threshold_));
break;
}
case SAC_MSAC:
{
PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
sac_.reset (new MEstimatorSampleConsensus<PointT> (model_, threshold_));
break;
}
case SAC_RRANSAC:
{
PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RRANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
sac_.reset (new RandomizedRandomSampleConsensus<PointT> (model_, threshold_));
break;
}
case SAC_RMSAC:
{
PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RMSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
sac_.reset (new RandomizedMEstimatorSampleConsensus<PointT> (model_, threshold_));
break;
}
case SAC_MLESAC:
{
PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MLESAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
sac_.reset (new MaximumLikelihoodSampleConsensus<PointT> (model_, threshold_));
break;
}
case SAC_PROSAC:
{
PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_PROSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
sac_.reset (new ProgressiveSampleConsensus<PointT> (model_, threshold_));
break;
}
}
// Set the Sample Consensus parameters if they are given/changed
if (sac_->getProbability () != probability_)
{
PCL_DEBUG ("[pcl::%s::initSAC] Setting the desired probability to %f\n", getClassName ().c_str (), probability_);
sac_->setProbability (probability_);
}
if (max_iterations_ != -1 && sac_->getMaxIterations () != max_iterations_)
{
PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum number of iterations to %d\n", getClassName ().c_str (), max_iterations_);
sac_->setMaxIterations (max_iterations_);
}
}
template <typename PointXYZT, typename PointRGBT> bool
pcl::LineRGBD<PointXYZT, PointRGBT>::loadTemplates (const std::string &file_name, const size_t object_id)
{
// Open the file
int ltm_fd = pcl_open (file_name.c_str (), O_RDONLY);
if (ltm_fd == -1)
return (false);
int ltm_offset = 0;
pcl::io::TARHeader ltm_header;
PCDReader pcd_reader;
std::string pcd_ext (".pcd");
std::string sqmmt_ext (".sqmmt");
// While there still is an LTM header to be read
while (readLTMHeader (ltm_fd, ltm_header))
{
ltm_offset += 512;
// Search for extension
std::string chunk_name (ltm_header.file_name);
std::transform (chunk_name.begin (), chunk_name.end (), chunk_name.begin (), tolower);
std::string::size_type it;
if ((it = chunk_name.find (pcd_ext)) != std::string::npos &&
(pcd_ext.size () - (chunk_name.size () - it)) == 0)
{
PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a PCD file.\n", chunk_name.c_str ());
// Read the next PCD file
template_point_clouds_.resize (template_point_clouds_.size () + 1);
pcd_reader.read (file_name, template_point_clouds_[template_point_clouds_.size () - 1], ltm_offset);
// Increment the offset for the next file
ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512);
}
else if ((it = chunk_name.find (sqmmt_ext)) != std::string::npos &&
(sqmmt_ext.size () - (chunk_name.size () - it)) == 0)
{
PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a SQMMT file.\n", chunk_name.c_str ());
unsigned int fsize = ltm_header.getFileSize ();
char *buffer = new char[fsize];
int result = static_cast<int> (::read (ltm_fd, reinterpret_cast<char*> (&buffer[0]), fsize));
if (result == -1)
{
delete [] buffer;
PCL_ERROR ("[pcl::LineRGBD::loadTemplates] Error reading SQMMT template from file!\n");
break;
}
// Read a SQMMT file
std::stringstream stream (std::stringstream::in | std::stringstream::out | std::stringstream::binary);
stream.write (buffer, fsize);
SparseQuantizedMultiModTemplate sqmmt;
sqmmt.deserialize (stream);
linemod_.addTemplate (sqmmt);
object_ids_.push_back (object_id);
// Increment the offset for the next file
ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512);
delete [] buffer;
}
if (static_cast<int> (pcl_lseek (ltm_fd, ltm_offset, SEEK_SET)) < 0)
break;
}
// Close the file
pcl_close (ltm_fd);
// Compute 3D bounding boxes from the template point clouds
bounding_boxes_.resize (template_point_clouds_.size ());
for (size_t i = 0; i < template_point_clouds_.size (); ++i)
{
PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i];
BoundingBoxXYZ & bb = bounding_boxes_[i];
bb.x = bb.y = bb.z = std::numeric_limits<float>::max ();
bb.width = bb.height = bb.depth = 0.0f;
float center_x = 0.0f;
float center_y = 0.0f;
float center_z = 0.0f;
float min_x = std::numeric_limits<float>::max ();
float min_y = std::numeric_limits<float>::max ();
float min_z = std::numeric_limits<float>::max ();
float max_x = -std::numeric_limits<float>::max ();
float max_y = -std::numeric_limits<float>::max ();
float max_z = -std::numeric_limits<float>::max ();
size_t counter = 0;
for (size_t j = 0; j < template_point_cloud.size (); ++j)
{
const PointXYZRGBA & p = template_point_cloud.points[j];
if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
continue;
min_x = std::min (min_x, p.x);
min_y = std::min (min_y, p.y);
min_z = std::min (min_z, p.z);
max_x = std::max (max_x, p.x);
max_y = std::max (max_y, p.y);
max_z = std::max (max_z, p.z);
center_x += p.x;
center_y += p.y;
center_z += p.z;
++counter;
}
center_x /= static_cast<float> (counter);
center_y /= static_cast<float> (counter);
center_z /= static_cast<float> (counter);
bb.width = max_x - min_x;
bb.height = max_y - min_y;
bb.depth = max_z - min_z;
bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f;
bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f;
bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f;
for (size_t j = 0; j < template_point_cloud.size (); ++j)
{
PointXYZRGBA p = template_point_cloud.points[j];
if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
continue;
p.x -= center_x;
p.y -= center_y;
p.z -= center_z;
template_point_cloud.points[j] = p;
}
}
return (true);
}
template <typename PointT> void
pcl::ApproximateProgressiveMorphologicalFilter<PointT>::extract (std::vector<int>& ground)
{
bool segmentation_is_possible = initCompute ();
if (!segmentation_is_possible)
{
deinitCompute ();
return;
}
// Compute the series of window sizes and height thresholds
std::vector<float> height_thresholds;
std::vector<float> window_sizes;
std::vector<int> half_sizes;
int iteration = 0;
int half_size = 0.0f;
float window_size = 0.0f;
float height_threshold = 0.0f;
while (window_size < max_window_size_)
{
// Determine the initial window size.
if (exponential_)
half_size = static_cast<int> (std::pow (static_cast<float> (base_), iteration));
else
half_size = (iteration+1) * base_;
window_size = 2 * half_size + 1;
// Calculate the height threshold to be used in the next iteration.
if (iteration == 0)
height_threshold = initial_distance_;
else
height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_;
// Enforce max distance on height threshold
if (height_threshold > max_distance_)
height_threshold = max_distance_;
half_sizes.push_back (half_size);
window_sizes.push_back (window_size);
height_thresholds.push_back (height_threshold);
iteration++;
}
// setup grid based on scale and extents
Eigen::Vector4f global_max, global_min;
pcl::getMinMax3D<PointT> (*input_, global_min, global_max);
float xextent = global_max.x () - global_min.x ();
float yextent = global_max.y () - global_min.y ();
int rows = static_cast<int> (std::floor (yextent / cell_size_) + 1);
int cols = static_cast<int> (std::floor (xextent / cell_size_) + 1);
Eigen::MatrixXf A (rows, cols);
A.setConstant (std::numeric_limits<float>::quiet_NaN ());
Eigen::MatrixXf Z (rows, cols);
Z.setConstant (std::numeric_limits<float>::quiet_NaN ());
Eigen::MatrixXf Zf (rows, cols);
Zf.setConstant (std::numeric_limits<float>::quiet_NaN ());
#ifdef _OPENMP
#pragma omp parallel for num_threads(threads_)
#endif
for (int i = 0; i < input_->points.size (); ++i)
{
// ...then test for lower points within the cell
PointT p = input_->points[i];
int row = std::floor(p.y - global_min.y ());
int col = std::floor(p.x - global_min.x ());
if (p.z < A (row, col) || pcl_isnan (A (row, col)))
{
A (row, col) = p.z;
}
}
// Ground indices are initially limited to those points in the input cloud we
// wish to process
ground = *indices_;
// Progressively filter ground returns using morphological open
for (int i = 0; i < window_sizes.size (); ++i)
{
PCL_DEBUG (" Iteration %d (height threshold = %f, window size = %f, half size = %d)...",
i, height_thresholds[i], window_sizes[i], half_sizes[i]);
// Limit filtering to those points currently considered ground returns
typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
// Apply the morphological opening operation at the current window size.
#ifdef _OPENMP
#pragma omp parallel for num_threads(threads_)
#endif
for (int row = 0; row < rows; ++row)
{
int rs, re;
rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
for (int col = 0; col < cols; ++col)
{
int cs, ce;
cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
float min_coeff = std::numeric_limits<float>::max ();
for (int j = rs; j < (re + 1); ++j)
{
for (int k = cs; k < (ce + 1); ++k)
{
if (A (j, k) != std::numeric_limits<float>::quiet_NaN ())
{
if (A (j, k) < min_coeff)
min_coeff = A (j, k);
}
}
}
if (min_coeff != std::numeric_limits<float>::max ())
Z(row, col) = min_coeff;
}
}
#ifdef _OPENMP
#pragma omp parallel for num_threads(threads_)
#endif
for (int row = 0; row < rows; ++row)
{
int rs, re;
rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
for (int col = 0; col < cols; ++col)
{
int cs, ce;
cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
float max_coeff = -std::numeric_limits<float>::max ();
for (int j = rs; j < (re + 1); ++j)
{
for (int k = cs; k < (ce + 1); ++k)
{
if (Z (j, k) != std::numeric_limits<float>::quiet_NaN ())
{
if (Z (j, k) > max_coeff)
max_coeff = Z (j, k);
}
}
}
if (max_coeff != -std::numeric_limits<float>::max ())
Zf (row, col) = max_coeff;
}
}
// Find indices of the points whose difference between the source and
// filtered point clouds is less than the current height threshold.
std::vector<int> pt_indices;
for (boost::int32_t p_idx = 0; p_idx < ground.size (); ++p_idx)
{
PointT p = cloud->points[p_idx];
int erow = static_cast<int> (std::floor ((p.y - global_min.y ()) / cell_size_));
int ecol = static_cast<int> (std::floor ((p.x - global_min.x ()) / cell_size_));
float diff = p.z - Zf (erow, ecol);
if (diff < height_thresholds[i])
pt_indices.push_back (ground[p_idx]);
}
A.swap (Zf);
// Ground is now limited to pt_indices
ground.swap (pt_indices);
PCL_DEBUG ("ground now has %d points\n", ground.size ());
}
deinitCompute ();
}
template <typename PointSource, typename PointTarget> inline void
TransformationEstimationJointOptimize<PointSource, PointTarget>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const std::vector<int> &indices_src_dfp,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const std::vector<int> &indices_tgt,
const std::vector<int> &indices_tgt_dfp,
float alpha_arg,
Eigen::Matrix4f &transformation_matrix)
{
if (indices_src.size () != indices_tgt.size ())
{
PCL_ERROR ("[pcl::registration::TransformationEstimationJointOptimize::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)indices_tgt.size ());
return;
}
if (indices_src_dfp.size () != indices_tgt_dfp.size ())
{
PCL_ERROR ("[pcl::registration::TransformationEstimationJointOptimize::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src_dfp.size (), (unsigned long)indices_tgt_dfp.size ());
return;
}
if ( (indices_src.size () + indices_tgt_dfp.size () )< 4) // need at least 4 samples
{
PCL_ERROR ("[pcl::registration::TransformationEstimationJointOptimize] ");
PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %lu points!",
(unsigned long)indices_src.size ());
return;
}
// If no warp function has been set, use the default (WarpPointRigid6D)
if (!warp_point_)
warp_point_.reset (new pcl::WarpPointRigid6D<PointSource, PointTarget>);
int n_unknowns = warp_point_->getDimension (); // get dimension of unknown space
int num_p = indices_src.size ();
int num_dfp = indices_src_dfp.size ();
Eigen::VectorXd x(n_unknowns);
x.setConstant (n_unknowns, 0);
// Set temporary pointers
tmp_src_ = &cloud_src;
tmp_tgt_ = &cloud_tgt;
tmp_idx_src_ = &indices_src;
tmp_idx_tgt_ = &indices_tgt;
tmp_idx_src_dfp_ = &indices_src_dfp;
tmp_idx_tgt_dfp_ = &indices_tgt_dfp;
// TODO: CHANGE NUMBER OF VALUES TO NUM_P + NUM_DFP
OptimizationFunctor functor (n_unknowns, 1, num_p, num_dfp, this); // Initialize functor
Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor); // Add derivative functionality
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor> > lm (num_diff);
int info = lm.minimize (x); // Minimize cost
// Compute the norm of the residuals
// PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
// PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
// PCL_DEBUG ("Final solution: [%f", x[0]);
std::cout << "[pcl::registration::TransformationEstimationJointOptimize::estimateRigidTransformation]" << std::endl;
std::cout << "LM solver finished with exit code " << info <<", having a residual norm of " << lm.fvec.norm () << std::endl;
std::cout << "Final solution: " << x[0] << std::endl;
for (int i = 1; i < n_unknowns; ++i)
PCL_DEBUG (" %f", x[i]);
PCL_DEBUG ("]\n");
// Return the correct transformation
Eigen::VectorXf params = x.cast<float> ();
warp_point_->setParam (params);
transformation_matrix = warp_point_->getTransform ();
tmp_src_ = NULL;
tmp_tgt_ = NULL;
tmp_idx_src_ = tmp_idx_tgt_ = NULL;
}
template <typename PointT> bool
pcl::RandomizedRandomSampleConsensus<PointT>::computeModel (int debug_verbosity_level)
{
// Warn and exit if no threshold was set
if (threshold_ == DBL_MAX)
{
PCL_ERROR ("[pcl::RandomizedRandomSampleConsensus::computeModel] No threshold set!\n");
return (false);
}
iterations_ = 0;
int n_best_inliers_count = -INT_MAX;
double k = 1.0;
std::vector<int> selection;
Eigen::VectorXf model_coefficients;
std::set<int> indices_subset;
int n_inliers_count = 0;
unsigned skipped_count = 0;
// supress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
const unsigned max_skip = max_iterations_ * 10;
// Number of samples to try randomly
size_t fraction_nr_points = pcl_lrint (sac_model_->getIndices ()->size () * fraction_nr_pretest_ / 100.0);
// Iterate
while (iterations_ < k && skipped_count < max_skip)
{
// Get X samples which satisfy the model criteria
sac_model_->getSamples (iterations_, selection);
if (selection.empty ()) break;
// Search for inliers in the point cloud for the current plane model M
if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
{
//iterations_++;
++ skipped_count;
continue;
}
// RRANSAC addon: verify a random fraction of the data
// Get X random samples which satisfy the model criterion
this->getRandomSamples (sac_model_->getIndices (), fraction_nr_points, indices_subset);
if (!sac_model_->doSamplesVerifyModel (indices_subset, model_coefficients, threshold_))
{
// Unfortunately we cannot "continue" after the first iteration, because k might not be set, while iterations gets incremented
if (k > 1.0)
{
++iterations_;
continue;
}
}
// Select the inliers that are within threshold_ from the model
n_inliers_count = sac_model_->countWithinDistance (model_coefficients, threshold_);
// Better match ?
if (n_inliers_count > n_best_inliers_count)
{
n_best_inliers_count = n_inliers_count;
// Save the current model/inlier/coefficients selection as being the best so far
model_ = selection;
model_coefficients_ = model_coefficients;
// Compute the k parameter (k=log(z)/log(1-w^n))
double w = (double)((double)n_inliers_count / (double)sac_model_->getIndices ()->size ());
double p_no_outliers = 1 - pow (w, (double)selection.size ());
p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by -Inf
p_no_outliers = (std::min) (1 - std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by 0.
k = log (1 - probability_) / log (p_no_outliers);
}
++iterations_;
if (debug_verbosity_level > 1)
PCL_DEBUG ("[pcl::RandomizedRandomSampleConsensus::computeModel] Trial %d out of %d: %d inliers (best is: %d so far).\n", iterations_, (int)ceil (k), n_inliers_count, n_best_inliers_count);
if (iterations_ > max_iterations_)
{
if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::RandomizedRandomSampleConsensus::computeModel] RRANSAC reached the maximum number of trials.\n");
break;
}
}
if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::RandomizedRandomSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", (unsigned long)model_.size (), n_best_inliers_count);
if (model_.empty ())
{
inliers_.clear ();
return (false);
}
// Get the set of inliers that correspond to the best model found so far
sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
return (true);
}
template <typename PointT> bool
pcl::ModelOutlierRemoval<PointT>::initSACModel (pcl::SacModel model_type)
{
// Build the model
switch (model_type)
{
case SACMODEL_PLANE:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPLANE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelPlane<PointT> (input_));
break;
}
case SACMODEL_LINE:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelLINE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelLine<PointT> (input_));
break;
}
case SACMODEL_CIRCLE2D:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelCIRCLE2D\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelCircle2D<PointT> (input_));
break;
}
case SACMODEL_SPHERE:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelSPHERE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelSphere<PointT> (input_));
break;
}
case SACMODEL_PARALLEL_LINE:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPARALLEL_LINE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelParallelLine<PointT> (input_));
break;
}
case SACMODEL_PERPENDICULAR_PLANE:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPERPENDICULAR_PLANE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelPerpendicularPlane<PointT> (input_));
break;
}
case SACMODEL_CYLINDER:
{
PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCYLINDER\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelCylinder<PointT, pcl::Normal> (input_));
break;
}
case SACMODEL_NORMAL_PLANE:
{
PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PLANE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelNormalPlane<PointT, pcl::Normal> (input_));
break;
}
case SACMODEL_CONE:
{
PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCONE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelCone<PointT, pcl::Normal> (input_));
break;
}
case SACMODEL_NORMAL_SPHERE:
{
PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_SPHERE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelNormalSphere<PointT, pcl::Normal> (input_));
break;
}
case SACMODEL_NORMAL_PARALLEL_PLANE:
{
PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelNormalParallelPlane<PointT, pcl::Normal> (input_));
break;
}
case SACMODEL_PARALLEL_PLANE:
{
PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelPARALLEL_PLANE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelParallelPlane<PointT> (input_));
break;
}
default:
{
PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
return (false);
}
}
return (true);
}
template <typename T> bool
pcl::visualization::ImageViewer::addRectangle (
const typename pcl::PointCloud<T>::ConstPtr &image,
const T &min_pt,
const T &max_pt,
const std::string &layer_id, double opacity)
{
// We assume that the data passed into image is organized, otherwise this doesn't make sense
if (!image->isOrganized ())
return (false);
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
if (am_it == layer_map_.end ())
{
PCL_DEBUG ("[pcl::visualization::ImageViewer::addRectangle] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
am_it = createLayer (layer_id, image_viewer_->GetRenderWindow ()->GetSize ()[0] - 1, image_viewer_->GetRenderWindow ()->GetSize ()[1] - 1, opacity, true);
}
// Construct a search object to get the camera parameters
pcl::search::OrganizedNeighbor<T> search;
search.setInputCloud (image);
// Project the 8 corners
T p1, p2, p3, p4, p5, p6, p7, p8;
p1.x = min_pt.x; p1.y = min_pt.y; p1.z = min_pt.z;
p2.x = min_pt.x; p2.y = min_pt.y; p2.z = max_pt.z;
p3.x = min_pt.x; p3.y = max_pt.y; p3.z = min_pt.z;
p4.x = min_pt.x; p4.y = max_pt.y; p4.z = max_pt.z;
p5.x = max_pt.x; p5.y = min_pt.y; p5.z = min_pt.z;
p6.x = max_pt.x; p6.y = min_pt.y; p6.z = max_pt.z;
p7.x = max_pt.x; p7.y = max_pt.y; p7.z = min_pt.z;
p8.x = max_pt.x; p8.y = max_pt.y; p8.z = max_pt.z;
std::vector<pcl::PointXY> pp_2d (8);
search.projectPoint (p1, pp_2d[0]);
search.projectPoint (p2, pp_2d[1]);
search.projectPoint (p3, pp_2d[2]);
search.projectPoint (p4, pp_2d[3]);
search.projectPoint (p5, pp_2d[4]);
search.projectPoint (p6, pp_2d[5]);
search.projectPoint (p7, pp_2d[6]);
search.projectPoint (p8, pp_2d[7]);
pcl::PointXY min_pt_2d, max_pt_2d;
min_pt_2d.x = min_pt_2d.y = std::numeric_limits<float>::max ();
max_pt_2d.x = max_pt_2d.y = std::numeric_limits<float>::min ();
// Search for the two extrema
for (size_t i = 0; i < pp_2d.size (); ++i)
{
if (pp_2d[i].x < min_pt_2d.x) min_pt_2d.x = pp_2d[i].x;
if (pp_2d[i].y < min_pt_2d.y) min_pt_2d.y = pp_2d[i].y;
if (pp_2d[i].x > max_pt_2d.x) max_pt_2d.x = pp_2d[i].x;
if (pp_2d[i].y > max_pt_2d.y) max_pt_2d.y = pp_2d[i].y;
}
min_pt_2d.y = float (image->height) - min_pt_2d.y;
max_pt_2d.y = float (image->height) - max_pt_2d.y;
am_it->canvas->SetDrawColor (0.0, 255.0, 0.0, opacity * 255.0);
am_it->canvas->DrawSegment (int (min_pt_2d.x), int (min_pt_2d.y), int (min_pt_2d.x), int (max_pt_2d.y));
am_it->canvas->DrawSegment (int (min_pt_2d.x), int (max_pt_2d.y), int (max_pt_2d.x), int (max_pt_2d.y));
am_it->canvas->DrawSegment (int (max_pt_2d.x), int (max_pt_2d.y), int (max_pt_2d.x), int (min_pt_2d.y));
am_it->canvas->DrawSegment (int (max_pt_2d.x), int (min_pt_2d.y), int (min_pt_2d.x), int (min_pt_2d.y));
return (true);
}
int PtuLaser::get3DScanWithIntensity(PointCloudXYZI::Ptr &cloud,
IplImage *& depth_img,
IplImage *& intensity_img,
const double step_angle,
const double pan_range,
const bool clockwise)
{
scans.clear();
signed short int offset = static_cast<signed short int> (step_angle / FULL_STEP_RES);
if (clockwise == false)
{
offset = -offset;
}
int afrt = laser.afrt();
long long int time_stamp_offset = laser.getHokuyoClockOffset(10);
laser.laserOn();
laser.requestScans(true, min_step, max_step, 1, 0, 0, -1);
int position = ptu::get_current(PAN, POSITION);
std::cout << "step number:" << abs(static_cast<int> ((pan_range / M_PI) * 180 / (FULL_STEP_RES
* static_cast<double > (offset)))) << std::endl;
for (int step = 0; step < abs(static_cast<int> ((pan_range / M_PI) * 180 / (FULL_STEP_RES
* static_cast<double > (offset))))+1; step++)
{
if ((status = ptu::set_desired(PAN, POSITION, (ptu::PTU_PARM_PTR *)&offset, RELATIVE)) == TRUE)
{
return -1;
}
if ((status = ptu::await_completion()) != PTU_OK)
{
return -1;
}
gettimeofday(&ptu_executed_time, NULL);
ptu_executed_time_stamp = (uint64_t)(ptu_executed_time.tv_sec) * 1000000000 + (uint64_t)(ptu_executed_time.tv_usec)
* 1000 + time_stamp_offset;
usleep(45000);
bool isScanLatterThanPtu = false;
while (!isScanLatterThanPtu)
{
laser.serviceScan(laser_scan);
if (ptu_executed_time_stamp < laser_scan.self_time_stamp)
{
isScanLatterThanPtu = true;
scans.push_back(laser_scan);
}
}
}
laser.stopScanning();
cloud->height = scans.size() * 2;
cloud->width = scans[0].ranges.size() / 2;
cloud->points.resize(cloud->height * cloud->width);
CvSize cs;
cs.height = cloud->height;
cs.width = cloud->width;
depth_img = cvCreateImage(cs,IPL_DEPTH_32F, 1);
intensity_img = cvCreateImage(cs,IPL_DEPTH_32F, 1);
for (int i = 0; i < static_cast<int> (scans.size()); i++)
{
double cos_1 = cos(-(position + i * offset) * FULL_STEP_RES * M_PI / 180);
double sin_1 = sin(-(position + i * offset) * FULL_STEP_RES * M_PI / 180);
double cos_2 = -cos_1;
double sin_2 = -sin_1;
for (int j = min_step; j < afrt; j++)
{
cloud->points[i * cloud->width + afrt - j - 1].x = scans[i].ranges[j] * sin((afrt - j) * 0.25 * M_PI / 180) * cos_1;
cloud->points[i * cloud->width + afrt - j - 1].y = scans[i].ranges[j] * sin((afrt - j) * 0.25 * M_PI / 180) * sin_1;
cloud->points[i * cloud->width + afrt - j - 1].z = scans[i].ranges[j] * cos((afrt - j) * 0.25 * M_PI / 180);
PCL_DEBUG("scans[%d].intensities[%d]: %f\n",i,j, scans[i].intensities[j]);
cloud->points[i * cloud->width + afrt - j - 1].intensity = scans[i].intensities[j];
cvSetReal2D(depth_img,i,afrt - j - 1,static_cast<double >(scans[i].ranges[j]));
cvSetReal2D(intensity_img,i,afrt - j - 1,static_cast<double >(scans[i].intensities[j]));
}
for (int j = afrt + 1; j <= max_step; j++)
{
cloud->points[(i + scans.size()) * cloud->width + j - 1 - afrt].x = scans[i].ranges[j] * sin((j - afrt) * 0.25 * M_PI / 180) * cos_2;
cloud->points[(i + scans.size()) * cloud->width + j - 1 - afrt].y = scans[i].ranges[j] * sin((j - afrt) * 0.25 * M_PI / 180) * sin_2;
cloud->points[(i + scans.size()) * cloud->width + j - 1 - afrt].z = scans[i].ranges[j] * cos((j - afrt) * 0.25 * M_PI / 180);
PCL_DEBUG("scans[%d].intensities[%d]: %f\n",i,j, scans[i].intensities[j]);
cloud->points[(i + scans.size()) * cloud->width + j - 1 - afrt].intensity = scans[i].intensities[j];
cvSetReal2D(depth_img,i + scans.size(),j - 1 - afrt,static_cast<double >(scans[i].ranges[j]));
cvSetReal2D(intensity_img,i + scans.size(),j - 1 - afrt,static_cast<double >(scans[i].intensities[j]));
}
}
double max_intensity, min_intensity;
double max_depth, min_depth;
cvMinMaxLoc(depth_img, &min_depth, &max_depth);
cvMinMaxLoc(intensity_img, &min_intensity, &max_intensity);
cvConvertScale(depth_img, depth_img, 255 / (max_depth - min_depth), -min_depth);
cvConvertScale(intensity_img, intensity_img, 255 / (max_intensity - min_intensity), -min_intensity);
for(int i=0;i< static_cast<int>(cloud->height);i++)
{
for(int j=0;j< static_cast<int>(cloud->width);j++)
{
cloud->points[i*cloud->width+j].intensity = (cloud->points[i*cloud->width+j].intensity - min_intensity) / (max_intensity-min_intensity);
//std::cout<<cloud->points[i*cloud->width+j].intensity<<" ";
}
}
PCL_INFO("3D scan has been acquired!\n");
return 0;
}
int PtuLaser::get3DScanWithRange(PointCloudWithRange::Ptr &cloud,
const double step_angle,
const double pan_range,
const bool clockwise)
{
scans.clear();
signed short int offset = static_cast<signed short int> (step_angle / FULL_STEP_RES);
if (clockwise == false)
{
offset = -offset;
}
int afrt = laser.afrt();
long long int time_stamp_offset = laser.getHokuyoClockOffset(10);
laser.laserOn();
laser.requestScans(false, min_step, max_step, 1, 0, 0, -1);
int position = ptu::get_current(PAN, POSITION);
std::cout << "step number:" << abs(static_cast<int> ((pan_range / M_PI) * 180 / (FULL_STEP_RES
* static_cast<double > (offset)))) << std::endl;
for (int step = 0; step < abs(static_cast<int> ((pan_range / M_PI) * 180 / (FULL_STEP_RES
* static_cast<double > (offset))))+1; step++)
{
if ((status = ptu::set_desired(PAN, POSITION, (ptu::PTU_PARM_PTR *)&offset, RELATIVE)) == TRUE)
{
return -1;
}
if ((status = ptu::await_completion()) != PTU_OK)
{
return -1;
}
gettimeofday(&ptu_executed_time, NULL);
ptu_executed_time_stamp = (uint64_t)(ptu_executed_time.tv_sec) * 1000000000 + (uint64_t)(ptu_executed_time.tv_usec)
* 1000 + time_stamp_offset;
usleep(20000);
bool isScanLatterThanPtu = false;
while (!isScanLatterThanPtu)
{
laser.serviceScan(laser_scan);
if (ptu_executed_time_stamp < laser_scan.self_time_stamp)
{
isScanLatterThanPtu = true;
scans.push_back(laser_scan);
}
}
}
laser.stopScanning();
cloud->height = scans.size() * 2;
cloud->width = scans[0].ranges.size() / 2;
cloud->points.resize(cloud->height * cloud->width);
for (int i = 0; i < static_cast<int>(scans.size()); i++)
{
double cos_1 = cos(-(position + i * offset) * FULL_STEP_RES * M_PI / 180);
double sin_1 = sin(-(position + i * offset) * FULL_STEP_RES * M_PI / 180);
double cos_2 = -cos_1;
double sin_2 = -sin_1;
for (int j = min_step; j < afrt; j++)
{
if (scans[i].ranges[j] > 0.1 && scans[i].ranges[j] < 30)
{
cloud->points[i * cloud->width + afrt - j - 1].x = scans[i].ranges[j] * sin((afrt - j) * 0.25 * M_PI / 180) * cos_1;
cloud->points[i * cloud->width + afrt - j - 1].y = scans[i].ranges[j] * sin((afrt - j) * 0.25 * M_PI / 180) * sin_1;
cloud->points[i * cloud->width + afrt - j - 1].z = scans[i].ranges[j] * cos((afrt - j) * 0.25 * M_PI / 180);
PCL_DEBUG("scans[%d].intensities[%d]: %f\n",i,j, scans[i].intensities[j]);
cloud->points[i * cloud->width + afrt - j - 1].range = scans[i].ranges[j];
}
else
{
cloud->points[i * cloud->width + afrt - j - 1].x = 0.0;
cloud->points[i * cloud->width + afrt - j - 1].y = 0.0;
cloud->points[i * cloud->width + afrt - j - 1].z = 0.0;
cloud->points[i * cloud->width + afrt - j - 1].range = 0.0;
}
}
for (int j = afrt + 1; j <= max_step; j++)
{
if (scans[i].ranges[j] > 0.1 && scans[i].ranges[j] < 30)
{
cloud->points[(i + scans.size()) * cloud->width + j - 1 - afrt].x = scans[i].ranges[j] * sin((j - afrt) * 0.25 * M_PI / 180) * cos_2;
cloud->points[(i + scans.size()) * cloud->width + j - 1 - afrt].y = scans[i].ranges[j] * sin((j - afrt) * 0.25 * M_PI / 180) * sin_2;
cloud->points[(i + scans.size()) * cloud->width + j - 1 - afrt].z = scans[i].ranges[j] * cos((j - afrt) * 0.25 * M_PI / 180);
PCL_DEBUG("scans[%d].intensities[%d]: %f\n",i,j, scans[i].intensities[j]);
cloud->points[(i + scans.size()) * cloud->width + j - 1 - afrt].range = scans[i].ranges[j];
}
else
{
cloud->points[(i + scans.size()) * cloud->width + j - 1 - afrt].x = 0.0;
cloud->points[(i + scans.size()) * cloud->width + j - 1 - afrt].y = 0.0;
cloud->points[(i + scans.size()) * cloud->width + j - 1 - afrt].z = 0.0;
cloud->points[(i + scans.size()) * cloud->width + j - 1 - afrt].range = 0.0;
}
}
}
PCL_INFO("3D scan has been acquired!\n");
return 0;
}
int
pcl::OBJReader::read (const std::string &file_name, pcl::PolygonMesh &mesh,
Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
int &file_version, const int offset)
{
pcl::console::TicToc tt;
tt.tic ();
int data_type;
unsigned int data_idx;
if (readHeader (file_name, mesh.cloud, origin, orientation, file_version, data_type, data_idx, offset))
{
PCL_ERROR ("[pcl::OBJReader::read] Problem reading header!\n");
return (-1);
}
std::ifstream fs;
fs.open (file_name.c_str (), std::ios::binary);
if (!fs.is_open () || fs.fail ())
{
PCL_ERROR ("[pcl::OBJReader::readHeader] Could not open file '%s'! Error : %s\n",
file_name.c_str (), strerror(errno));
fs.close ();
return (-1);
}
// Seek at the given offset
fs.seekg (data_idx, std::ios::beg);
// Get normal_x and rgba fields indices
int normal_x_field = -1;
// std::size_t rgba_field = 0;
for (std::size_t i = 0; i < mesh.cloud.fields.size (); ++i)
if (mesh.cloud.fields[i].name == "normal_x")
{
normal_x_field = i;
break;
}
std::size_t v_idx = 0;
std::size_t vn_idx = 0;
std::string line;
std::vector<std::string> st;
try
{
while (!fs.eof ())
{
getline (fs, line);
// Ignore empty lines
if (line == "")
continue;
// Tokenize the line
std::stringstream sstream (line);
sstream.imbue (std::locale::classic ());
line = sstream.str ();
boost::trim (line);
boost::split (st, line, boost::is_any_of ("\t\r "), boost::token_compress_on);
// Ignore comments
if (st[0] == "#")
continue;
// Vertex
if (st[0] == "v")
{
try
{
for (int i = 1, f = 0; i < 4; ++i, ++f)
{
float value = boost::lexical_cast<float> (st[i]);
memcpy (&mesh.cloud.data[v_idx * mesh.cloud.point_step + mesh.cloud.fields[f].offset],
&value,
sizeof (float));
}
++v_idx;
}
catch (const boost::bad_lexical_cast &e)
{
PCL_ERROR ("Unable to convert %s to vertex coordinates!", line.c_str ());
return (-1);
}
continue;
}
// Vertex normal
if (st[0] == "vn")
{
try
{
for (int i = 1, f = normal_x_field; i < 4; ++i, ++f)
{
float value = boost::lexical_cast<float> (st[i]);
memcpy (&mesh.cloud.data[vn_idx * mesh.cloud.point_step + mesh.cloud.fields[f].offset],
&value,
sizeof (float));
}
++vn_idx;
}
catch (const boost::bad_lexical_cast &e)
{
PCL_ERROR ("Unable to convert line %s to vertex normal!", line.c_str ());
return (-1);
}
continue;
}
// Face
if (st[0] == "f")
{
pcl::Vertices face_vertices; face_vertices.vertices.resize (st.size () - 1);
for (std::size_t i = 1; i < st.size (); ++i)
{
int v;
sscanf (st[i].c_str (), "%d", &v);
v = (v < 0) ? v_idx + v : v - 1;
face_vertices.vertices[i - 1] = v;
}
mesh.polygons.push_back (face_vertices);
continue;
}
}
}
catch (const char *exception)
{
PCL_ERROR ("[pcl::OBJReader::read] %s\n", exception);
fs.close ();
return (-1);
}
double total_time = tt.toc ();
PCL_DEBUG ("[pcl::OBJReader::read] Loaded %s as a PolygonMesh in %g ms with %g points and %g polygons.\n",
file_name.c_str (), total_time,
mesh.cloud.width * mesh.cloud.height, mesh.polygons.size ());
fs.close ();
return (0);
}
template <typename PointT> bool
pcl::RandomSampleConsensus<PointT>::computeModel (int debug_verbosity_level)
{
// Warn and exit if no threshold was set
if (threshold_ == std::numeric_limits<double>::max())
{
PCL_ERROR ("[pcl::RandomSampleConsensus::computeModel] No threshold set!\n");
return (false);
}
iterations_ = 0;
int n_best_inliers_count = -INT_MAX;
double k = 1.0;
std::vector<int> selection;
Eigen::VectorXf model_coefficients;
int n_inliers_count = 0;
unsigned skipped_count = 0;
// supress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
const unsigned max_skip = max_iterations_ * 10;
// Iterate
while (iterations_ < k && skipped_count < max_skip)
{
// Get X samples which satisfy the model criteria
sac_model_->getSamples (iterations_, selection);
if (selection.empty ())
{
PCL_ERROR ("[pcl::RandomSampleConsensus::computeModel] No samples could be selected!\n");
break;
}
// Search for inliers in the point cloud for the current plane model M
if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
{
//++iterations_;
++ skipped_count;
continue;
}
// Select the inliers that are within threshold_ from the model
//sac_model_->selectWithinDistance (model_coefficients, threshold_, inliers);
//if (inliers.empty () && k > 1.0)
// continue;
n_inliers_count = sac_model_->countWithinDistance (model_coefficients, threshold_);
// Better match ?
if (n_inliers_count > n_best_inliers_count)
{
n_best_inliers_count = n_inliers_count;
// Save the current model/inlier/coefficients selection as being the best so far
model_ = selection;
model_coefficients_ = model_coefficients;
// Compute the k parameter (k=log(z)/log(1-w^n))
double w = static_cast<double> (n_best_inliers_count) / static_cast<double> (sac_model_->getIndices ()->size ());
double p_no_outliers = 1.0 - pow (w, static_cast<double> (selection.size ()));
p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by -Inf
p_no_outliers = (std::min) (1.0 - std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by 0.
k = log (1.0 - probability_) / log (p_no_outliers);
}
++iterations_;
if (debug_verbosity_level > 1)
PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Trial %d out of %f: %d inliers (best is: %d so far).\n", iterations_, k, n_inliers_count, n_best_inliers_count);
if (iterations_ > max_iterations_)
{
if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
break;
}
}
if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Model: %zu size, %d inliers.\n", model_.size (), n_best_inliers_count);
if (model_.empty ())
{
inliers_.clear ();
return (false);
}
// Get the set of inliers that correspond to the best model found so far
sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
return (true);
}
template <typename PointT> bool
pcl::SACSegmentation<PointT>::initSACModel (const int model_type)
{
if (model_)
model_.reset ();
// Build the model
switch (model_type)
{
case SACMODEL_PLANE:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelPlane<PointT> (input_, *indices_));
break;
}
case SACMODEL_LINE:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelLine<PointT> (input_, *indices_));
break;
}
case SACMODEL_STICK:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_STICK\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelStick<PointT> (input_, *indices_));
double min_radius, max_radius;
model_->getRadiusLimits (min_radius, max_radius);
if (radius_min_ != min_radius && radius_max_ != max_radius)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
model_->setRadiusLimits (radius_min_, radius_max_);
}
break;
}
case SACMODEL_CIRCLE2D:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelCircle2D<PointT> (input_, *indices_));
typename SampleConsensusModelCircle2D<PointT>::Ptr model_circle = boost::static_pointer_cast<SampleConsensusModelCircle2D<PointT> > (model_);
double min_radius, max_radius;
model_circle->getRadiusLimits (min_radius, max_radius);
if (radius_min_ != min_radius && radius_max_ != max_radius)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
model_circle->setRadiusLimits (radius_min_, radius_max_);
}
break;
}
case SACMODEL_SPHERE:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelSphere<PointT> (input_, *indices_));
typename SampleConsensusModelSphere<PointT>::Ptr model_sphere = boost::static_pointer_cast<SampleConsensusModelSphere<PointT> > (model_);
double min_radius, max_radius;
model_sphere->getRadiusLimits (min_radius, max_radius);
if (radius_min_ != min_radius && radius_max_ != max_radius)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
model_sphere->setRadiusLimits (radius_min_, radius_max_);
}
break;
}
case SACMODEL_PARALLEL_LINE:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelParallelLine<PointT> (input_, *indices_));
typename SampleConsensusModelParallelLine<PointT>::Ptr model_parallel = boost::static_pointer_cast<SampleConsensusModelParallelLine<PointT> > (model_);
if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
model_parallel->setAxis (axis_);
}
if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
model_parallel->setEpsAngle (eps_angle_);
}
break;
}
case SACMODEL_PERPENDICULAR_PLANE:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelPerpendicularPlane<PointT> (input_, *indices_));
typename SampleConsensusModelPerpendicularPlane<PointT>::Ptr model_perpendicular = boost::static_pointer_cast<SampleConsensusModelPerpendicularPlane<PointT> > (model_);
if (axis_ != Eigen::Vector3f::Zero () && model_perpendicular->getAxis () != axis_)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
model_perpendicular->setAxis (axis_);
}
if (eps_angle_ != 0.0 && model_perpendicular->getEpsAngle () != eps_angle_)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
model_perpendicular->setEpsAngle (eps_angle_);
}
break;
}
case SACMODEL_PARALLEL_PLANE:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelParallelPlane<PointT> (input_, *indices_));
typename SampleConsensusModelParallelPlane<PointT>::Ptr model_parallel = boost::static_pointer_cast<SampleConsensusModelParallelPlane<PointT> > (model_);
if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
model_parallel->setAxis (axis_);
}
if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
model_parallel->setEpsAngle (eps_angle_);
}
break;
}
default:
{
PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
return (false);
}
}
return (true);
}
template <typename PointSource, typename PointTarget> void
pcl::IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
{
// Allocate enough space to hold the results
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
// Point cloud containing the correspondences of each point in <input, indices>
PointCloudTarget input_corresp;
input_corresp.points.resize (indices_->size ());
nr_iterations_ = 0;
converged_ = false;
double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
// If the guessed transformation is non identity
if (guess != Eigen::Matrix4f::Identity ())
{
// Initialise final transformation to the guessed one
final_transformation_ = guess;
// Apply guessed transformation prior to search for neighbours
transformPointCloud (output, output, guess);
}
// Resize the vector of distances between correspondences
std::vector<float> previous_correspondence_distances (indices_->size ());
correspondence_distances_.resize (indices_->size ());
while (!converged_) // repeat until convergence
{
// Save the previously estimated transformation
previous_transformation_ = transformation_;
// And the previous set of distances
previous_correspondence_distances = correspondence_distances_;
int cnt = 0;
std::vector<int> source_indices (indices_->size ());
std::vector<int> target_indices (indices_->size ());
// Iterating over the entire index vector and find all correspondences
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!this->searchForNeighbors (output, (*indices_)[idx], nn_indices, nn_dists))
{
PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[idx]);
return;
}
// Check if the distance to the nearest neighbor is smaller than the user imposed threshold
if (nn_dists[0] < dist_threshold)
{
source_indices[cnt] = (*indices_)[idx];
target_indices[cnt] = nn_indices[0];
cnt++;
}
// Save the nn_dists[0] to a global vector of distances
correspondence_distances_[(*indices_)[idx]] = std::min (nn_dists[0], static_cast<float> (dist_threshold));
}
if (cnt < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
converged_ = false;
return;
}
// Resize to the actual number of valid correspondences
source_indices.resize (cnt); target_indices.resize (cnt);
std::vector<int> source_indices_good;
std::vector<int> target_indices_good;
{
// From the set of correspondences found, attempt to remove outliers
// Create the registration model
typedef typename SampleConsensusModelRegistration<PointSource>::Ptr SampleConsensusModelRegistrationPtr;
SampleConsensusModelRegistrationPtr model;
model.reset (new SampleConsensusModelRegistration<PointSource> (output.makeShared (), source_indices));
// Pass the target_indices
model->setInputTarget (target_, target_indices);
// Create a RANSAC model
RandomSampleConsensus<PointSource> sac (model, inlier_threshold_);
sac.setMaxIterations (ransac_iterations_);
// Compute the set of inliers
if (!sac.computeModel ())
{
source_indices_good = source_indices;
target_indices_good = target_indices;
}
else
{
std::vector<int> inliers;
// Get the inliers
sac.getInliers (inliers);
source_indices_good.resize (inliers.size ());
target_indices_good.resize (inliers.size ());
boost::unordered_map<int, int> source_to_target;
for (unsigned int i = 0; i < source_indices.size(); ++i)
source_to_target[source_indices[i]] = target_indices[i];
// Copy just the inliers
std::copy(inliers.begin(), inliers.end(), source_indices_good.begin());
for (size_t i = 0; i < inliers.size (); ++i)
target_indices_good[i] = source_to_target[inliers[i]];
}
}
// Check whether we have enough correspondences
cnt = static_cast<int> (source_indices_good.size ());
if (cnt < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
converged_ = false;
return;
}
PCL_DEBUG ("[pcl::%s::computeTransformation] Number of correspondences %d [%f%%] out of %zu points [100.0%%], RANSAC rejected: %zu [%f%%].\n",
getClassName ().c_str (),
cnt,
(static_cast<float> (cnt) * 100.0f) / static_cast<float> (indices_->size ()),
indices_->size (),
source_indices.size () - cnt,
static_cast<float> (source_indices.size () - cnt) * 100.0f / static_cast<float> (source_indices.size ()));
// Estimate the transform
//rigid_transformation_estimation_(output, source_indices_good, *target_, target_indices_good, transformation_);
transformation_estimation_->estimateRigidTransformation (output, source_indices_good, *target_, target_indices_good, transformation_);
// Tranform the data
transformPointCloud (output, output, transformation_);
// Obtain the final transformation
final_transformation_ = transformation_ * final_transformation_;
nr_iterations_++;
// Update the vizualization of icp convergence
if (update_visualizer_ != 0)
update_visualizer_(output, source_indices_good, *target_, target_indices_good );
// Various/Different convergence termination criteria
// 1. Number of iterations has reached the maximum user imposed number of iterations (via
// setMaximumIterations)
// 2. The epsilon (difference) between the previous transformation and the current estimated transformation
// is smaller than an user imposed value (via setTransformationEpsilon)
// 3. The sum of Euclidean squared errors is smaller than a user defined threshold (via
// setEuclideanFitnessEpsilon)
if (nr_iterations_ >= max_iterations_ ||
(transformation_ - previous_transformation_).array ().abs ().sum () < transformation_epsilon_ ||
fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)) <= euclidean_fitness_epsilon_
)
{
converged_ = true;
PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ());
PCL_DEBUG ("nr_iterations_ (%d) >= max_iterations_ (%d)\n", nr_iterations_, max_iterations_);
PCL_DEBUG ("(transformation_ - previous_transformation_).array ().abs ().sum () (%f) < transformation_epsilon_ (%f)\n",
(transformation_ - previous_transformation_).array ().abs ().sum (), transformation_epsilon_);
PCL_DEBUG ("fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)) (%f) <= euclidean_fitness_epsilon_ (%f)\n",
fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)),
euclidean_fitness_epsilon_);
}
}
}
template <typename PointT, typename PointNT> bool
pcl::SACSegmentationFromNormals<PointT, PointNT>::initSACModel (const int model_type)
{
// Check if input is synced with the normals
if (input_->points.size () != normals_->points.size ())
{
PCL_ERROR ("[pcl::%s::initSACModel] The number of points inthe input point cloud differs than the number of points in the normals!\n", getClassName ().c_str ());
return (false);
}
if (model_)
model_.reset ();
// Build the model
switch (model_type)
{
case SACMODEL_CYLINDER:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelCylinder<PointT, PointNT > (input_, *indices_));
typename SampleConsensusModelCylinder<PointT, PointNT>::Ptr model_cylinder = boost::static_pointer_cast<SampleConsensusModelCylinder<PointT, PointNT> > (model_);
// Set the input normals
model_cylinder->setInputNormals (normals_);
double min_radius, max_radius;
model_cylinder->getRadiusLimits (min_radius, max_radius);
if (radius_min_ != min_radius && radius_max_ != max_radius)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
model_cylinder->setRadiusLimits (radius_min_, radius_max_);
}
if (distance_weight_ != model_cylinder->getNormalDistanceWeight ())
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
model_cylinder->setNormalDistanceWeight (distance_weight_);
}
if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->getAxis () != axis_)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
model_cylinder->setAxis (axis_);
}
if (eps_angle_ != 0.0 && model_cylinder->getEpsAngle () != eps_angle_)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
model_cylinder->setEpsAngle (eps_angle_);
}
break;
}
case SACMODEL_NORMAL_PLANE:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelNormalPlane<PointT, PointNT> (input_, *indices_));
typename SampleConsensusModelNormalPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalPlane<PointT, PointNT> > (model_);
// Set the input normals
model_normals->setInputNormals (normals_);
if (distance_weight_ != model_normals->getNormalDistanceWeight ())
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
model_normals->setNormalDistanceWeight (distance_weight_);
}
break;
}
case SACMODEL_NORMAL_PARALLEL_PLANE:
{
PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
model_.reset (new SampleConsensusModelNormalParallelPlane<PointT, PointNT> (input_, *indices_));
typename SampleConsensusModelNormalParallelPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalParallelPlane<PointT, PointNT> > (model_);
// Set the input normals
model_normals->setInputNormals (normals_);
if (distance_weight_ != model_normals->getNormalDistanceWeight ())
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
model_normals->setNormalDistanceWeight (distance_weight_);
}
if (axis_ != Eigen::Vector3f::Zero () && model_normals->getAxis () != axis_)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
model_normals->setAxis (axis_);
}
if (eps_angle_ != 0.0 && model_normals->getEpsAngle () != eps_angle_)
{
PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
model_normals->setEpsAngle (eps_angle_);
}
break;
}
// If nothing else, try SACSegmentation
default:
{
return (pcl::SACSegmentation<PointT>::initSACModel (model_type));
}
}
return (true);
}
template <typename PointT> bool
pcl::MEstimatorSampleConsensus<PointT>::computeModel (int debug_verbosity_level)
{
// Warn and exit if no threshold was set
if (threshold_ == std::numeric_limits<double>::max())
{
PCL_ERROR ("[pcl::MEstimatorSampleConsensus::computeModel] No threshold set!\n");
return (false);
}
iterations_ = 0;
double d_best_penalty = std::numeric_limits<double>::max();
double k = 1.0;
std::vector<int> best_model;
std::vector<int> selection;
Eigen::VectorXf model_coefficients;
std::vector<double> distances;
int n_inliers_count = 0;
unsigned skipped_count = 0;
// suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
const unsigned max_skip = max_iterations_ * 10;
// Iterate
while (iterations_ < k && skipped_count < max_skip)
{
// Get X samples which satisfy the model criteria
sac_model_->getSamples (iterations_, selection);
if (selection.empty ()) break;
// Search for inliers in the point cloud for the current plane model M
if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
{
//iterations_++;
++ skipped_count;
continue;
}
double d_cur_penalty = 0;
// Iterate through the 3d points and calculate the distances from them to the model
sac_model_->getDistancesToModel (model_coefficients, distances);
if (distances.empty () && k > 1.0)
continue;
for (const double &distance : distances)
d_cur_penalty += (std::min) (distance, threshold_);
// Better match ?
if (d_cur_penalty < d_best_penalty)
{
d_best_penalty = d_cur_penalty;
// Save the current model/coefficients selection as being the best so far
model_ = selection;
model_coefficients_ = model_coefficients;
n_inliers_count = 0;
// Need to compute the number of inliers for this model to adapt k
for (const double &distance : distances)
if (distance <= threshold_)
++n_inliers_count;
// Compute the k parameter (k=log(z)/log(1-w^n))
double w = static_cast<double> (n_inliers_count) / static_cast<double> (sac_model_->getIndices ()->size ());
double p_no_outliers = 1.0 - pow (w, static_cast<double> (selection.size ()));
p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by -Inf
p_no_outliers = (std::min) (1.0 - std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by 0.
k = log (1.0 - probability_) / log (p_no_outliers);
}
++iterations_;
if (debug_verbosity_level > 1)
PCL_DEBUG ("[pcl::MEstimatorSampleConsensus::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_, static_cast<int> (ceil (k)), d_best_penalty);
if (iterations_ > max_iterations_)
{
if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::MEstimatorSampleConsensus::computeModel] MSAC reached the maximum number of trials.\n");
break;
}
}
if (model_.empty ())
{
if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::MEstimatorSampleConsensus::computeModel] Unable to find a solution!\n");
return (false);
}
// Iterate through the 3d points and calculate the distances from them to the model again
sac_model_->getDistancesToModel (model_coefficients_, distances);
std::vector<int> &indices = *sac_model_->getIndices ();
if (distances.size () != indices.size ())
{
PCL_ERROR ("[pcl::MEstimatorSampleConsensus::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.size ());
return (false);
}
inliers_.resize (distances.size ());
// Get the inliers for the best model found
n_inliers_count = 0;
for (size_t i = 0; i < distances.size (); ++i)
if (distances[i] <= threshold_)
inliers_[n_inliers_count++] = indices[i];
// Resize the inliers vector
inliers_.resize (n_inliers_count);
if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::MEstimatorSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count);
return (true);
}
template <typename PointFeature> bool
pcl::PyramidFeatureHistogram<PointFeature>::initializeHistogram ()
{
// a few consistency checks before starting the computations
if (!PCLBase<PointFeature>::initCompute ())
{
PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute failed\n");
return false;
}
if (dimension_range_input_.size () == 0)
{
PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension range was not set\n");
return false;
}
if (dimension_range_target_.size () == 0)
{
PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension range was not set\n");
return false;
}
if (dimension_range_input_.size () != dimension_range_target_.size ())
{
PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target dimension ranges do not agree in size: %u vs %u\n",
dimension_range_input_.size (), dimension_range_target_.size ());
return false;
}
nr_dimensions = dimension_range_target_.size ();
nr_features = input_->points.size ();
float D = 0.0f;
for (std::vector<std::pair<float, float> >::iterator range_it = dimension_range_target_.begin (); range_it != dimension_range_target_.end (); ++range_it)
{
float aux = range_it->first - range_it->second;
D += aux * aux;
}
D = sqrt (D);
nr_levels = ceil (Log2 (D));
PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u levels with a hyper-parallelepiped diagonal size of %f\n", nr_levels, D);
hist_levels.resize (nr_levels);
for (size_t level_i = 0; level_i < nr_levels; ++level_i)
{
std::vector<size_t> bins_per_dimension (nr_dimensions);
std::vector<float> bin_step (nr_dimensions);
for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) {
bins_per_dimension[dim_i] = ceil ( (dimension_range_target_[dim_i].second - dimension_range_target_[dim_i].first) / (pow (2.0f, (int) level_i) * sqrt ((float) nr_dimensions)));
bin_step[dim_i] = pow (2.0f, (int) level_i) * sqrt ((float) nr_dimensions);
}
hist_levels[level_i] = PyramidFeatureHistogramLevel (bins_per_dimension, bin_step);
PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of size %u at level %u\nwith #bins per dimension:", hist_levels.back ().hist.size (), level_i);
for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
PCL_DEBUG ("%u ", bins_per_dimension[dim_i]);
PCL_DEBUG ("\n");
}
return true;
}
template <typename PointSource, typename PointTarget> void
pcl::PPFRegistration<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
{
if (!search_method_)
{
PCL_ERROR("[pcl::PPFRegistration::computeTransformation] Search method not set - skipping computeTransformation!\n");
return;
}
if (guess != Eigen::Matrix4f::Identity ())
{
PCL_ERROR("[pcl::PPFRegistration::computeTransformation] setting initial transform (guess) not implemented!\n");
}
PoseWithVotesList voted_poses;
std::vector <std::vector <unsigned int> > accumulator_array;
accumulator_array.resize (input_->points.size ());
size_t aux_size = static_cast<size_t> (floor (2 * M_PI / search_method_->getAngleDiscretizationStep ()));
for (size_t i = 0; i < input_->points.size (); ++i)
{
std::vector<unsigned int> aux (aux_size);
accumulator_array[i] = aux;
}
PCL_INFO ("Accumulator array size: %u x %u.\n", accumulator_array.size (), accumulator_array.back ().size ());
// Consider every <scene_reference_point_sampling_rate>-th point as the reference point => fix s_r
float f1, f2, f3, f4;
for (size_t scene_reference_index = 0; scene_reference_index < target_->points.size (); scene_reference_index += scene_reference_point_sampling_rate_)
{
Eigen::Vector3f scene_reference_point = target_->points[scene_reference_index].getVector3fMap (),
scene_reference_normal = target_->points[scene_reference_index].getNormalVector3fMap ();
Eigen::AngleAxisf rotation_sg (acosf (scene_reference_normal.dot (Eigen::Vector3f::UnitX ())),
scene_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized());
Eigen::Affine3f transform_sg (Eigen::Translation3f (rotation_sg * ((-1) * scene_reference_point)) * rotation_sg);
// For every other point in the scene => now have pair (s_r, s_i) fixed
std::vector<int> indices;
std::vector<float> distances;
scene_search_tree_->radiusSearch (target_->points[scene_reference_index],
search_method_->getModelDiameter () /2,
indices,
distances);
for(size_t i = 0; i < indices.size (); ++i)
// for(size_t i = 0; i < target_->points.size (); ++i)
{
//size_t scene_point_index = i;
size_t scene_point_index = indices[i];
if (scene_reference_index != scene_point_index)
{
if (/*pcl::computePPFPairFeature*/pcl::computePairFeatures (target_->points[scene_reference_index].getVector4fMap (),
target_->points[scene_reference_index].getNormalVector4fMap (),
target_->points[scene_point_index].getVector4fMap (),
target_->points[scene_point_index].getNormalVector4fMap (),
f1, f2, f3, f4))
{
std::vector<std::pair<size_t, size_t> > nearest_indices;
search_method_->nearestNeighborSearch (f1, f2, f3, f4, nearest_indices);
// Compute alpha_s angle
Eigen::Vector3f scene_point = target_->points[scene_point_index].getVector3fMap ();
Eigen::AngleAxisf rotation_sg (acosf (scene_reference_normal.dot (Eigen::Vector3f::UnitX ())),
scene_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ());
Eigen::Affine3f transform_sg = Eigen::Translation3f ( rotation_sg * ((-1) * scene_reference_point)) * rotation_sg;
// float alpha_s = acos (Eigen::Vector3f::UnitY ().dot ((transform_sg * scene_point).normalized ()));
Eigen::Vector3f scene_point_transformed = transform_sg * scene_point;
float alpha_s = atan2f ( -scene_point_transformed(2), scene_point_transformed(1));
if ( alpha_s != alpha_s)
{
PCL_ERROR ("alpha_s is nan\n");
continue;
}
if (sin (alpha_s) * scene_point_transformed(2) < 0.0f)
alpha_s *= (-1);
alpha_s *= (-1);
// Go through point pairs in the model with the same discretized feature
for (std::vector<std::pair<size_t, size_t> >::iterator v_it = nearest_indices.begin (); v_it != nearest_indices.end (); ++ v_it)
{
size_t model_reference_index = v_it->first,
model_point_index = v_it->second;
// Calculate angle alpha = alpha_m - alpha_s
float alpha = search_method_->alpha_m_[model_reference_index][model_point_index] - alpha_s;
unsigned int alpha_discretized = static_cast<unsigned int> (floor (alpha) + floor (M_PI / search_method_->getAngleDiscretizationStep ()));
accumulator_array[model_reference_index][alpha_discretized] ++;
}
}
else PCL_ERROR ("[pcl::PPFRegistration::computeTransformation] Computing pair feature vector between points %zu and %zu went wrong.\n", scene_reference_index, scene_point_index);
}
}
size_t max_votes_i = 0, max_votes_j = 0;
unsigned int max_votes = 0;
for (size_t i = 0; i < accumulator_array.size (); ++i)
for (size_t j = 0; j < accumulator_array.back ().size (); ++j)
{
if (accumulator_array[i][j] > max_votes)
{
max_votes = accumulator_array[i][j];
max_votes_i = i;
max_votes_j = j;
}
// Reset accumulator_array for the next set of iterations with a new scene reference point
accumulator_array[i][j] = 0;
}
Eigen::Vector3f model_reference_point = input_->points[max_votes_i].getVector3fMap (),
model_reference_normal = input_->points[max_votes_i].getNormalVector3fMap ();
Eigen::AngleAxisf rotation_mg (acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())), model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ());
Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg;
Eigen::Affine3f max_transform =
transform_sg.inverse () *
Eigen::AngleAxisf ((static_cast<float> (max_votes_j) - floorf (static_cast<float> (M_PI) / search_method_->getAngleDiscretizationStep ())) * search_method_->getAngleDiscretizationStep (), Eigen::Vector3f::UnitX ()) *
transform_mg;
voted_poses.push_back (PoseWithVotes (max_transform, max_votes));
}
PCL_DEBUG ("Done with the Hough Transform ...\n");
// Cluster poses for filtering out outliers and obtaining more precise results
PoseWithVotesList results;
clusterPoses (voted_poses, results);
pcl::transformPointCloud (*input_, output, results.front ().pose);
transformation_ = final_transformation_ = results.front ().pose.matrix ();
converged_ = true;
}
template <typename PointT, typename PointNT> void
pcl::SurfelSmoothing<PointT, PointNT>::smoothPoint (size_t &point_index,
PointT &output_point,
PointNT &output_normal)
{
Eigen::Vector4f average_normal = Eigen::Vector4f::Zero ();
Eigen::Vector4f result_point = input_->points[point_index].getVector4fMap ();
result_point(3) = 0.0f;
// @todo parameter
float error_residual_threshold_ = 1e-3;
float error_residual = error_residual_threshold_ + 1;
float last_error_residual = error_residual + 100;
std::vector<int> nn_indices;
std::vector<float> nn_distances;
int big_iterations = 0;
int max_big_iterations = 500;
while (fabs (error_residual) < fabs (last_error_residual) -error_residual_threshold_ &&
big_iterations < max_big_iterations)
{
average_normal = Eigen::Vector4f::Zero ();
big_iterations ++;
PointT aux_point; aux_point.x = result_point(0); aux_point.y = result_point(1); aux_point.z = result_point(2);
tree_->radiusSearch (aux_point, 5*scale_, nn_indices, nn_distances);
float theta_normalization_factor = 0.0;
std::vector<float> theta (nn_indices.size ());
for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
{
float dist = nn_distances[nn_index_i];
float theta_i = exp ( (-1) * dist / scale_squared_);
theta_normalization_factor += theta_i;
average_normal += theta_i * normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap ();
theta[nn_index_i] = theta_i;
}
average_normal /= theta_normalization_factor;
average_normal(3) = 0.0f;
average_normal.normalize ();
// find minimum along the normal
float e_residual_along_normal = 2, last_e_residual_along_normal = 3;
int small_iterations = 0;
int max_small_iterations = 10;
while ( fabs (e_residual_along_normal) < fabs (last_e_residual_along_normal) &&
small_iterations < max_small_iterations)
{
small_iterations ++;
e_residual_along_normal = 0.0f;
for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
{
Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap ();
neighbor(3) = 0.0f;
float dot_product = average_normal.dot (neighbor - result_point);
e_residual_along_normal += theta[nn_index_i] * dot_product;
}
e_residual_along_normal /= theta_normalization_factor;
if (e_residual_along_normal < 1e-3) break;
result_point = result_point + e_residual_along_normal * average_normal;
}
// if (small_iterations == max_small_iterations)
// PCL_INFO ("passed the number of small iterations %d\n", small_iterations);
last_error_residual = error_residual;
error_residual = e_residual_along_normal;
// PCL_INFO ("last %f current %f\n", last_error_residual, error_residual);
}
output_point.x = result_point(0);
output_point.y = result_point(1);
output_point.z = result_point(2);
output_normal = normals_->points[point_index];
if (big_iterations == max_big_iterations)
PCL_DEBUG ("[pcl::SurfelSmoothing::smoothPoint] Passed the number of BIG iterations: %d\n", big_iterations);
}
template <typename PointT> bool
pcl::MaximumLikelihoodSampleConsensus<PointT>::computeModel (int debug_verbosity_level)
{
// Warn and exit if no threshold was set
if (threshold_ == std::numeric_limits<double>::max())
{
PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] No threshold set!\n");
return (false);
}
iterations_ = 0;
double d_best_penalty = std::numeric_limits<double>::max();
double k = 1.0;
std::vector<int> best_model;
std::vector<int> selection;
Eigen::VectorXf model_coefficients;
std::vector<double> distances;
// Compute sigma - remember to set threshold_ correctly !
sigma_ = computeMedianAbsoluteDeviation (sac_model_->getInputCloud (), sac_model_->getIndices (), threshold_);
if (debug_verbosity_level > 1)
PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated sigma value: %f.\n", sigma_);
// Compute the bounding box diagonal: V = sqrt (sum (max(pointCloud) - min(pointCloud)^2))
Eigen::Vector4f min_pt, max_pt;
getMinMax (sac_model_->getInputCloud (), sac_model_->getIndices (), min_pt, max_pt);
max_pt -= min_pt;
double v = sqrt (max_pt.dot (max_pt));
int n_inliers_count = 0;
size_t indices_size;
unsigned skipped_count = 0;
// supress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
const unsigned max_skip = max_iterations_ * 10;
// Iterate
while (iterations_ < k && skipped_count < max_skip)
{
// Get X samples which satisfy the model criteria
sac_model_->getSamples (iterations_, selection);
if (selection.empty ()) break;
// Search for inliers in the point cloud for the current plane model M
if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
{
//iterations_++;
++ skipped_count;
continue;
}
// Iterate through the 3d points and calculate the distances from them to the model
sac_model_->getDistancesToModel (model_coefficients, distances);
// Use Expectiation-Maximization to find out the right value for d_cur_penalty
// ---[ Initial estimate for the gamma mixing parameter = 1/2
double gamma = 0.5;
double p_outlier_prob = 0;
indices_size = sac_model_->getIndices ()->size ();
std::vector<double> p_inlier_prob (indices_size);
for (int j = 0; j < iterations_EM_; ++j)
{
// Likelihood of a datum given that it is an inlier
for (size_t i = 0; i < indices_size; ++i)
p_inlier_prob[i] = gamma * exp (- (distances[i] * distances[i] ) / 2 * (sigma_ * sigma_) ) /
(sqrt (2 * M_PI) * sigma_);
// Likelihood of a datum given that it is an outlier
p_outlier_prob = (1 - gamma) / v;
gamma = 0;
for (size_t i = 0; i < indices_size; ++i)
gamma += p_inlier_prob [i] / (p_inlier_prob[i] + p_outlier_prob);
gamma /= static_cast<double>(sac_model_->getIndices ()->size ());
}
// Find the log likelihood of the model -L = -sum [log (pInlierProb + pOutlierProb)]
double d_cur_penalty = 0;
for (size_t i = 0; i < indices_size; ++i)
d_cur_penalty += log (p_inlier_prob[i] + p_outlier_prob);
d_cur_penalty = - d_cur_penalty;
// Better match ?
if (d_cur_penalty < d_best_penalty)
{
d_best_penalty = d_cur_penalty;
// Save the current model/coefficients selection as being the best so far
model_ = selection;
model_coefficients_ = model_coefficients;
n_inliers_count = 0;
// Need to compute the number of inliers for this model to adapt k
for (size_t i = 0; i < distances.size (); ++i)
if (distances[i] <= 2 * sigma_)
n_inliers_count++;
// Compute the k parameter (k=log(z)/log(1-w^n))
double w = static_cast<double> (n_inliers_count) / static_cast<double> (sac_model_->getIndices ()->size ());
double p_no_outliers = 1 - pow (w, static_cast<double> (selection.size ()));
p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by -Inf
p_no_outliers = (std::min) (1 - std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by 0.
k = log (1 - probability_) / log (p_no_outliers);
}
++iterations_;
if (debug_verbosity_level > 1)
PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_, static_cast<int> (ceil (k)), d_best_penalty);
if (iterations_ > max_iterations_)
{
if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] MLESAC reached the maximum number of trials.\n");
break;
}
}
if (model_.empty ())
{
if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Unable to find a solution!\n");
return (false);
}
// Iterate through the 3d points and calculate the distances from them to the model again
sac_model_->getDistancesToModel (model_coefficients_, distances);
std::vector<int> &indices = *sac_model_->getIndices ();
if (distances.size () != indices.size ())
{
PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated distances (%zu) differs than the normal of indices (%zu).\n", distances.size (), indices.size ());
return (false);
}
inliers_.resize (distances.size ());
// Get the inliers for the best model found
n_inliers_count = 0;
for (size_t i = 0; i < distances.size (); ++i)
if (distances[i] <= 2 * sigma_)
inliers_[n_inliers_count++] = indices[i];
// Resize the inliers vector
inliers_.resize (n_inliers_count);
if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Model: %zu size, %d inliers.\n", model_.size (), n_inliers_count);
return (true);
}
int
pcl::IFSReader::read (const std::string &file_name, pcl::PolygonMesh &mesh, int &ifs_version)
{
pcl::console::TicToc tt;
tt.tic ();
unsigned int data_idx;
int res = readHeader (file_name, mesh.cloud, ifs_version, data_idx);
if (res < 0)
return (res);
// Setting the is_dense property to true by default
mesh.cloud.is_dense = true;
boost::iostreams::mapped_file_source mapped_file;
size_t data_size = data_idx + mesh.cloud.data.size ();
try
{
mapped_file.open (file_name, data_size, 0);
}
catch (const char *exception)
{
PCL_ERROR ("[pcl::IFSReader::read] Error : %s!\n", file_name.c_str (), exception);
mapped_file.close ();
return (-1);
}
if(!mapped_file.is_open ())
{
PCL_ERROR ("[pcl::IFSReader::read] File mapping failure\n");
mapped_file.close ();
return (-1);
}
// Copy the data
memcpy (&mesh.cloud.data[0], mapped_file.data () + data_idx, mesh.cloud.data.size ());
mapped_file.close ();
// Reopen the file to load the facets
std::ifstream fs;
fs.open (file_name.c_str (), std::ios::binary);
if (!fs.is_open () || fs.fail ())
{
PCL_ERROR ("[pcl::IFSReader::read] Could not open file '%s'! Error : %s\n", file_name.c_str (), strerror(errno));
fs.close ();
return (-1);
}
// Jump to the end of cloud data
fs.seekg (data_size);
// Read the TRIANGLES keyword
uint32_t length_of_keyword;
fs.read ((char*)&length_of_keyword, sizeof (uint32_t));
char *keyword = new char [length_of_keyword];
fs.read (keyword, sizeof (char) * length_of_keyword);
if (strcmp (keyword, "TRIANGLES"))
{
PCL_ERROR ("[pcl::IFSReader::read] File %s is does not contain facets!\n", file_name.c_str ());
fs.close ();
return (-1);
}
delete[] keyword;
// Read the number of facets
uint32_t nr_facets;
fs.read ((char*)&nr_facets, sizeof (uint32_t));
if ((nr_facets == 0) || (nr_facets > 10000000))
{
PCL_ERROR ("[pcl::IFSReader::read] Bad number of facets %lu!\n", nr_facets);
fs.close ();
return (-1);
}
// Resize the mesh polygons
mesh.polygons.resize (nr_facets);
// Fill each polygon
for (uint32_t i = 0; i < nr_facets; ++i)
{
pcl::Vertices &facet = mesh.polygons[i];
facet.vertices.resize (3);
fs.read ((char*)&(facet.vertices[0]), sizeof (uint32_t));
fs.read ((char*)&(facet.vertices[1]), sizeof (uint32_t));
fs.read ((char*)&(facet.vertices[2]), sizeof (uint32_t));
}
// We are done, close the file
fs.close ();
// Display statistics
double total_time = tt.toc ();
PCL_DEBUG ("[pcl::IFSReader::read] Loaded %s as a polygon mesh in %g ms with %d points and %d facets.\n",
file_name.c_str (), total_time, mesh.cloud.width * mesh.cloud.height, mesh.polygons.size ());
return (0);
}
int
pcl::OBJReader::read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
int &file_version, const int offset)
{
pcl::console::TicToc tt;
tt.tic ();
int data_type;
unsigned int data_idx;
if (readHeader (file_name, cloud, origin, orientation, file_version, data_type, data_idx, offset))
{
PCL_ERROR ("[pcl::OBJReader::read] Problem reading header!\n");
return (-1);
}
std::ifstream fs;
fs.open (file_name.c_str (), std::ios::binary);
if (!fs.is_open () || fs.fail ())
{
PCL_ERROR ("[pcl::OBJReader::readHeader] Could not open file '%s'! Error : %s\n",
file_name.c_str (), strerror(errno));
fs.close ();
return (-1);
}
// Seek at the given offset
fs.seekg (data_idx, std::ios::beg);
// Get normal_x and rgba fields indices
int normal_x_field = -1;
// std::size_t rgba_field = 0;
for (std::size_t i = 0; i < cloud.fields.size (); ++i)
if (cloud.fields[i].name == "normal_x")
{
normal_x_field = i;
break;
}
// else if (cloud.fields[i].name == "rgba")
// rgba_field = i;
std::size_t point_idx = 0;
std::size_t normal_idx = 0;
std::string line;
std::vector<std::string> st;
try
{
while (!fs.eof ())
{
getline (fs, line);
// Ignore empty lines
if (line == "")
continue;
// Tokenize the line
std::stringstream sstream (line);
sstream.imbue (std::locale::classic ());
line = sstream.str ();
boost::trim (line);
boost::split (st, line, boost::is_any_of ("\t\r "), boost::token_compress_on);
// Ignore comments
if (st[0] == "#")
continue;
// Vertex
if (st[0] == "v")
{
try
{
for (int i = 1, f = 0; i < 4; ++i, ++f)
{
float value = boost::lexical_cast<float> (st[i]);
memcpy (&cloud.data[point_idx * cloud.point_step + cloud.fields[f].offset],
&value,
sizeof (float));
}
++point_idx;
}
catch (const boost::bad_lexical_cast &e)
{
PCL_ERROR ("Unable to convert %s to vertex coordinates!", line.c_str ());
return (-1);
}
continue;
}
// Vertex normal
if (st[0] == "vn")
{
try
{
for (int i = 1, f = normal_x_field; i < 4; ++i, ++f)
{
float value = boost::lexical_cast<float> (st[i]);
memcpy (&cloud.data[normal_idx * cloud.point_step + cloud.fields[f].offset],
&value,
sizeof (float));
}
++normal_idx;
}
catch (const boost::bad_lexical_cast &e)
{
PCL_ERROR ("Unable to convert line %s to vertex normal!", line.c_str ());
return (-1);
}
continue;
}
}
}
catch (const char *exception)
{
PCL_ERROR ("[pcl::OBJReader::read] %s\n", exception);
fs.close ();
return (-1);
}
double total_time = tt.toc ();
PCL_DEBUG ("[pcl::OBJReader::read] Loaded %s as a dense cloud in %g ms with %d points. Available dimensions: %s.\n",
file_name.c_str (), total_time,
cloud.width * cloud.height, pcl::getFieldsList (cloud).c_str ());
fs.close ();
return (0);
}
template <typename PointT> bool
pcl::LeastMedianSquares<PointT>::computeModel (int debug_verbosity_level)
{
// Warn and exit if no threshold was set
if (threshold_ == std::numeric_limits<double>::max())
{
PCL_ERROR ("[pcl::LeastMedianSquares::computeModel] No threshold set!\n");
return (false);
}
iterations_ = 0;
double d_best_penalty = std::numeric_limits<double>::max();
std::vector<int> best_model;
std::vector<int> selection;
Eigen::VectorXf model_coefficients;
std::vector<double> distances;
int n_inliers_count = 0;
unsigned skipped_count = 0;
// supress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
const unsigned max_skip = max_iterations_ * 10;
// Iterate
while (iterations_ < max_iterations_ && skipped_count < max_skip)
{
// Get X samples which satisfy the model criteria
sac_model_->getSamples (iterations_, selection);
if (selection.empty ()) break;
// Search for inliers in the point cloud for the current plane model M
if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
{
//iterations_++;
++skipped_count;
continue;
}
double d_cur_penalty = 0;
// d_cur_penalty = sum (min (dist, threshold))
// Iterate through the 3d points and calculate the distances from them to the model
sac_model_->getDistancesToModel (model_coefficients, distances);
// No distances? The model must not respect the user given constraints
if (distances.empty ())
{
//iterations_++;
++skipped_count;
continue;
}
std::sort (distances.begin (), distances.end ());
// d_cur_penalty = median (distances)
size_t mid = sac_model_->getIndices ()->size () / 2;
if (mid >= distances.size ())
{
//iterations_++;
++skipped_count;
continue;
}
// Do we have a "middle" point or should we "estimate" one ?
if (sac_model_->getIndices ()->size () % 2 == 0)
d_cur_penalty = (sqrt (distances[mid-1]) + sqrt (distances[mid])) / 2;
else
d_cur_penalty = sqrt (distances[mid]);
// Better match ?
if (d_cur_penalty < d_best_penalty)
{
d_best_penalty = d_cur_penalty;
// Save the current model/coefficients selection as being the best so far
model_ = selection;
model_coefficients_ = model_coefficients;
}
++iterations_;
if (debug_verbosity_level > 1)
PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_, max_iterations_, d_best_penalty);
}
if (model_.empty ())
{
if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Unable to find a solution!\n");
return (false);
}
// Classify the data points into inliers and outliers
// Sigma = 1.4826 * (1 + 5 / (n-d)) * sqrt (M)
// @note: See "Robust Regression Methods for Computer Vision: A Review"
//double sigma = 1.4826 * (1 + 5 / (sac_model_->getIndices ()->size () - best_model.size ())) * sqrt (d_best_penalty);
//double threshold = 2.5 * sigma;
// Iterate through the 3d points and calculate the distances from them to the model again
sac_model_->getDistancesToModel (model_coefficients_, distances);
// No distances? The model must not respect the user given constraints
if (distances.empty ())
{
PCL_ERROR ("[pcl::LeastMedianSquares::computeModel] The model found failed to verify against the given constraints!\n");
return (false);
}
std::vector<int> &indices = *sac_model_->getIndices ();
if (distances.size () != indices.size ())
{
PCL_ERROR ("[pcl::LeastMedianSquares::computeModel] Estimated distances (%zu) differs than the normal of indices (%zu).\n", distances.size (), indices.size ());
return (false);
}
inliers_.resize (distances.size ());
// Get the inliers for the best model found
n_inliers_count = 0;
for (size_t i = 0; i < distances.size (); ++i)
if (distances[i] <= threshold_)
inliers_[n_inliers_count++] = indices[i];
// Resize the inliers vector
inliers_.resize (n_inliers_count);
if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Model: %zu size, %d inliers.\n", model_.size (), n_inliers_count);
return (true);
}
int
pcl::OBJReader::read (const std::string &file_name, pcl::TextureMesh &mesh,
Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
int &file_version, const int offset)
{
pcl::console::TicToc tt;
tt.tic ();
int data_type;
unsigned int data_idx;
if (readHeader (file_name, mesh.cloud, origin, orientation, file_version, data_type, data_idx, offset))
{
PCL_ERROR ("[pcl::OBJReader::read] Problem reading header!\n");
return (-1);
}
std::ifstream fs;
fs.open (file_name.c_str (), std::ios::binary);
if (!fs.is_open () || fs.fail ())
{
PCL_ERROR ("[pcl::OBJReader::readHeader] Could not open file '%s'! Error : %s\n",
file_name.c_str (), strerror(errno));
fs.close ();
return (-1);
}
// Seek at the given offset
fs.seekg (data_idx, std::ios::beg);
// Get normal_x and rgba fields indices
int normal_x_field = -1;
// std::size_t rgba_field = 0;
for (std::size_t i = 0; i < mesh.cloud.fields.size (); ++i)
if (mesh.cloud.fields[i].name == "normal_x")
{
normal_x_field = i;
break;
}
std::size_t v_idx = 0;
std::size_t vn_idx = 0;
std::size_t vt_idx = 0;
std::size_t f_idx = 0;
std::string line;
std::vector<std::string> st;
std::vector<Eigen::Vector2f> coordinates;
try
{
while (!fs.eof ())
{
getline (fs, line);
// Ignore empty lines
if (line == "")
continue;
// Tokenize the line
std::stringstream sstream (line);
sstream.imbue (std::locale::classic ());
line = sstream.str ();
boost::trim (line);
boost::split (st, line, boost::is_any_of ("\t\r "), boost::token_compress_on);
// Ignore comments
if (st[0] == "#")
continue;
// Vertex
if (st[0] == "v")
{
try
{
for (int i = 1, f = 0; i < 4; ++i, ++f)
{
float value = boost::lexical_cast<float> (st[i]);
memcpy (&mesh.cloud.data[v_idx * mesh.cloud.point_step + mesh.cloud.fields[f].offset],
&value,
sizeof (float));
}
++v_idx;
}
catch (const boost::bad_lexical_cast &e)
{
PCL_ERROR ("Unable to convert %s to vertex coordinates!", line.c_str ());
return (-1);
}
continue;
}
// Vertex normal
if (st[0] == "vn")
{
try
{
for (int i = 1, f = normal_x_field; i < 4; ++i, ++f)
{
float value = boost::lexical_cast<float> (st[i]);
memcpy (&mesh.cloud.data[vn_idx * mesh.cloud.point_step + mesh.cloud.fields[f].offset],
&value,
sizeof (float));
}
++vn_idx;
}
catch (const boost::bad_lexical_cast &e)
{
PCL_ERROR ("Unable to convert line %s to vertex normal!", line.c_str ());
return (-1);
}
continue;
}
// Texture coordinates
if (st[0] == "vt")
{
try
{
Eigen::Vector3f c (0, 0, 0);
for (std::size_t i = 1; i < st.size (); ++i)
c[i-1] = boost::lexical_cast<float> (st[i]);
if (c[2] == 0)
coordinates.push_back (Eigen::Vector2f (c[0], c[1]));
else
coordinates.push_back (Eigen::Vector2f (c[0]/c[2], c[1]/c[2]));
++vt_idx;
}
catch (const boost::bad_lexical_cast &e)
{
PCL_ERROR ("Unable to convert line %s to texture coordinates!", line.c_str ());
return (-1);
}
continue;
}
// Material
if (st[0] == "usemtl")
{
mesh.tex_polygons.push_back (std::vector<pcl::Vertices> ());
mesh.tex_materials.push_back (pcl::TexMaterial ());
for (std::size_t i = 0; i < companions_.size (); ++i)
{
std::vector<pcl::TexMaterial>::const_iterator mat_it = companions_[i].getMaterial (st[1]);
if (mat_it != companions_[i].materials_.end ())
{
mesh.tex_materials.back () = *mat_it;
break;
}
}
// We didn't find the appropriate material so we create it here with name only.
if (mesh.tex_materials.back ().tex_name == "")
mesh.tex_materials.back ().tex_name = st[1];
mesh.tex_coordinates.push_back (coordinates);
coordinates.clear ();
continue;
}
// Face
if (st[0] == "f")
{
//We only care for vertices indices
pcl::Vertices face_v; face_v.vertices.resize (st.size () - 1);
for (std::size_t i = 1; i < st.size (); ++i)
{
int v;
sscanf (st[i].c_str (), "%d", &v);
v = (v < 0) ? v_idx + v : v - 1;
face_v.vertices[i-1] = v;
}
mesh.tex_polygons.back ().push_back (face_v);
++f_idx;
continue;
}
}
}
catch (const char *exception)
{
PCL_ERROR ("[pcl::OBJReader::read] %s\n", exception);
fs.close ();
return (-1);
}
double total_time = tt.toc ();
PCL_DEBUG ("[pcl::OBJReader::read] Loaded %s as a TextureMesh in %g ms with %g points, %g texture materials, %g polygons.\n",
file_name.c_str (), total_time,
v_idx -1, mesh.tex_materials.size (), f_idx -1);
fs.close ();
return (0);
}
template <typename PointT> bool
pcl::visualization::ImageViewer::showCorrespondences (
const pcl::PointCloud<PointT> &source_img,
const pcl::PointCloud<PointT> &target_img,
const pcl::Correspondences &correspondences,
int nth,
const std::string &layer_id)
{
if (correspondences.empty ())
{
PCL_DEBUG ("[pcl::visualization::ImageViewer::addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
return (false);
}
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
if (am_it == layer_map_.end ())
{
PCL_DEBUG ("[pcl::visualization::ImageViewer::addCorrespondences] No layer with ID='%s' found. Creating new one...\n", layer_id.c_str ());
am_it = createLayer (layer_id, source_img.width + target_img.width, std::max (source_img.height, target_img.height), 1.0, false);
}
int src_size = source_img.width * source_img.height * 3;
int tgt_size = target_img.width * target_img.height * 3;
// Set window size
setSize (source_img.width + target_img.width , std::max (source_img.height, target_img.height));
// Set data size
if (data_size_ < (src_size + tgt_size))
{
data_size_ = src_size + tgt_size;
data_.reset (new unsigned char[data_size_]);
}
// Copy data in VTK format
int j = 0;
for (size_t i = 0; i < std::max (source_img.height, target_img.height); ++i)
{
// Still need to copy the source?
if (i < source_img.height)
{
for (size_t k = 0; k < source_img.width; ++k)
{
data_[j++] = source_img[i * source_img.width + k].r;
data_[j++] = source_img[i * source_img.width + k].g;
data_[j++] = source_img[i * source_img.width + k].b;
}
}
else
{
memcpy (&data_[j], 0, source_img.width * 3);
j += source_img.width * 3;
}
// Still need to copy the target?
if (i < source_img.height)
{
for (size_t k = 0; k < target_img.width; ++k)
{
data_[j++] = target_img[i * source_img.width + k].r;
data_[j++] = target_img[i * source_img.width + k].g;
data_[j++] = target_img[i * source_img.width + k].b;
}
}
else
{
memcpy (&data_[j], 0, target_img.width * 3);
j += target_img.width * 3;
}
}
void* data = const_cast<void*> (reinterpret_cast<const void*> (data_.get ()));
vtkSmartPointer<vtkImageData> image = vtkSmartPointer<vtkImageData>::New ();
image->SetDimensions (source_img.width + target_img.width, std::max (source_img.height, target_img.height), 1);
image->SetScalarTypeToUnsignedChar ();
image->SetNumberOfScalarComponents (3);
image->AllocateScalars ();
image->GetPointData ()->GetScalars ()->SetVoidArray (data, data_size_, 1);
vtkSmartPointer<PCLContextImageItem> image_item = vtkSmartPointer<PCLContextImageItem>::New ();
#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION < 10))
// Now create filter and set previously created transformation
algo_->SetInput (image);
algo_->Update ();
image_item->set (0, 0, algo_->GetOutput ());
#else
image_item->set (0, 0, image);
interactor_style_->adjustCamera (image, ren_);
#endif
am_it->actor->GetScene ()->AddItem (image_item);
image_viewer_->SetSize (image->GetDimensions ()[0], image->GetDimensions ()[1]);
// Draw lines between the best corresponding points
for (size_t i = 0; i < correspondences.size (); i += nth)
{
double r, g, b;
getRandomColors (r, g, b);
unsigned char u_r = static_cast<unsigned char> (255.0 * r);
unsigned char u_g = static_cast<unsigned char> (255.0 * g);
unsigned char u_b = static_cast<unsigned char> (255.0 * b);
vtkSmartPointer<context_items::Circle> query_circle = vtkSmartPointer<context_items::Circle>::New ();
query_circle->setColors (u_r, u_g, u_b);
vtkSmartPointer<context_items::Circle> match_circle = vtkSmartPointer<context_items::Circle>::New ();
match_circle->setColors (u_r, u_g, u_b);
vtkSmartPointer<context_items::Line> line = vtkSmartPointer<context_items::Line>::New ();
line->setColors (u_r, u_g, u_b);
float query_x = correspondences[i].index_query % source_img.width;
float match_x = correspondences[i].index_match % target_img.width + source_img.width;
#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION >= 10))
float query_y = correspondences[i].index_query / source_img.width;
float match_y = correspondences[i].index_match / target_img.width;
#else
float query_y = getSize ()[1] - correspondences[i].index_query / source_img.width;
float match_y = getSize ()[1] - correspondences[i].index_match / target_img.width;
#endif
query_circle->set (query_x, query_y, 3.0);
match_circle->set (match_x, match_y, 3.0);
line->set (query_x, query_y, match_x, match_y);
am_it->actor->GetScene ()->AddItem (query_circle);
am_it->actor->GetScene ()->AddItem (match_circle);
am_it->actor->GetScene ()->AddItem (line);
}
return (true);
}
template <typename PointT> void
pcl::ProgressiveMorphologicalFilter<PointT>::extract (std::vector<int>& ground)
{
bool segmentation_is_possible = initCompute ();
if (!segmentation_is_possible)
{
deinitCompute ();
return;
}
// Compute the series of window sizes and height thresholds
std::vector<float> height_thresholds;
std::vector<float> window_sizes;
int iteration = 0;
float window_size = 0.0f;
float height_threshold = 0.0f;
while (window_size < max_window_size_)
{
// Determine the initial window size.
if (exponential_)
window_size = cell_size_ * (2.0f * std::pow (base_, iteration) + 1.0f);
else
window_size = cell_size_ * (2.0f * (iteration+1) * base_ + 1.0f);
// Calculate the height threshold to be used in the next iteration.
if (iteration == 0)
height_threshold = initial_distance_;
else
height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_;
// Enforce max distance on height threshold
if (height_threshold > max_distance_)
height_threshold = max_distance_;
window_sizes.push_back (window_size);
height_thresholds.push_back (height_threshold);
iteration++;
}
// Ground indices are initially limited to those points in the input cloud we
// wish to process
ground = *indices_;
// Progressively filter ground returns using morphological open
for (int i = 0; i < window_sizes.size (); ++i)
{
PCL_DEBUG (" Iteration %d (height threshold = %f, window size = %f)...",
i, height_thresholds[i], window_sizes[i]);
// Limit filtering to those points currently considered ground returns
typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
// Create new cloud to hold the filtered results. Apply the morphological
// opening operation at the current window size.
typename pcl::PointCloud<PointT>::Ptr cloud_f (new pcl::PointCloud<PointT>);
pcl::applyMorphologicalOperator<PointT> (cloud, window_sizes[i], MORPH_OPEN, *cloud_f);
// Find indices of the points whose difference between the source and
// filtered point clouds is less than the current height threshold.
std::vector<int> pt_indices;
for (boost::int32_t p_idx = 0; p_idx < ground.size (); ++p_idx)
{
float diff = cloud->points[p_idx].z - cloud_f->points[p_idx].z;
if (diff < height_thresholds[i])
pt_indices.push_back (ground[p_idx]);
}
// Ground is now limited to pt_indices
ground.swap (pt_indices);
PCL_DEBUG ("ground now has %d points\n", ground.size ());
}
deinitCompute ();
}
