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