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 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> 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);
}
}
}
void IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
{
// Allocate enough space to hold the results
nr_iterations_ = 0;
converged_ = false;
output_ = &output;
assert(target_->points.size() && output.points.size());
assert(wInput_.size() && wOutput_.size());
float emd_change;
int flowSize = wInput_.size() + wOutput_.size() - 1;
flow_ = new flow_t[flowSize];
std::vector<int*> samples;
// samples.push_back
const int RAND_NUM = 3;
const int RAND_TOTAL = 6;
int a[] = { 0, 1, 2};
int b[] = { 0, 2, 1};
int c[] = { 1, 0, 2};
int d[] = { 1, 2, 0};
int e[] = { 2, 1, 0};
int f[] = { 2, 0, 1};
samples.push_back(a);
samples.push_back(b);
samples.push_back(c);
samples.push_back(d);
samples.push_back(e);
samples.push_back(f);
std::vector<pair<float,int>> vecOutput;
std::vector<pair<float,int>> spOutput;
std::sort(wInput_.begin(), wInput_.end(), std::greater<float>());
std::sort(wOutput_.begin(), wOutput_.end(), std::greater<float>());
int flag = 0;
PointCloudPtr pos(new pcl::PointCloud<PointT>);
for (auto &sample: samples) {
/// @todo to interate over all possible case
/// rather than sample 100 times
/// reset condition
bool stop = false;
nr_iterations_ = 0;
emd_change = 10000;
/// random sample initial correspondences
/// sampled first MINIMUM_CRSP elements in #output
myCorrespondence_.clear();
const int MINIMUM_CRSP = 3;
// copy the first MINIMUM_CRSP elements in #output
for (int i = 0; i < RAND_NUM; i++) {
myCorrespondence_.insert(BiValue(i, sample[i]));
}
assert(myCorrespondence_.size() == MINIMUM_CRSP);
// std::cout<<"*************correspondences*****"<<std::endl;
// m_util::print_map(myCorrespondence_);
// std::cout<<"*********************************"<<std::endl;
/// repeat until convergence
while (!stop)
{
/// if converged
if (emd_change < 0.001)
{
stop = true;
converged_ = true;
break;
}
if (nr_iterations_ >= max_iterations_ ) {
stop = true;
break;
}
/// transform the points
int cnt = myCorrespondence_.size();
// ignore this sample
if (cnt < min_number_correspondences_)
break;
std::vector<int> source_indices (cnt); // output
std::vector<int> target_indices (cnt); // input
int idx = 0;
for(auto v : myCorrespondence_.left) {
source_indices[idx] = v.second;
target_indices[idx] = v.first;
idx++;
}
// Estimate the transform
transformation_estimation_->estimateRigidTransformation (output, source_indices,
*target_, target_indices, transformation_);
// Tranform the data
transformPointCloud (output, output, transformation_);
// Obtain the final transformation
final_transformation_ = transformation_ * final_transformation_;
nr_iterations_++;
/// calcuate EMD distance
for (int i = 0; i < flowSize; i++) {
flow_[i].amount = 0;
}
float e = emd_wapper(target_->points, wInput_, output.points, wOutput_, flow_, dist);
emd_change = abs(e - pre_EMD_);
pre_EMD_ = e;
/// set up the new correspondences
// sort the flow according to the weight
std::stable_sort(flow_, flow_ + flowSize);
// I don't know why #std::sort cause a bug here,
// It would modify #from or #to member of flow_t struct
// std::sort(flow_, flow_ + flowSize);
myCorrespondence_.clear();
for (int i = 0; i < flowSize; i++) {
myCorrespondence_.insert(BiValue(flow_[i].from, flow_[i].to));
// maximum number of correspondences
/// @todo arguments
if(myCorrespondence_.size() == 5)
break;
}
// std::cout<<"*********************************"<<std::endl;
// for (int i = 0; i < flowSize; i++) {
// std::cout<<flow_[i].from<<" "<<flow_[i].to<<" "<<flow_[i].amount<<std::endl;
// }
// std::cout<<"***********----------************"<<std::endl;
// for(auto p : myCorrespondence_.left){
// std::cout<<p.first<<" "<<p.second<<std::endl;
// }
// std::cout<<myCorrespondence_.size()<<std::endl;
// assert(myCorrespondence_.size() >= MINIMUM_CRSP);
}
if(pre_EMD_ < best_EMD_) {
if (visualize_EMD)
pcl::copyPointCloud(output, *pos);
best_EMD_ = pre_EMD_;
}
}
if (visualize_EMD && pos->points.size() > 0)
pcl::copyPointCloud(*pos, output);
}
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);
}
}
typename PointCloud<PointT>::Ptr transform(
const typename PointCloud<PointT>::Ptr source,
const typename PointCloud<PointT>::Ptr target,boost::shared_ptr<Configuration> configuration)
{
source_cloud = *source;
target_cloud = *target;
cout << "[PFHTransformStrategy::transform] Setting source cloud: "
<< source->size() << " points" << endl;
cout << "[PFHTransformStrategy::transform] Setting target cloud: "
<< target->size() << " points" << endl;
typename PointCloud<PointT>::Ptr transformed(new PointCloud<PointT>);
typename PointCloud<PointXYZRGB>::Ptr source_points(source);
typename PointCloud<PointXYZRGB>::Ptr source_filtered(
new PointCloud<PointT>);
PointCloud<Normal>::Ptr source_normals(new PointCloud<Normal>);
PointCloud<PointWithScale>::Ptr source_keypoints(
new PointCloud<PointWithScale>);
PointCloud<PFHSignature125>::Ptr source_descriptors(
new PointCloud<PFHSignature125>);
typename PointCloud<PointT>::Ptr target_points(target);
typename PointCloud<PointT>::Ptr target_filtered(
new PointCloud<PointT>);
PointCloud<Normal>::Ptr target_normals(
new PointCloud<Normal>);
PointCloud<PointWithScale>::Ptr target_keypoints(
new PointCloud<PointWithScale>);
PointCloud<PFHSignature125>::Ptr target_descriptors(
new PointCloud<PFHSignature125>);
cout << "[PFHTransformStrategy::transform] Downsampling source and "
<< "target clouds..." << endl;
//filter(source_points, source_filtered, 0.01f);
//filter(target_points, target_filtered, 0.01f);
source_filtered=source_points;
target_filtered=target_points;
cout << "[PFHTransformStrategy::transform] Creating normals for "
<< "source and target cloud..." << endl;
create_normals<Normal>(source_filtered, source_normals);
create_normals<Normal>(target_filtered, target_normals);
cout << "[PFHTransformStrategy::transform] Finding keypoints in "
<< "source and target cloud..." << endl;
detect_keypoints(source_filtered, source_keypoints);
detect_keypoints(target_filtered, target_keypoints);
for(PointWithScale p: source_keypoints->points){
cout<<"keypoint "<<p;
}
vector<int> source_indices(source_keypoints->points.size());
vector<int> target_indices(target_keypoints->points.size());
cout << "[PFHTransformStrategy::transform] Computing PFH features "
<< "for source and target cloud..." << endl;
compute_PFH_features_at_keypoints(source_filtered, source_normals,source_keypoints,
source_descriptors, target_indices);
compute_PFH_features_at_keypoints(target_filtered, target_normals,target_keypoints,
target_descriptors, target_indices);
vector<int> correspondences;
vector<float> correspondence_scores;
find_feature_correspondence(source_descriptors, target_descriptors,
correspondences, correspondence_scores);
cout << "correspondences: " << correspondences.size() << endl;
cout << "c. scores: " << correspondence_scores.size() << endl;
cout << "First cloud: Found " << source_keypoints->size() << " keypoints "
<< "out of " << source_filtered->size() << " total points." << endl;
cout << "Second cloud: Found " << target_keypoints->size() << " keypoints"
<< " out of " << target_filtered->size() << " total points." << endl;
// Start with the actual transformation. Yeay :)
TransformationFromCorrespondences tfc;
tfc.reset();
vector<int> sorted_scores;
cv::sortIdx(correspondence_scores, sorted_scores, CV_SORT_EVERY_ROW);
vector<float> tmp(correspondence_scores);
sort(tmp.begin(), tmp.end());
float median_score = tmp[tmp.size() / 2];
vector<int> fidx;
vector<int> fidxt;
Eigen::Vector3f source_position(0, 0, 0);
Eigen::Vector3f target_position(0, 0, 0);
for (size_t i = 0; i < correspondence_scores.size(); i++) {
int index = sorted_scores[i];
if (median_score >= correspondence_scores[index]) {
source_position[0] = source_keypoints->points[index].x;
source_position[1] = source_keypoints->points[index].y;
source_position[2] = source_keypoints->points[index].z;
target_position[0] = target_keypoints->points[index].x;
target_position[1] = target_keypoints->points[index].y;
target_position[2] = target_keypoints->points[index].z;
if (abs(source_position[1] - target_position[1]) > 0.2) {
continue;
}
// cout<< "abs position difference:"<<abs(source_position[1] - target_position[1])<<"C-Score"<<correspondence_scores[index]<<endl;
// cout<<" Source Position " <<source_position<<endl;
// cout<<" target position "<<target_position<<endl;
tfc.add(source_position, target_position,
correspondence_scores[index]);
fidx.push_back(source_indices[index]);
fidxt.push_back(target_indices[correspondences[index]]);
}
}
cout << "TFC samples: "<<tfc.getNoOfSamples()<<" AccumulatedWeight "<<tfc.getAccumulatedWeight()<<endl;
Eigen::Affine3f tr;
tr = tfc.getTransformation();
cout << "TFC transformation: " << endl;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
cout << tr.rotation()(i, i) << "\t";
}
cout << endl;
}
transformPointCloud(*source_filtered, *transformed,
tfc.getTransformation());
cout << "transformation finished";
return transformed;
};
