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;
}
