bool SQ_fitter<PointT>::SQFitting( const PointCloudPtr &_cloud,
const double &_smax,
const double &_smin,
const int &_N,
const double &_thresh ) {
// Store parameters
smax_ = _smax;
smin_ = _smin;
N_ = _N;
thresh_ = _thresh;
cloud_in_ = _cloud;
double ds;
double error_i; double error_i_1;
double s_i;
bool fitted;
SQ_params par_i, par_i_1;
// 1. Initialize a ellipsoid with bounding box dimensions
ds = (smax_ - smin_) / (double) N_;
error_i = initialize( cloud_in_, par_in_ );
par_i = par_in_;
std::cout << "\t [DEBUG] Iteration ["<<0<<"] Cloud size: "<< cloud_in_->points.size()<< ". Error: "<< error_i << std::endl;
std::cout << "\t Initialization parameters:"<< std::endl;
printParamsInfo( par_in_ );
// 2. Iterate through downsampling levels
fitted = false;
for( int i = 1; i <= N_; ++i ) {
error_i_1 = error_i;
par_i_1 = par_i;
s_i = _smax - (i-1)*ds;
PointCloudPtr cloud_i( new pcl::PointCloud<PointT>() );
cloud_i = downsample( cloud_in_, s_i );
error_i = fitting( cloud_i,
par_i_1, par_i );
std::cout << "\t [DEBUG] Iteration ["<<i<<"] Cloud size: "<< cloud_i->points.size()<<
". Voxel size: "<< s_i <<". Error: "<< error_i << std::endl;
// 3. If error between levels is below threshold, stop
if( fabs( error_i - error_i_1 ) < thresh_ ) {
std::cout << "\t [DEBUG-GOOD] Errors diff below threshold. Stop: "<< thresh_ << std::endl;
par_out_ = par_i;
fitted = true;
break;
}
}
return fitted;
}
int ExtractSIFT::compute()
{
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
PCLCloud::Ptr sm_cloud (new PCLCloud);
std::vector<std::string> req_fields;
req_fields.resize(2);
req_fields[0] = "xyz"; // always needed
switch (m_mode)
{
case RGB:
req_fields[1] = "rgb";
break;
case SCALAR_FIELD:
req_fields[1] = m_field_to_use;
break;
}
cc2smReader converter;
converter.setInputCloud(cloud);
converter.getAsSM(req_fields, *sm_cloud);
//Now change the name of the field to use to a standard name, only if in OTHER_FIELD mode
if (m_mode == SCALAR_FIELD)
{
int field_index = pcl::getFieldIndex(*sm_cloud, m_field_to_use_no_space);
sm_cloud->fields.at(field_index).name = "intensity"; //we always use intensity as name... even if it is curvature or another field.
}
//initialize all possible clouds
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_i (new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud (new pcl::PointCloud<pcl::PointXYZ>);
//Now do the actual computation
if (m_mode == SCALAR_FIELD)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_i);
estimateSIFT<pcl::PointXYZI, pcl::PointXYZ>(cloud_i, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
else if (m_mode == RGB)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_rgb);
estimateSIFT<pcl::PointXYZRGB, pcl::PointXYZ>(cloud_rgb, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
PCLCloud::Ptr out_cloud_sm (new PCLCloud);
TO_PCL_CLOUD(*out_cloud, *out_cloud_sm);
if ( (out_cloud_sm->height * out_cloud_sm->width) == 0)
{
//cloud is empty
return -53;
}
ccPointCloud* out_cloud_cc = sm2ccConverter(out_cloud_sm).getCCloud();
if (!out_cloud_cc)
{
//conversion failed (not enough memory?)
return -1;
}
std::stringstream name;
if (m_mode == RGB)
name << "SIFT Keypoints_" << m_nr_octaves << "_" << "rgb" << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
else
name << "SIFT Keypoints_" << m_nr_octaves << "_" << m_field_to_use_no_space << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
out_cloud_cc->setName(name.str().c_str());
out_cloud_cc->setDisplay(cloud->getDisplay());
if (cloud->getParent())
cloud->getParent()->addChild(out_cloud_cc);
emit newEntity(out_cloud_cc);
return 1;
}
int ExtractSIFT::compute()
{
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
std::list<std::string> req_fields;
try
{
req_fields.push_back("xyz"); // always needed
switch (m_mode)
{
case RGB:
req_fields.push_back("rgb");
break;
case SCALAR_FIELD:
req_fields.push_back(qPrintable(m_field_to_use)); //DGM: warning, toStdString doesn't preserve "local" characters
break;
default:
assert(false);
break;
}
}
catch (const std::bad_alloc&)
{
//not enough memory
return -1;
}
PCLCloud::Ptr sm_cloud = cc2smReader(cloud).getAsSM(req_fields);
if (!sm_cloud)
return -1;
//Now change the name of the field to use to a standard name, only if in OTHER_FIELD mode
if (m_mode == SCALAR_FIELD)
{
int field_index = pcl::getFieldIndex(*sm_cloud, m_field_to_use_no_space);
sm_cloud->fields.at(field_index).name = "intensity"; //we always use intensity as name... even if it is curvature or another field.
}
//initialize all possible clouds
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_i (new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud (new pcl::PointCloud<pcl::PointXYZ>);
//Now do the actual computation
if (m_mode == SCALAR_FIELD)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_i);
estimateSIFT<pcl::PointXYZI, pcl::PointXYZ>(cloud_i, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
else if (m_mode == RGB)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_rgb);
estimateSIFT<pcl::PointXYZRGB, pcl::PointXYZ>(cloud_rgb, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
PCLCloud::Ptr out_cloud_sm (new PCLCloud);
TO_PCL_CLOUD(*out_cloud, *out_cloud_sm);
if ( out_cloud_sm->height * out_cloud_sm->width == 0)
{
//cloud is empty
return -53;
}
ccPointCloud* out_cloud_cc = sm2ccConverter(out_cloud_sm).getCloud();
if (!out_cloud_cc)
{
//conversion failed (not enough memory?)
return -1;
}
std::stringstream name;
if (m_mode == RGB)
name << "SIFT Keypoints_" << m_nr_octaves << "_" << "rgb" << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
else
name << "SIFT Keypoints_" << m_nr_octaves << "_" << m_field_to_use_no_space << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
out_cloud_cc->setName(name.str().c_str());
out_cloud_cc->setDisplay(cloud->getDisplay());
//copy global shift & scale
out_cloud_cc->setGlobalScale(cloud->getGlobalScale());
out_cloud_cc->setGlobalShift(cloud->getGlobalShift());
if (cloud->getParent())
cloud->getParent()->addChild(out_cloud_cc);
emit newEntity(out_cloud_cc);
return 1;
}
bool SQ_fitter_b<PointT>::fit( const int &_type,
const double &_smax,
const double &_smin,
const int &_N,
const double &_thresh ) {
// 0. Store parameters
this->smax_ = _smax;
this->smin_ = _smin;
this->N_ = _N;
this->thresh_ = _thresh;
double ds; double error_i; double error_i_1;
double s_i; bool fitted;
SQ_parameters par_i, par_i_1;
if( this->N_ == 1 ) { ds = 0; }
else { ds = (this->smax_ - this->smin_)/(double)(this->N_-1); }
// 1. Initialize par_in_ with bounding box values
if( this->mGotInitApprox ) {
for( int i = 0; i < 3; ++i ) {
this->par_in_.dim[i] = this->mInitDim[i];
this->par_in_.trans[i] = this->mInitTrans[i];
this->par_in_.rot[i] = this->mInitRot[i];
}
this->mGotInitApprox = false;
} else {
this->getBoundingBox( this->cloud_,
this->par_in_.dim,
this->par_in_.trans,
this->par_in_.rot,
false );
}
//for( int i = 0; i < 3; ++i ) { this->mUpperLim_dim[i] = this->mDimFactor*this->par_in_.dim[i]; }
// 1.0 Set tampering start to 1 (0: No bending )
this->par_in_.k = 0.01;
this->par_in_.alpha = 0;
this->par_in_.type = BENT;
// 1.1. Set e1 and e2 to middle value in range
this->par_in_.e[0] = 0.5; this->par_in_.e[1] = 1.0;
// Run loop
par_i = this->par_in_;
double eg, er;
this->get_error( par_i, this->cloud_, eg, er, error_i );
fitted = false;
///
for( int i = 0; i < this->N_; ++i ) {
s_i = this->smax_ - (i)*ds;
par_i_1 = par_i;
error_i_1 = error_i;
// Update limits**********
double dim_i[3];
this->getBoundingBoxAlignedToTf( this->cloud_,
par_i_1.trans,
par_i_1.rot,
dim_i );
for( int j = 0; j < 3; ++j ) {
if( this->mUpperLim_dim[j] < this->mDimFactor*dim_i[j] ) {
this->mUpperLim_dim[j] = this->mDimFactor*dim_i[j];
}
}
//****************************
PointCloudPtr cloud_i( new pcl::PointCloud<PointT>() );
if( this->N_ == 1 ) { cloud_i = this->cloud_; }
else { downsampling<PointT>( this->cloud_, s_i, cloud_i ); }
if( cloud_i->points.size() < 12 ) { continue; }
minimize( _type,
cloud_i,
par_i_1,
par_i,
error_i );
// [CONDITION]
double de = (error_i_1 - error_i);
this->final_error_ = error_i;
// if( fabs(de) < this->thresh_ && error_i < 0.01 ) {
if( fabs(de) < this->thresh_ ) {
fitted = true;
break;
}
}
this->par_out_ = par_i;
printf( "Dim: %f %f %f E: %f %f alpha: %f k: %f\n",
this->par_out_.dim[0], this->par_out_.dim[1], this->par_out_.dim[2],
this->par_out_.e[0], this->par_out_.e[1],
this->par_out_.alpha,
this->par_out_.k );
return fitted;
}
