void
pcl::registration::CorrespondenceRejectorMedianDistance::getRemainingCorrespondences (
const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences)
{
std::vector <double> dists;
dists.resize (original_correspondences.size ());
for (size_t i = 0; i < original_correspondences.size (); ++i)
{
if (data_container_)
dists[i] = data_container_->getCorrespondenceScore (original_correspondences[i]);
else
dists[i] = original_correspondences[i].distance;
}
std::vector <double> nth (dists);
nth_element (nth.begin (), nth.begin () + (nth.size () / 2), nth.end ());
median_distance_ = nth [nth.size () / 2];
unsigned int number_valid_correspondences = 0;
remaining_correspondences.resize (original_correspondences.size ());
for (size_t i = 0; i < original_correspondences.size (); ++i)
if (dists[i] <= median_distance_ * factor_)
remaining_correspondences[number_valid_correspondences++] = original_correspondences[i];
remaining_correspondences.resize (number_valid_correspondences);
}
void
pcl::registration::CorrespondenceRejectorDistance::getRemainingCorrespondences (
const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences)
{
unsigned int number_valid_correspondences = 0;
remaining_correspondences.resize (original_correspondences.size ());
for (size_t i = 0; i < original_correspondences.size (); ++i)
{
if (data_container_)
{
if (data_container_->getCorrespondenceScore (original_correspondences[i]) < max_distance_)
{
remaining_correspondences[number_valid_correspondences] = original_correspondences[i];
++number_valid_correspondences;
}
}
else
{
if (original_correspondences[i].distance < max_distance_)
{
remaining_correspondences[number_valid_correspondences] = original_correspondences[i];
++number_valid_correspondences;
}
}
}
remaining_correspondences.resize (number_valid_correspondences);
}
template <typename PointSource, typename PointTarget> inline void
TransformationEstimationJointOptimize<PointSource, PointTarget>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
const pcl::Correspondences &correspondences_dfp,
float alpha_arg,
Eigen::Matrix4f &transformation_matrix)
{
const int nr_correspondences = (int)correspondences.size();
std::vector<int> indices_src(nr_correspondences);
std::vector<int> indices_tgt(nr_correspondences);
for (int i = 0; i < nr_correspondences; ++i)
{
indices_src[i] = correspondences[i].index_query;
indices_tgt[i] = correspondences[i].index_match;
}
const int nr_correspondences_dfp = (int)correspondences_dfp.size();
std::vector<int> indices_src_dfp(nr_correspondences_dfp);
std::vector<int> indices_tgt_dfp(nr_correspondences_dfp);
for (int i = 0; i < nr_correspondences_dfp; ++i)
{
indices_src_dfp[i] = correspondences_dfp[i].index_query;
indices_tgt_dfp[i] = correspondences_dfp[i].index_match;
}
estimateRigidTransformation(cloud_src, indices_src, indices_src_dfp, cloud_tgt, indices_tgt, indices_tgt_dfp,
alpha_arg, transformation_matrix);
}
inline void
pcl::registration::getMatchIndices (const pcl::Correspondences& correspondences, std::vector<int>& indices)
{
indices.resize (correspondences.size ());
for (size_t i = 0; i < correspondences.size (); ++i)
indices[i] = correspondences[i].index_match;
}
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
Matrix4 &transformation_matrix) const
{
ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
std::vector<Scalar> weights (correspondences.size ());
for (size_t i = 0; i < correspondences.size (); ++i)
weights[i] = correspondences[i].weight;
typename std::vector<Scalar>::const_iterator weights_it = weights.begin ();
estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
}
inline void
pcl::registration::getCorDistMeanStd (const pcl::Correspondences &correspondences, double &mean, double &stddev)
{
if (correspondences.empty ())
return;
double sum = 0, sq_sum = 0;
for (size_t i = 0; i < correspondences.size (); ++i)
{
sum += correspondences[i].distance;
sq_sum += correspondences[i].distance * correspondences[i].distance;
}
mean = sum / static_cast<double> (correspondences.size ());
double variance = (sq_sum - sum * sum / static_cast<double> (correspondences.size ())) / static_cast<double> (correspondences.size () - 1);
stddev = sqrt (variance);
}
Eigen::Matrix4f SHOTObjectGenerator::computeTransformationSAC(const pcl::PointCloud<PointXYZSHOT>::ConstPtr &cloud_src, const pcl::PointCloud<PointXYZSHOT>::ConstPtr &cloud_trg,
const pcl::CorrespondencesConstPtr& correspondences, pcl::Correspondences& inliers)
{
CLOG(LTRACE) << "Computing SAC" << std::endl ;
pcl::registration::CorrespondenceRejectorSampleConsensus<PointXYZSHOT> sac ;
sac.setInputSource(cloud_src) ;
sac.setInputTarget(cloud_trg) ;
sac.setInlierThreshold(0.001f) ;
sac.setMaximumIterations(2000) ;
sac.setInputCorrespondences(correspondences) ;
sac.getCorrespondences(inliers) ;
CLOG(LINFO) << "SAC inliers " << inliers.size();
if ( ((float)inliers.size()/(float)correspondences->size()) >85)
return Eigen::Matrix4f::Identity();
return sac.getBestTransformation() ;
}
void
pcl::getRejectedQueryIndices (const pcl::Correspondences &correspondences_before,
const pcl::Correspondences &correspondences_after,
std::vector<int>& indices,
bool presorting_required)
{
indices.clear();
const int nr_correspondences_before = static_cast<int> (correspondences_before.size ());
const int nr_correspondences_after = static_cast<int> (correspondences_after.size ());
if (nr_correspondences_before == 0)
return;
else if (nr_correspondences_after == 0)
{
indices.resize(nr_correspondences_before);
for (int i = 0; i < nr_correspondences_before; ++i)
indices[i] = correspondences_before[i].index_query;
return;
}
std::vector<int> indices_before (nr_correspondences_before);
for (int i = 0; i < nr_correspondences_before; ++i)
indices_before[i] = correspondences_before[i].index_query;
std::vector<int> indices_after (nr_correspondences_after);
for (int i = 0; i < nr_correspondences_after; ++i)
indices_after[i] = correspondences_after[i].index_query;
if (presorting_required)
{
std::sort (indices_before.begin (), indices_before.end ());
std::sort (indices_after.begin (), indices_after.end ());
}
set_difference (
indices_before.begin (), indices_before.end (),
indices_after.begin (), indices_after.end (),
inserter (indices, indices.begin ()));
}
void
pcl::registration::CorrespondenceRejectionOrganizedBoundary::getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences)
{
pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud = boost::static_pointer_cast<pcl::registration::DataContainer<pcl::PointXYZ, pcl::PointNormal> >(data_container_)->getInputTarget ();
if (!cloud->isOrganized ())
{
PCL_ERROR ("[pcl::registration::CorrespondenceRejectionOrganizedBoundary::getRemainingCorrespondences] The target cloud is not organized.\n");
remaining_correspondences.clear ();
return;
}
remaining_correspondences.reserve (original_correspondences.size ());
for (size_t c_i = 0; c_i < original_correspondences.size (); ++c_i)
{
/// Count how many NaNs bound the target point
int x = original_correspondences[c_i].index_match % cloud->width;
int y = original_correspondences[c_i].index_match / cloud->width;
int nan_count_tgt = 0;
for (int x_d = -window_size_/2; x_d <= window_size_/2; ++x_d)
for (int y_d = -window_size_/2; y_d <= window_size_/2; ++y_d)
if (x + x_d >= 0 && x + x_d < cloud->width &&
y + y_d >= 0 && y + y_d < cloud->height)
{
if (!pcl_isfinite ((*cloud)(x + x_d, y + y_d).z) ||
fabs ((*cloud)(x, y).z - (*cloud)(x + x_d, y + y_d).z) > depth_step_threshold_)
nan_count_tgt ++;
}
if (nan_count_tgt >= boundary_nans_threshold_)
continue;
/// The correspondence passes both tests, add it to the filtered set of correspondences
remaining_correspondences.push_back (original_correspondences[c_i]);
}
}
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::bruteForceCorrespondences (
int idx1,
int idx2,
pcl::Correspondences &pairs)
{
const float max_norm_diff = 0.5f * max_norm_diff_ * M_PI / 180.f;
// calculate reference segment distance and normal angle
float ref_dist = pcl::euclideanDistance (target_->points[idx1], target_->points[idx2]);
float ref_norm_angle = (use_normals_ ? (target_normals_->points[idx1].getNormalVector3fMap () -
target_normals_->points[idx2].getNormalVector3fMap ()).norm () : 0.f);
// loop over all pairs of points in source point cloud
std::vector <int>::iterator it_out = source_indices_->begin (), it_out_e = source_indices_->end () - 1;
std::vector <int>::iterator it_in, it_in_e = source_indices_->end ();
for ( ; it_out != it_out_e; it_out++)
{
it_in = it_out + 1;
const PointSource *pt1 = &(*input_)[*it_out];
for ( ; it_in != it_in_e; it_in++)
{
const PointSource *pt2 = &(*input_)[*it_in];
// check point distance compared to reference dist (from base)
float dist = pcl::euclideanDistance (*pt1, *pt2);
if (std::abs(dist - ref_dist) < max_pair_diff_)
{
// add here normal evaluation if normals are given
if (use_normals_)
{
const NormalT *pt1_n = &(source_normals_->points[*it_out]);
const NormalT *pt2_n = &(source_normals_->points[*it_in]);
float norm_angle_1 = (pt1_n->getNormalVector3fMap () - pt2_n->getNormalVector3fMap ()).norm ();
float norm_angle_2 = (pt1_n->getNormalVector3fMap () + pt2_n->getNormalVector3fMap ()).norm ();
float norm_diff = std::min <float> (std::abs (norm_angle_1 - ref_norm_angle), std::abs (norm_angle_2 - ref_norm_angle));
if (norm_diff > max_norm_diff)
continue;
}
pairs.push_back (pcl::Correspondence (*it_in, *it_out, dist));
pairs.push_back (pcl::Correspondence (*it_out, *it_in, dist));
}
}
}
// return success if at least one correspondence was found
return (pairs.size () == 0 ? -1 : 0);
}
void
pcl::registration::CorrespondenceRejectorSurfaceNormal::getRemainingCorrespondences (
const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences)
{
if (!data_container_)
{
PCL_ERROR ("[pcl::registratin::%s::getRemainingCorrespondences] DataContainer object is not initialized!\n", getClassName ().c_str ());
return;
}
unsigned int number_valid_correspondences = 0;
remaining_correspondences.resize (original_correspondences.size ());
// Test each correspondence
for (size_t i = 0; i < original_correspondences.size (); ++i)
{
if (boost::static_pointer_cast<DataContainer<pcl::PointXYZ, pcl::PointNormal> >
(data_container_)->getCorrespondenceScoreFromNormals (original_correspondences[i]) > threshold_)
remaining_correspondences[number_valid_correspondences++] = original_correspondences[i];
}
remaining_correspondences.resize (number_valid_correspondences);
}
template <typename PointSource, typename PointTarget> inline void
TransformationEstimationJointOptimize<PointSource, PointTarget>::setCorrespondecesDFP ( pcl::Correspondences correspondences_dfp ) {
const int nr_correspondences_dfp = (int)correspondences_dfp.size();
indices_src_dfp_.resize(nr_correspondences_dfp);
indices_tgt_dfp_.resize(nr_correspondences_dfp);
for (int i = 0; i < nr_correspondences_dfp; ++i)
{
indices_src_dfp_[i] = correspondences_dfp[i].index_query;
indices_tgt_dfp_[i] = correspondences_dfp[i].index_match;
}
// Update flags
indices_src_dfp_set_ = true;
indices_tgt_dfp_set_ = true;
}
void
pcl::registration::CorrespondenceRejectorVarTrimmed::getRemainingCorrespondences (
const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences)
{
std::vector <double> dists;
dists.resize (original_correspondences.size ());
for (size_t i = 0; i < original_correspondences.size (); ++i)
{
if (data_container_)
{
dists[i] = data_container_->getCorrespondenceScore (original_correspondences[i]);
}
else
{
dists[i] = original_correspondences[i].distance;
}
}
factor_ = optimizeInlierRatio (dists);
nth_element (dists.begin (), dists.begin () + int (double (dists.size ()) * factor_), dists.end ());
trimmed_distance_ = dists [int (double (dists.size ()) * factor_)];
unsigned int number_valid_correspondences = 0;
remaining_correspondences.resize (original_correspondences.size ());
for (size_t i = 0; i < original_correspondences.size (); ++i)
{
if ( dists[i] < trimmed_distance_)
{
remaining_correspondences[number_valid_correspondences] = original_correspondences[i];
++number_valid_correspondences;
}
}
remaining_correspondences.resize (number_valid_correspondences);
}
template <typename PointSource, typename PointTarget, typename MatScalar> inline void
pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
Matrix4 &transformation_matrix) const
{
const int nr_correspondences = static_cast<const int> (correspondences.size ());
std::vector<int> indices_src (nr_correspondences);
std::vector<int> indices_tgt (nr_correspondences);
for (int i = 0; i < nr_correspondences; ++i)
{
indices_src[i] = correspondences[i].index_query;
indices_tgt[i] = correspondences[i].index_match;
}
estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
}
template <typename PointSource, typename PointTarget> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
Eigen::Matrix4f &transformation_matrix)
{
size_t nr_points = correspondences.size ();
// Approximate as a linear least squares problem
Eigen::MatrixXf A (nr_points, 6);
Eigen::MatrixXf b (nr_points, 1);
for (size_t i = 0; i < nr_points; ++i)
{
const int & src_idx = correspondences[i].index_query;
const int & tgt_idx = correspondences[i].index_match;
const float & sx = cloud_src.points[src_idx].x;
const float & sy = cloud_src.points[src_idx].y;
const float & sz = cloud_src.points[src_idx].z;
const float & dx = cloud_tgt.points[tgt_idx].x;
const float & dy = cloud_tgt.points[tgt_idx].y;
const float & dz = cloud_tgt.points[tgt_idx].z;
const float & nx = cloud_tgt.points[tgt_idx].normal[0];
const float & ny = cloud_tgt.points[tgt_idx].normal[1];
const float & nz = cloud_tgt.points[tgt_idx].normal[2];
A (i, 0) = nz*sy - ny*sz;
A (i, 1) = nx*sz - nz*sx;
A (i, 2) = ny*sx - nx*sy;
A (i, 3) = nx;
A (i, 4) = ny;
A (i, 5) = nz;
b (i, 0) = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
}
// Solve A*x = b
Eigen::VectorXf x = A.colPivHouseholderQr ().solve (b);
// Construct the transformation matrix from x
constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
}
void
pcl::registration::CorrespondenceRejectorSurfaceNormal::getRemainingCorrespondences (
const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences)
{
if (!data_container_)
{
PCL_ERROR ("[pcl::registratin::%s::getRemainingCorrespondences] DataContainer object is not initialized!\n", getClassName ().c_str ());
return;
}
unsigned int number_valid_correspondences = 0;
remaining_correspondences.resize (original_correspondences.size ());
// Test each correspondence
for (const auto &original_correspondence : original_correspondences)
{
if (data_container_->getCorrespondenceScoreFromNormals (original_correspondence) > threshold_)
remaining_correspondences[number_valid_correspondences++] = original_correspondence;
}
remaining_correspondences.resize (number_valid_correspondences);
}
template <typename PointT> void
pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::getRemainingCorrespondences (
const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences)
{
if (!input_)
{
PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input cloud dataset was given!\n", getClassName ().c_str ());
return;
}
if (!target_)
{
PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input target dataset was given!\n", getClassName ().c_str ());
return;
}
if (save_inliers_)
inlier_indices_.clear ();
int nr_correspondences = static_cast<int> (original_correspondences.size ());
std::vector<int> source_indices (nr_correspondences);
std::vector<int> target_indices (nr_correspondences);
// Copy the query-match indices
for (size_t i = 0; i < original_correspondences.size (); ++i)
{
source_indices[i] = original_correspondences[i].index_query;
target_indices[i] = original_correspondences[i].index_match;
}
// from pcl/registration/icp.hpp:
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 pcl::SampleConsensusModelRegistration<PointT>::Ptr SampleConsensusModelRegistrationPtr;
SampleConsensusModelRegistrationPtr model;
model.reset (new pcl::SampleConsensusModelRegistration<PointT> (input_, source_indices));
// Pass the target_indices
model->setInputTarget (target_, target_indices);
// Create a RANSAC model
pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_);
sac.setMaxIterations (max_iterations_);
// Compute the set of inliers
if (!sac.computeModel ())
{
remaining_correspondences = original_correspondences;
best_transformation_.setIdentity ();
return;
}
else
{
if (refine_ && !sac.refineModel ())
{
PCL_ERROR ("[pcl::registration::CorrespondenceRejectorSampleConsensus::getRemainingCorrespondences] Could not refine the model! Returning an empty solution.\n");
return;
}
std::vector<int> inliers;
sac.getInliers (inliers);
if (inliers.size () < 3)
{
remaining_correspondences = original_correspondences;
best_transformation_.setIdentity ();
return;
}
boost::unordered_map<int, int> index_to_correspondence;
for (int i = 0; i < nr_correspondences; ++i)
index_to_correspondence[original_correspondences[i].index_query] = i;
remaining_correspondences.resize (inliers.size ());
for (size_t i = 0; i < inliers.size (); ++i)
remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]];
if (save_inliers_)
{
inlier_indices_.reserve (inliers.size ());
for (const int &inlier : inliers)
inlier_indices_.push_back (index_to_correspondence[inlier]);
}
// get best transformation
Eigen::VectorXf model_coefficients;
sac.getModelCoefficients (model_coefficients);
best_transformation_.row (0) = model_coefficients.segment<4>(0);
best_transformation_.row (1) = model_coefficients.segment<4>(4);
best_transformation_.row (2) = model_coefficients.segment<4>(8);
best_transformation_.row (3) = model_coefficients.segment<4>(12);
}
}
}
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::registration::CorrespondenceRejectorProcrustes<PointT>::getRemainingCorrespondences (
const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences)
{
int nr_correspondences = (int)original_correspondences.size ();
std::vector<int> source_indices (nr_correspondences);
std::vector<int> target_indices (nr_correspondences);
// Copy the query-match indices
for (size_t i = 0; i < original_correspondences.size (); ++i)
{
source_indices[i] = original_correspondences[i].index_query;
target_indices[i] = original_correspondences[i].index_match;
}
// from pcl/registration/icp.hpp:
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 pcl::SampleConsensusModelNonRigid<PointT>::Ptr SampleConsensusModelNonRigidPtr;
SampleConsensusModelNonRigidPtr model;
model.reset (new pcl::SampleConsensusModelNonRigid<PointT> (input_, source_indices));
// Pass the target_indices
model->setInputTarget (target_, target_indices);
// Create a RANSAC model
pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_);
sac.setMaxIterations (max_iterations_);
// Compute the set of inliers
if (!sac.computeModel ())
{
remaining_correspondences = original_correspondences;
best_transformation_.setIdentity ();
return;
}
else
{
std::vector<int> inliers;
sac.getInliers (inliers);
if (inliers.size () < 3)
{
remaining_correspondences = original_correspondences;
best_transformation_.setIdentity ();
return;
}
boost::unordered_map<int, int> index_to_correspondence;
for (int i = 0; i < nr_correspondences; ++i)
index_to_correspondence[original_correspondences[i].index_query] = i;
remaining_correspondences.resize (inliers.size ());
for (size_t i = 0; i < inliers.size (); ++i)
remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]];
// get best transformation
Eigen::VectorXf model_coefficients;
sac.getModelCoefficients (model_coefficients);
best_transformation_.row (0) = model_coefficients.segment<4>(0);
best_transformation_.row (1) = model_coefficients.segment<4>(4);
best_transformation_.row (2) = model_coefficients.segment<4>(8);
best_transformation_.row (3) = model_coefficients.segment<4>(12);
}
}
}
template <typename PointSource, typename PointTarget, typename Scalar> void
VoxelBasedCorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences (pcl::Correspondences &correspondences,
double /*max_distance*/) {
if (!initCompute ())
return;
//find correspondences between input and target_ by looking into the distance transform thing...
pcl::Correspondence corr;
unsigned int nr_valid_correspondences = 0;
//std::vector<int> indices = (*indices_);
//std::random_shuffle(indices.begin(), indices.end());
//indices.resize(std::min(static_cast<int>(indices.size()),100));
correspondences.resize (indices_->size ());
int idx_match;
float distance;
float color_distance = -1.f;
for(size_t i=0; i < correspondences.size(); i++) {
color_distance = -1.f;
vgdt_target_->getCorrespondence(input_->points[(*indices_)[i]], &idx_match, &distance, sigma_, &color_distance);
if(idx_match < 0)
continue;
if(distance > max_distance_)
continue;
if(sigma_ != -1.f && color_distance != -1.f) {
if(color_distance < max_color_distance_)
{
//PCL_WARN("Rejected because color %f\n", color_distance, max_color_distance_);
continue;
}
}
/*corr.index_query = static_cast<int>(indices[i]);
corr.index_match = idx_match;
corr.distance = distance;//min_dist;*/
correspondences[nr_valid_correspondences].index_query = static_cast<int>((*indices_)[i]);
correspondences[nr_valid_correspondences].index_match = idx_match;
correspondences[nr_valid_correspondences].distance = distance;
nr_valid_correspondences++;
}
/*boost::shared_ptr<pcl::GeometricConsistencyGrouping<PointSource, PointTarget> >
gcg_alg (new pcl::GeometricConsistencyGrouping<PointSource, PointTarget>);
typename pcl::PointCloud<PointSource>::Ptr model_cloud(new pcl::PointCloud<PointSource>);
vgdt_target_->getVoxelizedCloud(model_cloud);
gcg_alg->setGCThreshold (10);
gcg_alg->setGCSize (0.01);
//gcg_alg->setSceneCloud (model_cloud);
//gcg_alg->setInputCloud (input_);
gcg_alg->setSceneCloud (input_);
gcg_alg->setInputCloud (model_cloud);
pcl::CorrespondencesPtr corr_ptr;
corr_ptr.reset(new pcl::Correspondences(correspondences));
gcg_alg->setModelSceneCorrespondences (corr_ptr);
std::vector < pcl::Correspondences > corresp_clusters;
gcg_alg->cluster (corresp_clusters);
std::cout << "correspondence clusters:" << corresp_clusters.size() << std::endl;*/
//std::cout << "Number of valid correspondences:" << indices_->size () << " " << nr_valid_correspondences << std::endl;
//PCL_WARN("Number of valid correspondences... %d %d\n", indices_->size (), nr_valid_correspondences);
correspondences.resize (nr_valid_correspondences);
deinitCompute ();
}
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::determineBaseMatches (
const std::vector <int> &base_indices,
std::vector <std::vector <int> > &matches,
const pcl::Correspondences &pairs_a,
const pcl::Correspondences &pairs_b,
const float (&ratio)[2])
{
// calculate edge lengths of base
float dist_base[4];
dist_base[0] = pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[2]]);
dist_base[1] = pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[3]]);
dist_base[2] = pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[2]]);
dist_base[3] = pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[3]]);
// loop over first point pair correspondences and store intermediate points 'e' in new point cloud
PointCloudSourcePtr cloud_e (new PointCloudSource);
cloud_e->resize (pairs_a.size () * 2);
PointCloudSourceIterator it_pt = cloud_e->begin ();
for (pcl::Correspondences::const_iterator it_pair = pairs_a.begin (), it_pair_e = pairs_a.end () ; it_pair != it_pair_e; it_pair++)
{
const PointSource *pt1 = &(input_->points[it_pair->index_match]);
const PointSource *pt2 = &(input_->points[it_pair->index_query]);
// calculate intermediate points using both ratios from base (r1,r2)
for (int i = 0; i < 2; i++, it_pt++)
{
it_pt->x = pt1->x + ratio[i] * (pt2->x - pt1->x);
it_pt->y = pt1->y + ratio[i] * (pt2->y - pt1->y);
it_pt->z = pt1->z + ratio[i] * (pt2->z - pt1->z);
}
}
// initialize new kd tree of intermediate points from first point pair correspondences
KdTreeReciprocalPtr tree_e (new KdTreeReciprocal);
tree_e->setInputCloud (cloud_e);
std::vector <int> ids;
std::vector <float> dists_sqr;
// loop over second point pair correspondences
for (pcl::Correspondences::const_iterator it_pair = pairs_b.begin (), it_pair_e = pairs_b.end () ; it_pair != it_pair_e; it_pair++)
{
const PointTarget *pt1 = &(input_->points[it_pair->index_match]);
const PointTarget *pt2 = &(input_->points[it_pair->index_query]);
// calculate intermediate points using both ratios from base (r1,r2)
for (int i = 0; i < 2; i++)
{
PointTarget pt_e;
pt_e.x = pt1->x + ratio[i] * (pt2->x - pt1->x);
pt_e.y = pt1->y + ratio[i] * (pt2->y - pt1->y);
pt_e.z = pt1->z + ratio[i] * (pt2->z - pt1->z);
// search for corresponding intermediate points
tree_e->radiusSearch (pt_e, coincidation_limit_, ids, dists_sqr);
for (std::vector <int>::iterator it = ids.begin (), it_e = ids.end (); it != it_e; it++)
{
std::vector <int> match_indices (4);
match_indices[0] = pairs_a[static_cast <int> (std::floor ((float)(*it/2.f)))].index_match;
match_indices[1] = pairs_a[static_cast <int> (std::floor ((float)(*it/2.f)))].index_query;
match_indices[2] = it_pair->index_match;
match_indices[3] = it_pair->index_query;
// EDITED: added coarse check of match based on edge length (due to rigid-body )
if (checkBaseMatch (match_indices, dist_base) < 0)
continue;
matches.push_back (match_indices);
}
}
}
// return unsuccessfull if no match was found
return (matches.size () > 0 ? 0 : -1);
}
template <typename PointT> void
pcl::registration::CorrespondenceRejectorSampleConsensus2D<PointT>::getRemainingCorrespondences (
const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences)
{
if (!input_)
{
PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input cloud dataset was given!\n", getClassName ().c_str ());
return;
}
if (!target_)
{
PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input target dataset was given!\n", getClassName ().c_str ());
return;
}
if (projection_matrix_ == Eigen::Matrix3f::Identity ())
{
PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Intrinsic camera parameters not given!\n", getClassName ().c_str ());
return;
}
int nr_correspondences = static_cast<int> (original_correspondences.size ());
std::vector<int> source_indices (nr_correspondences);
std::vector<int> target_indices (nr_correspondences);
// Copy the query-match indices
for (size_t i = 0; i < original_correspondences.size (); ++i)
{
source_indices[i] = original_correspondences[i].index_query;
target_indices[i] = original_correspondences[i].index_match;
}
// from pcl/registration/icp.hpp:
std::vector<int> source_indices_good;
std::vector<int> target_indices_good;
// From the set of correspondences found, attempt to remove outliers
typename pcl::SampleConsensusModelRegistration2D<PointT>::Ptr model (new pcl::SampleConsensusModelRegistration2D<PointT> (input_, source_indices));
// Pass the target_indices
model->setInputTarget (target_, target_indices);
model->setProjectionMatrix (projection_matrix_);
// Create a RANSAC model
pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_);
sac.setMaxIterations (max_iterations_);
// Compute the set of inliers
if (!sac.computeModel ())
{
PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Error computing model! Returning the original correspondences...\n", getClassName ().c_str ());
remaining_correspondences = original_correspondences;
best_transformation_.setIdentity ();
return;
}
else
{
if (refine_ && !sac.refineModel (2.0))
PCL_WARN ("[pcl::registration::%s::getRemainingCorrespondences] Error refining model!\n", getClassName ().c_str ());
std::vector<int> inliers;
sac.getInliers (inliers);
if (inliers.size () < 3)
{
PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Less than 3 correspondences found!\n", getClassName ().c_str ());
remaining_correspondences = original_correspondences;
best_transformation_.setIdentity ();
return;
}
boost::unordered_map<int, int> index_to_correspondence;
for (int i = 0; i < nr_correspondences; ++i)
index_to_correspondence[original_correspondences[i].index_query] = i;
remaining_correspondences.resize (inliers.size ());
for (size_t i = 0; i < inliers.size (); ++i)
remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]];
// get best transformation
Eigen::VectorXf model_coefficients;
sac.getModelCoefficients (model_coefficients);
best_transformation_.row (0) = model_coefficients.segment<4>(0);
best_transformation_.row (1) = model_coefficients.segment<4>(4);
best_transformation_.row (2) = model_coefficients.segment<4>(8);
best_transformation_.row (3) = model_coefficients.segment<4>(12);
}
}