Eigen::Matrix4f DenseColorICP(const pcl::PointCloud<PointT>::Ptr source, const pcl::PointCloud<PointT>::Ptr target, Eigen::Matrix4f &initial_guess) { //if the ICP fail to converge, return false. Otherwise, return true! size_t Iter_num = 50; float icp_inlier_threshold = 0.01; pcl::PointCloud<PointT>::Ptr init_source(new pcl::PointCloud<PointT>); pcl::transformPointCloud(*source, *init_source, initial_guess); pcl::PointCloud<pcl::PointXYZHSV>::Ptr source_hue(new pcl::PointCloud<pcl::PointXYZHSV>); pcl::PointCloud<pcl::PointXYZHSV>::Ptr target_hue(new pcl::PointCloud<pcl::PointXYZHSV>); ExtractHue(init_source, source_hue); ExtractHue(target, target_hue); Eigen::Matrix4f final_tran = initial_guess; pcl::search::KdTree<pcl::PointXYZHSV> target_tree; target_tree.setInputCloud(target_hue); pcl::registration::TransformationEstimationSVD<pcl::PointXYZHSV, pcl::PointXYZHSV> SVD; double last_error = 100000; for( size_t iter = 0 ; iter < Iter_num ; iter++ ) { //In each round of ICP, transform the model_hue cloud float diffh, diffs, diffv, diffsum; pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ()); for( size_t i = 0 ; i < source_hue->size() ; i++ ) { std::vector<int> pointIdx; std::vector<float> pointDistance; if ( pcl_isfinite(source_hue->at(i).z) && target_tree.radiusSearch (source_hue->at(i), 0.01, pointIdx, pointDistance) > 0 ) { float diffmin = 1000; int diffmin_idx = -1; for (size_t j = 0; j < pointIdx.size (); j++) { //std::cerr<<"Finding..."<<scene_hue->points[pointIdx[j]].h <<" "<<model_hue->points[i].h<<std::endl; if( pointDistance.at(j) < icp_inlier_threshold ) { diffh = std::min( fabs(target_hue->points[pointIdx[j]].h - source_hue->points[i].h), std::min(fabs(target_hue->points[pointIdx[j]].h - 1 - source_hue->points[i].h), fabs(target_hue->points[pointIdx[j]].h + 1 - source_hue->points[i].h))); diffs = fabs(target_hue->points[pointIdx[j]].s - source_hue->points[i].s); diffv = fabs(target_hue->points[pointIdx[j]].v - source_hue->points[i].v); diffsum = 2*diffh + 2*diffs + diffv; if( diffmin > diffsum ) { diffmin = diffsum; diffmin_idx = j; } } } //std::cerr<<diffcurvature<<" "; //std::cerr<<diffmin<<" "; if( diffmin <= 0.1 ) { pcl::Correspondence temp; temp.index_query = i; temp.index_match = pointIdx[diffmin_idx]; temp.distance = pointDistance.at(diffmin_idx); model_scene_corrs->push_back(temp); } } } Eigen::Matrix4f svdRt; SVD.estimateRigidTransformation(*source_hue, *target_hue, *model_scene_corrs, svdRt); pcl::transformPointCloud(*source_hue, *source_hue, svdRt); final_tran = svdRt * final_tran ; std::cerr<<"Ratio "<<(model_scene_corrs->size()+0.0) / source_hue->size()<<std::endl; if( (model_scene_corrs->size()+0.0) / source_hue->size() >= 0.2 ) //sufficient inlier found { size_t corrs = model_scene_corrs->size(); double this_error=0; for( size_t j = 0; j < corrs; j++ ) { pcl::PointXYZHSV source_pt = source_hue->points[model_scene_corrs->at(j).index_query]; pcl::PointXYZHSV target_pt = target_hue->points[model_scene_corrs->at(j).index_match]; double diffx = source_pt.x - target_pt.x, diffy = source_pt.y - target_pt.y, diffz = source_pt.z - target_pt.z; double dist = sqrt( diffx*diffx + diffy*diffy + diffz*diffz ); this_error += dist; } this_error = this_error / corrs; if( fabs(this_error - last_error) < 1e-6 ) //Convergence reach { std::cerr<<"Convergence Reached. Error: "<<this_error<<std::endl; std::cerr<<"Iter Num: "<<iter<<std::endl; return final_tran; } else last_error = this_error; } } return final_tran; }
static SCM open_card(SCM device) { // soundcard acquisition and configuration char *dname; snd_pcm_t *handle; ALSA_CARD *card; snd_pcm_sw_params_t *sparams; SOURCE_HANDLE *src; snd_pcm_format_t f, format; SCM smob; int i, ret, dir; unsigned int rate, buffer_time; dname = scm_to_locale_string(device); for (i = 0; i < 10; i++) { ret = snd_pcm_open(&handle, dname, SND_PCM_STREAM_CAPTURE, SND_PCM_NONBLOCK); if (ret >= 0) break; } if (ret < 0) { log_msg("ALSA: can't open %s (%s)\n", dname, snd_strerror(ret)); free(dname); return SCM_BOOL_F; } card = (ALSA_CARD *)my_malloc(sizeof(ALSA_CARD), "alsa card"); card->device = dname; card->name = card_name(card->device); card->ringbuf = NULL; init_source(&card->base); card->handle = handle; card->running = 0; snd_pcm_hw_params_malloc(&card->hparams); snd_pcm_hw_params_any(card->handle, card->hparams); for (f = format = 0; f < SND_PCM_FORMAT_LAST; f++) { ret = snd_pcm_hw_params_test_format(card->handle, card->hparams, f); if (ret == 0) { log_msg("ALSA: - %s\n", snd_pcm_format_name(f)); format = f; } } ret = snd_pcm_hw_params_set_access(card->handle, card->hparams, SND_PCM_ACCESS_RW_INTERLEAVED); if (ret < 0) log_msg("ALSA: access %s\n", snd_strerror(ret)); ret = snd_pcm_hw_params_set_format(card->handle, card->hparams, format); log_msg("ALSA: format: %s\n", snd_pcm_format_description(format)); if (ret < 0) log_msg("ALSA: format error %s\n", snd_strerror(ret)); snd_pcm_hw_params_get_buffer_time_max(card->hparams, &buffer_time, 0); rate = sampling_rate; ret = snd_pcm_hw_params_set_rate(card->handle, card->hparams, rate, 0); log_msg("ALSA: rate %d\n", rate); if (ret < 0) log_msg("ALSA: rate error %s\n", snd_strerror(ret)); snd_pcm_hw_params_set_channels(card->handle, card->hparams, QMX_CHANNELS); card->blocksize = BLOCKSIZE; snd_pcm_hw_params_set_period_size(card->handle, card->hparams, card->blocksize, 0); log_msg("ALSA: period %ld\n", card->blocksize); snd_pcm_hw_params_set_buffer_time_near(card->handle, card->hparams, &buffer_time, &dir); log_msg("ALSA: buffer time %u\n", buffer_time); if ((ret = snd_pcm_hw_params(card->handle, card->hparams)) < 0) { log_msg("ALSA: can't set hardware: %s\n", snd_strerror(ret)); close_card(card); return SCM_BOOL_F; } else log_msg("ALSA: hardware configd\n"); snd_pcm_sw_params_malloc(&sparams); snd_pcm_sw_params_current(card->handle, sparams); snd_pcm_sw_params_set_avail_min(card->handle, sparams, card->blocksize); snd_pcm_sw_params_set_start_threshold(card->handle, sparams, 0U); if ((ret = snd_pcm_sw_params(card->handle, sparams)) < 0) { log_msg("ALSA: can't set software: %s\n", snd_strerror(ret)); } else log_msg("ALSA: software configd\n"); snd_pcm_sw_params_free(sparams); card->link = cards; cards = card; src = (SOURCE_HANDLE *)my_gc_malloc(sizeof(SOURCE_HANDLE), "alsa_card", "alsa card handle"); src->body = (void *)card; src->src = &card->base; log_msg("ALSA: opened %s '%s'\n", card->device, card->name); SCM_NEWSMOB(smob, alsa_card_tag, src); return smob; }