void CPDNRigid<Scalar, Dim>::e_step()
{
if (!this->fgt_)
{
correspondences();
this->P1_ = this->corres_ * TVector(this->N_).setOnes();
this->PT1_ = this->corres_.transpose() * TVector(this->M_).setOnes();
this->PX_ = this->corres_ * this->data_;
}
else
{
Scalar c = pow((2*M_PI*paras_.sigma2_), 0.5*Dim) * (this->w_/(1-this->w_)) * (Scalar(this->M_)/this->N_);
TMatrix KT1 = fgt<Scalar, Dim>(this->T_, this->data_, TVector(this->M_).setOnes(), sqrt(2*paras_.sigma2_), this->fgt_eps_);
TVector a = (TVector(KT1) + c*TVector(this->N_).setOnes()).cwiseInverse();
TMatrix aX = MatrixType<Scalar, Dim>::Matrix::Zero(this->N_, Dim);
for (size_t i = 0; i < Dim; i ++)
{
aX.col(i) = this->data_.col(i).cwiseProduct(a);
}
this->PT1_ = TVector(this->N_).setOnes() - c * a;
this->P1_ = fgt<Scalar, Dim>(this->data_, this->T_, a, sqrt(2*paras_.sigma2_), this->fgt_eps_);
this->PX_ = fgt<Scalar, Dim>(this->data_, this->T_, aX, sqrt(2*paras_.sigma2_), this->fgt_eps_);
}
}
void CPDRigid<T, D>::e_step()
{
if (!_fgt)
{
correspondences();
_P1 = _corres * TVector(_N).setOnes();
_PT1 = _corres.transpose() * TVector(_M).setOnes();
_PX = _corres * _data;
}
else
{
T c = pow((2*M_PI*_paras._sigma2), 0.5*D) * (_w/(1-_w)) * (T(_M)/_N);
TMatrix KT1 = fgt<T, D>(_T, _data, TVector(_M).setOnes(), sqrt(2*_paras._sigma2), _fgt_eps);
TVector a = (TVector(KT1) + c*TVector(_N).setOnes()).cwiseInverse();
TMatrix aX = TMatrix::Zero(_N, D);
for (size_t i = 0; i < D; i ++)
{
aX.col(i) = _data.col(i).cwiseProduct(a);
}
_PT1 = TVector(_N).setOnes() - c * a;
_P1 = fgt<T, D>(_data, _T, a, sqrt(2*_paras._sigma2), _fgt_eps);
_PX = fgt<T, D>(_data, _T, aX, sqrt(2*_paras._sigma2), _fgt_eps);
}
}
TEST (PCL, TransformationEstimationLM)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr source (new pcl::PointCloud<pcl::PointXYZ>(cloud_source));
pcl::PointCloud<pcl::PointXYZ>::Ptr target (new pcl::PointCloud<pcl::PointXYZ>(cloud_target));
// re-do reciprocal correspondence estimation
boost::shared_ptr<pcl::Correspondences> correspondences (new pcl::Correspondences);
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> corr_est;
corr_est.setInputCloud (source);
corr_est.setInputTarget (target);
corr_est.determineReciprocalCorrespondences (*correspondences);
Eigen::Matrix4f transform_res_from_LM;
pcl::registration::TransformationEstimationLM<pcl::PointXYZ, pcl::PointXYZ> trans_est_lm;
trans_est_lm.estimateRigidTransformation(*source, *target,
*correspondences,
transform_res_from_LM);
// check for correct matches and number of matches
EXPECT_EQ (int (correspondences->size ()), nr_reciprocal_correspondences);
for (int i = 0; i < nr_reciprocal_correspondences; ++i)
{
EXPECT_EQ ((*correspondences)[i].index_query, correspondences_reciprocal[i][0]);
EXPECT_EQ ((*correspondences)[i].index_match, correspondences_reciprocal[i][1]);
}
// // check for correct transformation
// for (int i = 0; i < 4; ++i)
// for (int j = 0; j < 4; ++j)
// EXPECT_NEAR (transform_res_from_LM(i, j), transform_from_LM[i][j], 1e-4);
}
TEST (PCL, CorrespondenceRejectorVarTrimmed)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr source (new pcl::PointCloud<pcl::PointXYZ>(cloud_source));
pcl::PointCloud<pcl::PointXYZ>::Ptr target (new pcl::PointCloud<pcl::PointXYZ>(cloud_target));
// re-do correspondence estimation
boost::shared_ptr<pcl::Correspondences> correspondences (new pcl::Correspondences);
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> corr_est;
corr_est.setInputCloud (source);
corr_est.setInputTarget (target);
corr_est.determineCorrespondences (*correspondences);
boost::shared_ptr<pcl::Correspondences> correspondences_result_rej_var_trimmed_dist (new pcl::Correspondences);
pcl::registration::CorrespondenceRejectorVarTrimmed corr_rej_var_trimmed_dist;
corr_rej_var_trimmed_dist.setInputCloud<pcl::PointXYZ> (source);
corr_rej_var_trimmed_dist.setInputTarget<pcl::PointXYZ> (target);
corr_rej_var_trimmed_dist.setInputCorrespondences(correspondences);
corr_rej_var_trimmed_dist.getCorrespondences(*correspondences_result_rej_var_trimmed_dist);
// check for correct matches
if (int (correspondences_result_rej_var_trimmed_dist->size ()) == nr_correspondences_result_rej_dist)
for (int i = 0; i < nr_correspondences_result_rej_dist; ++i)
EXPECT_EQ ((*correspondences_result_rej_var_trimmed_dist)[i].index_match, correspondences_dist[i][1]);
}
TEST (PCL, CorrespondenceRejectorSampleConsensus)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr source (new pcl::PointCloud<pcl::PointXYZ>(cloud_source));
pcl::PointCloud<pcl::PointXYZ>::Ptr target (new pcl::PointCloud<pcl::PointXYZ>(cloud_target));
// re-do correspondence estimation
boost::shared_ptr<pcl::Correspondences> correspondences (new pcl::Correspondences);
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> corr_est;
corr_est.setInputCloud (source);
corr_est.setInputTarget (target);
corr_est.determineCorrespondences (*correspondences);
EXPECT_EQ (int (correspondences->size ()), nr_original_correspondences);
boost::shared_ptr<pcl::Correspondences> correspondences_result_rej_sac (new pcl::Correspondences);
pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZ> corr_rej_sac;
corr_rej_sac.setInputCloud (source);
corr_rej_sac.setTargetCloud (target);
corr_rej_sac.setInlierThreshold (rej_sac_max_dist);
corr_rej_sac.setMaxIterations (rej_sac_max_iter);
corr_rej_sac.setInputCorrespondences (correspondences);
corr_rej_sac.getCorrespondences (*correspondences_result_rej_sac);
Eigen::Matrix4f transform_res_from_SAC = corr_rej_sac.getBestTransformation ();
// check for correct matches and number of matches
EXPECT_EQ (int (correspondences_result_rej_sac->size ()), nr_correspondences_result_rej_sac);
if (int (correspondences_result_rej_sac->size ()) == nr_correspondences_result_rej_sac)
for (int i = 0; i < nr_correspondences_result_rej_sac; ++i)
EXPECT_EQ ((*correspondences_result_rej_sac)[i].index_match, correspondences_sac[i][1]);
// check for correct transformation
for (int i = 0; i < 4; ++i)
for (int j = 0; j < 4; ++j)
EXPECT_NEAR (transform_res_from_SAC (i, j), transform_from_SAC[i][j], 1e-4);
}
TEST (PCL, CorrespondenceRejectorTrimmed)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr source (new pcl::PointCloud<pcl::PointXYZ>(cloud_source));
pcl::PointCloud<pcl::PointXYZ>::Ptr target (new pcl::PointCloud<pcl::PointXYZ>(cloud_target));
// re-do correspondence estimation
boost::shared_ptr<pcl::Correspondences> correspondences (new pcl::Correspondences);
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> corr_est;
corr_est.setInputCloud (source);
corr_est.setInputTarget (target);
corr_est.determineCorrespondences (*correspondences);
boost::shared_ptr<pcl::Correspondences> correspondences_result_rej_trimmed (new pcl::Correspondences);
pcl::registration::CorrespondenceRejectorTrimmed corr_rej_trimmed;
corr_rej_trimmed.setOverlapRadio(rej_trimmed_overlap);
corr_rej_trimmed.setInputCorrespondences(correspondences);
corr_rej_trimmed.getCorrespondences(*correspondences_result_rej_trimmed);
// check for correct matches, number of matches, and for sorting (correspondences should be sorted w.r.t. distance)
EXPECT_EQ (int (correspondences_result_rej_trimmed->size ()), nr_correspondences_result_rej_trimmed);
if (int (correspondences_result_rej_trimmed->size ()) == nr_correspondences_result_rej_trimmed)
{
for (int i = 0; i < nr_correspondences_result_rej_trimmed; ++i)
EXPECT_EQ ((*correspondences_result_rej_trimmed)[i].index_query, correspondences_trimmed[i][0]);
for (int i = 0; i < nr_correspondences_result_rej_trimmed; ++i)
EXPECT_EQ ((*correspondences_result_rej_trimmed)[i].index_match, correspondences_trimmed[i][1]);
}
}
TEST (PCL, CorrespondenceRejectorSurfaceNormal)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr source (new pcl::PointCloud<pcl::PointXYZ>(cloud_source));
pcl::PointCloud<pcl::PointXYZ>::Ptr target (new pcl::PointCloud<pcl::PointXYZ>(cloud_target));
// re-do correspondence estimation
boost::shared_ptr<pcl::Correspondences> correspondences (new pcl::Correspondences);
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> corr_est;
corr_est.setInputCloud (source);
corr_est.setInputTarget (target);
corr_est.determineCorrespondences (*correspondences);
pcl::PointCloud<pcl::PointNormal>::Ptr source_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::copyPointCloud(*source, *source_normals);
pcl::PointCloud<pcl::PointNormal>::Ptr target_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::copyPointCloud(*target, *target_normals);
pcl::NormalEstimation<pcl::PointNormal, pcl::PointNormal> norm_est_src;
norm_est_src.setSearchMethod (pcl::search::KdTree<pcl::PointNormal>::Ptr (new pcl::search::KdTree<pcl::PointNormal>));
norm_est_src.setKSearch (10);
norm_est_src.setInputCloud (source_normals);
norm_est_src.compute (*source_normals);
pcl::NormalEstimation<pcl::PointNormal, pcl::PointNormal> norm_est_tgt;
norm_est_tgt.setSearchMethod (pcl::search::KdTree<pcl::PointNormal>::Ptr (new pcl::search::KdTree<pcl::PointNormal>));
norm_est_tgt.setKSearch (10);
norm_est_tgt.setInputCloud (target_normals);
norm_est_tgt.compute (*target_normals);
pcl::registration::CorrespondenceRejectorSurfaceNormal corr_rej_surf_norm;
corr_rej_surf_norm.initializeDataContainer <pcl::PointXYZ, pcl::PointNormal> ();
corr_rej_surf_norm.setInputCloud <pcl::PointXYZ> (source);
corr_rej_surf_norm.setInputTarget <pcl::PointXYZ> (target);
corr_rej_surf_norm.setInputNormals <pcl::PointXYZ, pcl::PointNormal> (source_normals);
corr_rej_surf_norm.setTargetNormals <pcl::PointXYZ, pcl::PointNormal> (target_normals);
boost::shared_ptr<pcl::Correspondences> correspondences_result_rej_surf_norm (new pcl::Correspondences);
corr_rej_surf_norm.setInputCorrespondences (correspondences);
corr_rej_surf_norm.setThreshold (0.5);
corr_rej_surf_norm.getCorrespondences (*correspondences_result_rej_surf_norm);
// check for correct matches
if (int (correspondences_result_rej_surf_norm->size ()) == nr_correspondences_result_rej_dist)
for (int i = 0; i < nr_correspondences_result_rej_dist; ++i)
EXPECT_EQ ((*correspondences_result_rej_surf_norm)[i].index_match, correspondences_dist[i][1]);
}
TEST (PCL, CorrespondenceEstimationReciprocal)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr source (new pcl::PointCloud<pcl::PointXYZ>(cloud_source));
pcl::PointCloud<pcl::PointXYZ>::Ptr target (new pcl::PointCloud<pcl::PointXYZ>(cloud_target));
boost::shared_ptr<pcl::Correspondences> correspondences (new pcl::Correspondences);
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> corr_est;
corr_est.setInputCloud (source);
corr_est.setInputTarget (target);
corr_est.determineReciprocalCorrespondences (*correspondences);
// check for correct matches and number of matches
EXPECT_EQ (int (correspondences->size ()), nr_reciprocal_correspondences);
if (int (correspondences->size ()) == nr_reciprocal_correspondences)
for (int i = 0; i < nr_reciprocal_correspondences; ++i)
EXPECT_EQ ((*correspondences)[i].index_match, correspondences_reciprocal[i][1]);
}
shot_detector::shot_detector()
{
data = false;
activated = false;
std::cerr << "Starting" << std::endl;
// Start the ros stuff such as the subscriber and the service
nh = ros::NodeHandle("apc_object_detection");
loadParameters();
//kinect=nh.subscribe("/kinect2_cool/depth_highres/points", 1, &shot_detector::PointCloudCallback,this);
processor = nh.advertiseService("Shot_detector", &shot_detector::processCloud, this);
// As we use Ptr to access our pointcloud we first have to initalize something to point to
pcl::PointCloud<PointType>::Ptr model_ (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr model_keypoints_ (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr scene_ (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr scene_keypoints_ (new pcl::PointCloud<PointType> ());
pcl::PointCloud<NormalType>::Ptr model_normals_ (new pcl::PointCloud<NormalType> ());
pcl::PointCloud<NormalType>::Ptr scene_normals_ (new pcl::PointCloud<NormalType> ());
pcl::PointCloud<DescriptorType>::Ptr model_descriptors_ (new pcl::PointCloud<DescriptorType> ());
pcl::PointCloud<DescriptorType>::Ptr scene_descriptors_ (new pcl::PointCloud<DescriptorType> ());
pcl::CorrespondencesPtr model_scene_corrs_ (new pcl::Correspondences ());
pcl::PointCloud<PointType>::Ptr model_good_kp_ (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr scene_good_kp_ (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr objects_ (new pcl::PointCloud<PointType> ());
//pcl::PointCloud<DescriptorType>::Ptr model_descriptors_ (new pcl::PointCloud<DescriptorType> ());
//pcl::PointCloud<DescriptorType>::Ptr scene_descriptors_ (new pcl::PointCloud<DescriptorType> ());
model = model_;
model_keypoints = model_keypoints_;
scene = scene_;
scene_keypoints = scene_keypoints_;
model_normals = model_normals_;
scene_normals = scene_normals_;
model_descriptors = model_descriptors_;
scene_descriptors = scene_descriptors_;
model_scene_corrs = model_scene_corrs_;
model_good_kp = model_good_kp_;
scene_good_kp = scene_good_kp_;
objects = objects_;
pcl::CorrespondencesPtr correspondences (new pcl::Correspondences);
correspondences_ = correspondences;
//Set up a couple of the pcl settings
descr_est.setRadiusSearch (descr_rad_);
norm_est.setKSearch (10);
//calcPFHRGBDescriptors(model,model_keypoints,model_normals,model_descriptors);
}
void CPDNRigid<Scalar, Dim>::compute()
{
size_t iter_num = 0;
Scalar e_tol = 10 + this->e_tol_;
Scalar e = 0;
this->normalize();
initialization();
/*if (this->_vision)
{
RenderThread<Scalar, Dim>::instance()->updateModel(this->model_);
RenderThread<Scalar, Dim>::instance()->updateData(this->data_);
RenderThread<Scalar, Dim>::instance()->startThread();
}*/
while (iter_num < this->iter_num_ && e_tol > this->e_tol_ && paras_.sigma2_ > 10 * this->v_tol_)
{
e_step();
Scalar old_e = e;
e = energy();
e += paras_.lambda_/2 * (paras_.W_.transpose()*G_*paras_.W_).trace();
e_tol = fabs((e - old_e) / e);
m_step();
/*if (this->_vision == true)
RenderThread<Scalar, Dim>::instance()->updateModel(this->T_);*/
iter_num ++;
}
correspondences();
this->updateModel();
this->denormalize();
this->rewriteOriginalSource();
/*RenderThread<Scalar, Dim>::instance()->cancel();*/
}
void CPDNRigid<T, D>::run()
{
size_t iter_num = 0;
T e_tol = 10 + _e_tol;
T e = 0;
normalize();
initialization();
if (_vision)
{
RenderThread<T, D>::instance()->updateModel(_model);
RenderThread<T, D>::instance()->updateData(_data);
RenderThread<T, D>::instance()->startThread();
}
while (iter_num < _iter_num && e_tol > _e_tol && _paras._sigma2 > 10 * _v_tol)
{
e_step();
T old_e = e;
e = energy();
e += _paras._lambda/2 * (_paras._W.transpose()*_G*_paras._W).trace();
e_tol = abs((e - old_e) / e);
m_step();
if (_vision == true)
RenderThread<T, D>::instance()->updateModel(_T);
iter_num ++;
}
correspondences();
updateModel();
denormalize();
RenderThread<T, D>::instance()->cancel();
}
TEST (PCL, TransformationEstimationSVD)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr source (new pcl::PointCloud<pcl::PointXYZ>(cloud_source));
pcl::PointCloud<pcl::PointXYZ>::Ptr target (new pcl::PointCloud<pcl::PointXYZ>(cloud_target));
// re-do reciprocal correspondence estimation
boost::shared_ptr<pcl::Correspondences> correspondences (new pcl::Correspondences);
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> corr_est;
corr_est.setInputCloud (source);
corr_est.setInputTarget (target);
corr_est.determineReciprocalCorrespondences (*correspondences);
Eigen::Matrix4f transform_res_from_SVD;
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> trans_est_svd;
trans_est_svd.estimateRigidTransformation(*source, *target,
*correspondences,
transform_res_from_SVD);
// check for correct transformation
for (int i = 0; i < 4; ++i)
for (int j = 0; j < 4; ++j)
EXPECT_NEAR (transform_res_from_SVD(i, j), transform_from_SVD[i][j], 1e-4);
}
TEST (PCL, CorrespondenceRejectorOneToOne)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr source (new pcl::PointCloud<pcl::PointXYZ>(cloud_source));
pcl::PointCloud<pcl::PointXYZ>::Ptr target (new pcl::PointCloud<pcl::PointXYZ>(cloud_target));
// re-do correspondence estimation
boost::shared_ptr<pcl::Correspondences> correspondences (new pcl::Correspondences);
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> corr_est;
corr_est.setInputCloud (source);
corr_est.setInputTarget (target);
corr_est.determineCorrespondences (*correspondences);
boost::shared_ptr<pcl::Correspondences> correspondences_result_rej_one_to_one (new pcl::Correspondences);
pcl::registration::CorrespondenceRejectorOneToOne corr_rej_one_to_one;
corr_rej_one_to_one.setInputCorrespondences(correspondences);
corr_rej_one_to_one.getCorrespondences(*correspondences_result_rej_one_to_one);
// check for correct matches and number of matches
EXPECT_EQ (int (correspondences_result_rej_one_to_one->size ()), nr_correspondences_result_rej_one_to_one);
if (int (correspondences_result_rej_one_to_one->size ()) == nr_correspondences_result_rej_one_to_one)
for (int i = 0; i < nr_correspondences_result_rej_one_to_one; ++i)
EXPECT_EQ ((*correspondences_result_rej_one_to_one)[i].index_match, correspondences_one_to_one[i][1]);
}
pcl::CorrespondencesPtr CorrespondenceEstimation::estimateCorrespondences(pcl::PointCloud<PointXYZSIFT>::Ptr src_cloud_, pcl::PointCloud<PointXYZSIFT>::Ptr trg_cloud_) {
CLOG(LTRACE) << "estimateCorrespondences(src,trg)";
// Empty list of correspondences.
pcl::CorrespondencesPtr correspondences(new pcl::Correspondences());
// If pass_through - return empty list.
if (pass_through) {
CLOG(LDEBUG) << "Passthrough - returning no correspondences";
return correspondences;
}//: if passthrough
// Create object responsible for correspondence estimation.
pcl::registration::CorrespondenceEstimation<PointXYZSIFT, PointXYZSIFT>::Ptr correst(new pcl::registration::CorrespondenceEstimation<PointXYZSIFT, PointXYZSIFT>());
// Set feature representation.
SIFTFeatureRepresentation::Ptr point_representation(new SIFTFeatureRepresentation());
correst->setPointRepresentation(point_representation);
// Filter NaN points.
pcl::PointCloud<PointXYZSIFT>::Ptr filtered_src_cloud(new pcl::PointCloud<PointXYZSIFT>);
std::vector<int> src_indices;
pcl::removeNaNFromPointCloud(*src_cloud_, *filtered_src_cloud, src_indices);
//filtered_src_cloud->is_dense = false;
pcl::PointCloud<PointXYZSIFT>::Ptr filtered_trg_cloud(new pcl::PointCloud<PointXYZSIFT>);
std::vector<int> trg_indices;
pcl::removeNaNFromPointCloud(*trg_cloud_, *filtered_trg_cloud, trg_indices);
//filtered_trg_cloud->is_dense = false;
// Set input clouds.
correst->setInputSource(filtered_src_cloud);
correst->setInputTarget(filtered_trg_cloud);
// Find correspondences.
correst->determineReciprocalCorrespondences(*correspondences);
CLOG(LINFO) << "Found correspondences: " << correspondences->size();
out_corest.write(correst);
// Correspondence rejection.
if (prop_reject_correspondences) {
CLOG(LDEBUG) << "Correspondence rejection";
if (prop_use_RanSAC) {
CLOG(LDEBUG) << "Using RanSAC-based correspondence rejection";
// Use RANSAC to filter correspondences.
pcl::CorrespondencesPtr inliers(new pcl::Correspondences()) ;
pcl::registration::CorrespondenceRejectorSampleConsensus<PointXYZSIFT> crsac ;
crsac.setInputSource(filtered_src_cloud);
crsac.setInputTarget(filtered_trg_cloud);
crsac.setInlierThreshold(prop_RanSAC_inliers_threshold);
crsac.setMaximumIterations(prop_RanSAC_max_iterations);
crsac.setInputCorrespondences(correspondences);
// Reject correspondences.
crsac.getCorrespondences(*inliers);
CLOG(LINFO) << "Correspondences remaining after RANSAC-based rejection: " << inliers->size();
// Get computed transformation.
Types::HomogMatrix hm = crsac.getBestTransformation() ;
CLOG(LINFO) << "Found transformation:\n" << hm;
// Return correspondences - inliers.
return inliers;
} else {
// Use correspondence rejection distance.
CLOG(LDEBUG) << "Using correspondence rejection based on Euclidean distance";
// Use RANSAC to filter correspondences.
pcl::CorrespondencesPtr inliers(new pcl::Correspondences()) ;
pcl::registration:: CorrespondenceRejectorDistance cr ;
cr.setInputSource<PointXYZSIFT>(filtered_src_cloud);
cr.setInputTarget<PointXYZSIFT>(filtered_trg_cloud);
cr.setMaximumDistance(prop_max_distance);
cr.setInputCorrespondences(correspondences);
// Reject correspondences.
cr.getCorrespondences(*inliers);
CLOG(LINFO) << "Correspondences remaining after Euclidean based rejection: " << inliers->size();
// Return correspondences - inliers.
return inliers;
}//: else euclidean
} else {
CLOG(LDEBUG) << "Returning all correspondences";
// Return found correspondences.
return correspondences;
}//: if
}
double ICP::registration(intensityCloud::Ptr P, std::vector<BlockInfo>* Y, std::vector<double> timeStamps, std::vector<Vector3d>* transforms, Vector3d *T, Matrix4f *R, Vector3d direction){
double error = 0;
pcl::PointXYZI centroidP = 0, centroidY = 0;
pcl::registration::TransformationEstimationSVD<pcl::PointXYZI, pcl::PointXYZI> svd;
pcl::registration::CorrespondenceEstimation<pcl::PointXYZI, pcl::PointXYZI> correspondenceEstimator;
int i = 0;
intensityCloud::iterator cloudIterator = P->begin();
unsigned int validPoints = 0;
intensityCloud candidatePointCloud, pointCloud;
pcl::PointXYZI p;
int ySize = Y->size();
//Centroid calculation
for (;cloudIterator != P->end();cloudIterator++){
if (isnan(cloudIterator->x) || isnan(cloudIterator->y) || isnan(cloudIterator->z)) continue;
validPoints++;
centroidP.x += cloudIterator->x;
centroidP.y += cloudIterator->y;
centroidP.z += cloudIterator->z;
}
centroidP.x /= validPoints;
centroidP.y /= validPoints;
centroidP.z /= validPoints;
for(i=0;i<ySize;i++){
centroidY.x += Y->at(i).blockPtr->mean_.x;
centroidY.y += Y->at(i).blockPtr->mean_.y;
centroidY.z += Y->at(i).blockPtr->mean_.z;
}
centroidY.x /= Y->size();
centroidY.y /= Y->size();
centroidY.z /= Y->size();
//ROS_INFO("Candidates centroid %f %f %f",centroidY.x,centroidY.y,centroidY.z);
//ROS_INFO("Points centroid %f %f %f",centroidP.x,centroidP.y,centroidP.z);
//Move both to same origin
for(i=0;i<ySize;i++){
p.x = Y->at(i).blockPtr->mean_.x - centroidY.x;
p.y = Y->at(i).blockPtr->mean_.y - centroidY.y;
p.z = Y->at(i).blockPtr->mean_.z - centroidY.z;
candidatePointCloud.push_back(p);
}
for (cloudIterator = P->begin();cloudIterator != P->end();cloudIterator++){
if (isnan(cloudIterator->x) || isnan(cloudIterator->y) || isnan(cloudIterator->z)) continue;
p.x = cloudIterator->x - centroidP.x;
p.y = cloudIterator->y - centroidP.y;
p.z = cloudIterator->z - centroidP.z;
pointCloud.push_back(p);
}
if ((direction[0] == 0)&&(direction[1] == 0)&&(direction[2] == 0)){
// Calculate SVD
//pcl::ConstCloudIterator<pcl::PointXYZI> p1(pointCloud);
//pcl::ConstCloudIterator<pcl::PointXYZI> p2(candidatePointCloud);
pcl::registration::TransformationEstimationSVD<pcl::PointXYZI, pcl::PointXYZI>::Matrix4 rotation;
rotation = Matrix4f::Identity();
pcl::PointCloud<pcl::PointXYZI>::Ptr source (new pcl::PointCloud<pcl::PointXYZI>(pointCloud));
pcl::PointCloud<pcl::PointXYZI>::Ptr target (new pcl::PointCloud<pcl::PointXYZI>(candidatePointCloud));
boost::shared_ptr<pcl::Correspondences> correspondences (new pcl::Correspondences);
correspondenceEstimator.setInputSource(source);
correspondenceEstimator.setInputTarget(target);
correspondenceEstimator.determineCorrespondences(*correspondences);
svd.estimateRigidTransformation(pointCloud,candidatePointCloud, *correspondences,rotation);
ROS_INFO("\n \tSVD Rotation \n%f %f %f\n%f %f %f\n%f %f %f",rotation(0,0),rotation(0,1),rotation(0,2),
rotation(1,0),rotation(1,1),rotation(1,2),
rotation(2,0),rotation(2,1),rotation(2,2));
//lm.estimateRigidTransformation(pointCloud,candidatePointCloud, *correspondences,rotation);
//ROS_INFO("\n \tLM Rotation \n%f %f %f\n%f %f %f\n%f %f %f",rotation(0,0),rotation(0,1),rotation(0,2),
// rotation(1,0),rotation(1,1),rotation(1,2),
// rotation(2,0),rotation(2,1),rotation(2,2));
/*pcl::PointCloud<pcl::PointXYZI>::Ptr source (new pcl::PointCloud<pcl::PointXYZI>(pointCloud));
pcl::PointCloud<pcl::PointXYZI>::Ptr target (new pcl::PointCloud<pcl::PointXYZI>(candidatePointCloud));
pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
icp.setInputSource(source);
icp.setInputTarget(target);
// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
icp.setMaxCorrespondenceDistance (10);
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (50);
// Set the transformation epsilon (criterion 2)
icp.setTransformationEpsilon (1e-8);
// Set the euclidean distance difference epsilon (criterion 3)
icp.setEuclideanFitnessEpsilon (1);
pcl::PointCloud<pcl::PointXYZI> Final;
icp.align(Final);
//std::cout << "has converged:" << icp.hasConverged() << " score: " <<
//icp.getFitnessScore() << std::endl;
//std::cout << icp.getFinalTransformation() << std::endl;
Eigen::Matrix4f rotation = icp.getFinalTransformation ();*/
//ROS_INFO("TransformPointCloud");
pcl::transformPointCloud(pointCloud,pointCloud,rotation);
(*R) = rotation;
}
else {
double angle, closestAngle = M_PI, longest;
(*T)[0] = 0;
(*T)[1] = 0;
(*T)[2] = 0;
int bestI = 0;
std::vector<double> errorsX, errorsY, errorsZ;
double slopeX, slopeY, slopeZ;
std::vector<double> positiveStamps;
i = 0;
bool moving = false;
double tX = 0,tY = 0,tZ = 0;
int samples = 0;
int accountedSamples = 0;
Vector3d linearTranslation,totalLinearTranslation;
linearTranslation[0] = 0;
linearTranslation[1] = 0;
linearTranslation[2] = 0;
totalLinearTranslation[0] = 0;
totalLinearTranslation[1] = 0;
totalLinearTranslation[2] = 0;
int linearSegments = 0;
int dynamicSamples = 0;
int slopeBegin = 0;
int lineBegin = 0;
while (i < ySize){
if (timeStamps.at(i) < 0){
if ((moving == true) && (errorsX.size() > 0)){
// Calculate movement with dinamic data
slopeX = slope(positiveStamps,errorsX);
slopeY = slope(positiveStamps,errorsY);
slopeZ = slope(positiveStamps,errorsZ);
for (int j = slopeBegin;j<i-1;j++){
transforms->at(j)[0] = fabs(positiveStamps.at(j-slopeBegin)) * slopeX;
transforms->at(j)[1] = fabs(positiveStamps.at(j-slopeBegin)) * slopeY;
transforms->at(j)[2] = fabs(positiveStamps.at(j-slopeBegin)) * slopeZ;
}
lineBegin = i;
ROS_INFO("SlopeX %f SlopeY %f Slopez %f", slopeX, slopeY, slopeZ);
ROS_INFO("Total SlopeX %f SlopeY %f Slopez %f", slopeX * positiveStamps.back(), slopeY * positiveStamps.back(), slopeZ * positiveStamps.back());
(*T)[0] += slopeX * positiveStamps.back();
(*T)[1] += slopeY * positiveStamps.back();
(*T)[2] += slopeZ * positiveStamps.back();
positiveStamps.clear();
moving = false;
errorsX.clear();
errorsY.clear();
errorsZ.clear();
tX = 0;
tY = 0;
tZ = 0;
samples = 0;
linearTranslation[0] = 0;
linearTranslation[1] = 0;
linearTranslation[2] = 0;
ROS_INFO("==============STATIC=============");
}
moving = false;
tX = (fabs(Y->at(i).blockPtr->mean_.x - P->at(i).x) < errorThreshold_) ? 0 : Y->at(i).blockPtr->mean_.x - P->at(i).x;
tY = (fabs(Y->at(i).blockPtr->mean_.y - P->at(i).y) < errorThreshold_) ? 0 : Y->at(i).blockPtr->mean_.y - P->at(i).y;
tZ = (fabs(Y->at(i).blockPtr->mean_.z - P->at(i).z) < errorThreshold_) ? 0 : Y->at(i).blockPtr->mean_.z - P->at(i).z;
//ROS_INFO("");
//ROS_INFO("SPoint %d X %f Y %f Z %f",i , P->at(i).x, P->at(i).y,P->at(i).z);
//ROS_INFO("SCandt %d X %f Y %f Z %f",i, Y->at(i).blockPtr->mean_.x, Y->at(i).blockPtr->mean_.y, Y->at(i).blockPtr->mean_.z);
//ROS_INFO("SError %d X %f Y %f Z %f Time %f",i , tX, tY, tZ,fabs(fabs(timeStamps.at(i))-fabs(timeStamps.at(slopeBegin))));
linearTranslation[0] = (((double)samples * linearTranslation[0]) / (samples+1.0)) + (tX/(samples+1.0));
linearTranslation[1] = (((double)samples * linearTranslation[1]) / (samples+1.0)) + (tY/(samples+1.0));
linearTranslation[2] = (((double)samples * linearTranslation[2]) / (samples+1.0)) + (tZ/(samples+1.0));
samples++;
i++;
}
else {
if ((moving == false) && (samples > 0)){
// Calculate movement with static data
slopeBegin = i;
for (int j = lineBegin;j<i-1;j++){
transforms->at(j)[0] = linearTranslation[0];
transforms->at(j)[1] = linearTranslation[1];
transforms->at(j)[2] = linearTranslation[2];
}
ROS_INFO("Linear X %f Y %f Z %f", tX/samples, tY/samples, tZ/samples);
totalLinearTranslation[0] = (accountedSamples * totalLinearTranslation[0] + linearTranslation[0] * samples) / (accountedSamples + samples);
totalLinearTranslation[1] = (accountedSamples * totalLinearTranslation[1] + linearTranslation[1] * samples) / (accountedSamples + samples);
totalLinearTranslation[2] = (accountedSamples * totalLinearTranslation[2] + linearTranslation[2] * samples) / (accountedSamples + samples);
ROS_INFO("Total Linear X %f Y %f Z %f", totalLinearTranslation[0], totalLinearTranslation[1], totalLinearTranslation[2]);
linearSegments++;
linearTranslation[0] = 0;
linearTranslation[1] = 0;
linearTranslation[2] = 0;
//(*T)[0] = (accountedSamples*(*T)[0])/(accountedSamples + samples) + tX/(accountedSamples + samples);
//(*T)[1] = (accountedSamples*(*T)[1])/(accountedSamples + samples) + tY/(accountedSamples + samples);
//(*T)[2] = (accountedSamples*(*T)[2])/(accountedSamples + samples) + tZ/(accountedSamples + samples);
accountedSamples += samples;
samples = 0;
tX = 0;
tY = 0;
tZ = 0;
ROS_INFO("==============MOVING=============");
}
tX = (fabs(Y->at(i).blockPtr->mean_.x - P->at(i).x) < errorThreshold_) ? 0 : Y->at(i).blockPtr->mean_.x - P->at(i).x;
tY = (fabs(Y->at(i).blockPtr->mean_.y - P->at(i).y) < errorThreshold_) ? 0 : Y->at(i).blockPtr->mean_.y - P->at(i).y;
tZ = (fabs(Y->at(i).blockPtr->mean_.z - P->at(i).z) < errorThreshold_) ? 0 : Y->at(i).blockPtr->mean_.z - P->at(i).z;
dynamicSamples++;
//ROS_INFO("");
//ROS_INFO("DPoint %d X %f Y %f Z %f",i , P->at(i).x, P->at(i).y,P->at(i).z);
//ROS_INFO("DCandt %d X %f Y %f Z %f",i, Y->at(i).blockPtr->mean_.x, Y->at(i).blockPtr->mean_.y, Y->at(i).blockPtr->mean_.z);
//ROS_INFO("DError %d X %f Y %f Z %f Time %f",i , tX, tY, tZ,fabs(fabs(timeStamps.at(i))-fabs(timeStamps.at(slopeBegin))));
errorsX.push_back(tX);
errorsY.push_back(tY);
errorsZ.push_back(tZ);
moving = true;
positiveStamps.push_back(fabs(fabs(timeStamps.at(i))-fabs(timeStamps.at(slopeBegin))));
i++;
}
}
if ((moving == false) && (samples > 0)){
// Calculate movement with static data
slopeBegin = i;
for (int j = lineBegin;j<i-1;j++){
transforms->at(j)[0] = linearTranslation[0];
transforms->at(j)[1] = linearTranslation[1];
transforms->at(j)[2] = linearTranslation[2];
}
ROS_INFO("Last Linear X %f Y %f Z %f", linearTranslation[0], linearTranslation[1], linearTranslation[2]);
totalLinearTranslation[0] = (accountedSamples * totalLinearTranslation[0] + linearTranslation[0] * samples) / (accountedSamples + samples);
totalLinearTranslation[1] = (accountedSamples * totalLinearTranslation[1] + linearTranslation[1] * samples) / (accountedSamples + samples);
totalLinearTranslation[2] = (accountedSamples * totalLinearTranslation[2] + linearTranslation[2] * samples) / (accountedSamples + samples);
ROS_INFO("Last Total Linear X %f Y %f Z %f", totalLinearTranslation[0], totalLinearTranslation[1], totalLinearTranslation[2]);
samples = 0;
tX = 0;
tY = 0;
tZ = 0;
}
if ((moving == true) && (errorsX.size() > 0)){
// Calculate movement with dinamic data
slopeX = slope(positiveStamps,errorsX);
slopeY = slope(positiveStamps,errorsY);
slopeZ = slope(positiveStamps,errorsZ);
ROS_INFO("Last SlopeX %f SlopeY %f Slopez %f", slopeX, slopeY, slopeZ);
for (int j = slopeBegin;j<i;j++){
transforms->at(j)[0] = fabs(positiveStamps.at(j-slopeBegin)) * slopeX;
transforms->at(j)[1] = fabs(positiveStamps.at(j-slopeBegin)) * slopeY;
transforms->at(j)[2] = fabs(positiveStamps.at(j-slopeBegin)) * slopeZ;
}
ROS_INFO("Last total SlopeX %f SlopeY %f Slopez %f", slopeX * positiveStamps.back(), slopeY* positiveStamps.back(), slopeZ* positiveStamps.back());
(*T)[0] += slopeX * positiveStamps.back();
(*T)[1] += slopeY * positiveStamps.back();
(*T)[2] += slopeZ * positiveStamps.back();
positiveStamps.clear();
errorsX.clear();
errorsY.clear();
errorsZ.clear();
}
(*T)[0] += totalLinearTranslation[0];
(*T)[1] += totalLinearTranslation[1];
(*T)[2] += totalLinearTranslation[2];
}
ROS_INFO("Translation x = %f y = %f z = %f",(*T)[0], (*T)[1], (*T)[2]);
return error;
}
int main (int argc, char** argv)
{
double f;
double vd;
double r;
int iter;
double h;
// argiment parsing
if (pcl::console::parse_argument (argc, argv, "-r", r) >= 0)
{
MIN_DISTANCE = r;
cout << "adjust the registration threshold to " << r << endl;
}
if (pcl::console::parse_argument (argc, argv, "-giter", iter) >= 0)
{
GITER = iter;
cout << "adjust global alignment iterations to " << iter <<" iterations"<< endl;
}
if (pcl::console::parse_argument (argc, argv, "-hessian", h) >= 0)
{
HESSIAN=h;
cout << "adjust SIFT matching threshold to " << h << endl;
}
if (pcl::console::find_argument (argc, argv, "-h") >= 0 || argc == 1)
{
printUsage (argv[0]);
return 0;
}
if (pcl::console::find_argument (argc, argv, "-g") >= 0)
{
GLOBAL_FLAG = 1;
cout << "Global Reistration is ON" << endl;
}
if (pcl::console::find_argument (argc, argv, "-lcoff") >= 0)
{
LOOP_CLOSURE = 0;
cout << "Loop CLosure Detection is OFF" << endl;
}
if (pcl::console::find_argument (argc, argv, "-m") >= 0)
{
MLS = 1;
cout << "Moving least square smoothing is ON" << endl;
}
if (pcl::console::find_argument (argc, argv, "-dn") >= 0)
{
DENOISING= 1;
cout << "De-noising (statistic) is ON" << endl;
}
if (pcl::console::parse_argument (argc, argv, "-vd", vd) >= 0)
{
DEPTH_AVG = 1;
cout << "Down-sampling is ON" << endl;
if (vd>0.00001)
{
DOWN_SAMPLE = vd;
cout<<"Down sample threshold is changed to "<<DOWN_SAMPLE<<endl;
}
}
vector<PointCloud, Eigen::aligned_allocator<PointCloud> > data;
vector<PointCloud, Eigen::aligned_allocator<PointCloud> > key3data;
vector<vector<KeyPoint> > key2data;
vector<pcl::CorrespondencesPtr> correspondencesdata;
Mat previousrgb;
vector<KeyPoint> previouskey2d;
// read the rgb-d data one by one
ifstream index;
string root_dir = argv[1];
index.open (string(root_dir + "/input.txt").c_str());
int i = 0;
while (!index.eof ())
{
string file;
index >> file;
if (file.empty ())
break;
file = root_dir + "/" + file;
Mat rgbimg;
Mat depimg;
Mat mask;
// read data
readImg (file, "_rgb.png", rgbimg);
readImg (file, "_depth.png", depimg);
readImg (file, "_mask.png", mask);
if (rgbimg.empty () || depimg.empty () || mask.empty ())
{
printf (
"Error: The RGB-D file does not exit. Please note that the right format should be: \n");
printf (
"X_rgb.png, X_depth.png, X_mask.png with the same size. ('X' is your input) \n");
return (0);
}
if (rgbimg.size () != depimg.size () || rgbimg.size () != mask.size ())
{
printf (
"Error: The RGB-D files are not consistent. The rgb, depth and mask images should have the same size. \n");
return (0);
}
cout << "loading: " << file << endl;
// check the image size and the offest coordinate
int x1, y1;
if (rgbimg.rows == 480 && rgbimg.cols == 640)
{
// if the image is full size (480*640), no offset coordinate
x1 = 0;
y1 = 0;
}
else
{
// if not, read the offset file
ifstream loc;
string location1 = file + "_loc.txt";
loc.open (location1.c_str ());
if (!loc.is_open ())
{
if (TOP_LEFT1!=0 && TOP_LEFT2!=0)
{
x1=TOP_LEFT2;
y1=TOP_LEFT1;
}
else
{
cout << "Error! There is no associated off-set location file." << endl;
return 0;
}
}
else
{
char comma;
loc >> x1 >> comma >> y1;
}
loc.close();
}
// perform erosion to remove the noises around the boundary
erosion (mask, 4);
vector<KeyPoint> key2d;
PointCloud pointcloud;
PointCloud keypoints;
// read data and perform surf feature matching
read (rgbimg, depimg, mask, pointcloud, keypoints, key2d, x1, y1, HESSIAN);
filter (pointcloud, 0.001, pointcloud);
data.push_back (pointcloud);
key3data.push_back (keypoints);
key2data.push_back (key2d);
// find the pairwise correspondecnes based on surf feature
if (i >= 1)
{
pcl::CorrespondencesPtr correspondences (new pcl::Correspondences ()); //may have bugs??
vector<DMatch> good_matches;
match (rgbimg, previousrgb, key2d, previouskey2d, good_matches,
correspondences);
correspondencesdata.push_back (correspondences);
correspondences.reset (new pcl::Correspondences);
}
previousrgb = rgbimg;
previouskey2d = key2d;
i++;
}
index.close();
printf ("Loaded %d datasets.\n", (int) data.size ());
PointCloudPtr final (new PointCloud);
vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > matrix_buffer;
// registration: initial+fine+global
//initialize the PCL viewer
p = new pcl::visualization::PCLVisualizer (argc, argv, "3D Moeling example");
p->setSize (480, 640);
p->setPosition (480, 200);
// registration
int first = 0;
int last = 0;
if (!LOOP_CLOSURE)
{
last=(int) data.size ()-1;
}
// need to change back
double threshold = MIN_DISTANCE;
for (int z = 0; z < GITER; z++)
{
vector<PointCloud, Eigen::aligned_allocator<PointCloud> > out;
PointCloudPtr output (new PointCloud);
PointCloudPtr final_temp (new PointCloud);
if (GLOBAL_FLAG)
{
vector<PointCloud, Eigen::aligned_allocator<PointCloud> > out_global;
if (z == 0)
pairwiseAlign (data, correspondencesdata, key3data, *output, out,
matrix_buffer, 1, threshold);
else
{
pairwiseAlign (data, correspondencesdata, key3data, *output, out,
matrix_buffer, 0, threshold);
}
// global optimization
cout << endl << "Global optimization begins ... at iteration " <<z+1<< endl;
globalAlign (out, final_temp, matrix_buffer, out_global, first, last);
data = out_global;
}
else
{
pairwiseAlign (data, correspondencesdata, key3data, *output, out,
matrix_buffer, 1, threshold);
final = output;
break;
}
threshold = threshold * 0.9;
final = final_temp;