template <typename PointT> bool
pcl::visualization::PointCloudColorHandlerRandom<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
{
if (!capable_ || !cloud_)
return (false);
if (!scalars)
scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
scalars->SetNumberOfComponents (3);
vtkIdType nr_points = cloud_->points.size ();
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
// Get a random color
unsigned char* colors = new unsigned char[nr_points * 3];
double r, g, b;
pcl::visualization::getRandomColors (r, g, b);
int r_ = static_cast<int> (pcl_lrint (r * 255.0)),
g_ = static_cast<int> (pcl_lrint (g * 255.0)),
b_ = static_cast<int> (pcl_lrint (b * 255.0));
// Color every point
for (vtkIdType cp = 0; cp < nr_points; ++cp)
{
colors[cp * 3 + 0] = static_cast<unsigned char> (r_);
colors[cp * 3 + 1] = static_cast<unsigned char> (g_);
colors[cp * 3 + 2] = static_cast<unsigned char> (b_);
}
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
return (true);
}
bool
pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
{
if (!capable_ || !cloud_)
return (false);
if (!scalars)
scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
scalars->SetNumberOfComponents (3);
vtkIdType nr_points = cloud_->width * cloud_->height;
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
// Get a random color
unsigned char* colors = new unsigned char[nr_points * 3];
double r, g, b;
pcl::visualization::getRandomColors (r, g, b);
long r_ = pcl_lrint (r * 255.0), g_ = pcl_lrint (g * 255.0), b_ = pcl_lrint (b * 255.0);
// Color every point
for (vtkIdType cp = 0; cp < nr_points; ++cp)
{
colors[cp * 3 + 0] = static_cast<unsigned char> (r_);
colors[cp * 3 + 1] = static_cast<unsigned char> (g_);
colors[cp * 3 + 2] = static_cast<unsigned char> (b_);
}
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
return (true);
}
template <typename PointT> void
pcl::visualization::PointCloudColorHandlerRandom<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
{
if (!capable_)
return;
if (!scalars)
scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
scalars->SetNumberOfComponents (3);
vtkIdType nr_points = cloud_->points.size ();
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
// Get a random color
unsigned char* colors = new unsigned char[nr_points * 3];
double r, g, b;
pcl::visualization::getRandomColors (r, g, b);
int r_ = pcl_lrint (r * 255.0), g_ = pcl_lrint (g * 255.0), b_ = pcl_lrint (b * 255.0);
// Color every point
for (vtkIdType cp = 0; cp < nr_points; ++cp)
{
colors[cp * 3 + 0] = r_;
colors[cp * 3 + 1] = g_;
colors[cp * 3 + 2] = b_;
}
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
}
void
RangeImagePlanar::setDisparityImage (const float* disparity_image, int di_width, int di_height,
float focal_length, float base_line, float desired_angular_resolution)
{
//MEASURE_FUNCTION_TIME;
reset ();
float original_angular_resolution = atanf (0.5f * static_cast<float> (di_width) / static_cast<float> (focal_length)) / (0.5f * static_cast<float> (di_width));
int skip = 1;
if (desired_angular_resolution >= 2.0f*original_angular_resolution)
skip = static_cast<int> (pcl_lrint (floor (desired_angular_resolution / original_angular_resolution)));
setAngularResolution (original_angular_resolution * static_cast<float> (skip));
width = di_width / skip;
height = di_height / skip;
focal_length_x_ = focal_length_y_ = focal_length / static_cast<float> (skip);
focal_length_x_reciprocal_ = focal_length_y_reciprocal_ = 1.0f / focal_length_x_;
center_x_ = static_cast<float> (di_width) / static_cast<float> (2 * skip);
center_y_ = static_cast<float> (di_height) / static_cast<float> (2 * skip);
points.resize (width*height);
//cout << PVARN (*this);
float normalization_factor = static_cast<float> (skip) * focal_length_x_ * base_line;
for (int y=0; y < static_cast<int> (height); ++y)
{
for (int x=0; x < static_cast<int> (width); ++x)
{
PointWithRange& point = getPointNoCheck (x,y);
float disparity = disparity_image[ (y*skip)*di_width + x*skip];
if (disparity <= 0.0f)
{
//std::cout << disparity << ", "<<std::flush;
point = unobserved_point;
continue;
}
point.z = normalization_factor / disparity;
point.x = (static_cast<float> (x) - center_x_) * point.z * focal_length_x_reciprocal_;
point.y = (static_cast<float> (y) - center_y_) * point.z * focal_length_y_reciprocal_;
point.range = point.getVector3fMap ().norm ();
//cout << point<<std::flush;
//// Just a test:
//PointWithRange test_point1;
//calculate3DPoint (float (x), float (y), point.range, test_point1);
//if ( (point.getVector3fMap ()-test_point1.getVector3fMap ()).norm () > 1e-5)
//cerr << "Something's wrong here...\n";
//float image_x, image_y;
//getImagePoint (point.getVector3fMap (), image_x, image_y);
//if (fabsf (image_x-x)+fabsf (image_y-y)>1e-4)
//cerr << PVARC (x)<<PVARC (y)<<PVARC (image_x)<<PVARN (image_y);
}
}
}
void
RangeImagePlanar::setDepthImage (const unsigned short* depth_image, int di_width, int di_height,
float di_center_x, float di_center_y,
float di_focal_length_x, float di_focal_length_y,
float desired_angular_resolution)
{
//MEASURE_FUNCTION_TIME;
reset ();
float original_angular_resolution = asinf (0.5f*static_cast<float> (di_width)/static_cast<float> (di_focal_length_x)) / (0.5f*static_cast<float> (di_width));
int skip = 1;
if (desired_angular_resolution >= 2.0f*original_angular_resolution)
skip = static_cast<int> (pcl_lrint (floor (desired_angular_resolution/original_angular_resolution)));
setAngularResolution (original_angular_resolution * static_cast<float> (skip));
width = di_width / skip;
height = di_height / skip;
focal_length_x_ = di_focal_length_x / static_cast<float> (skip);
focal_length_x_reciprocal_ = 1.0f / focal_length_x_;
focal_length_y_ = di_focal_length_y / static_cast<float> (skip);
focal_length_y_reciprocal_ = 1.0f / focal_length_y_;
center_x_ = static_cast<float> (di_center_x) / static_cast<float> (skip);
center_y_ = static_cast<float> (di_center_y) / static_cast<float> (skip);
points.resize (width * height);
for (int y = 0; y < static_cast<int> (height); ++y)
{
for (int x = 0; x < static_cast<int> (width); ++x)
{
PointWithRange& point = getPointNoCheck (x, y);
float depth = depth_image[ (y*skip)*di_width + x*skip] * 0.001f;
if (depth <= 0.0f || !pcl_isfinite (depth))
{
point = unobserved_point;
continue;
}
point.z = depth;
point.x = (static_cast<float> (x) - center_x_) * point.z * focal_length_x_reciprocal_;
point.y = (static_cast<float> (y) - center_y_) * point.z * focal_length_y_reciprocal_;
point.range = point.getVector3fMap ().norm ();
}
}
}
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> 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);
}
