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;