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;
}
void CompassCalibrator::update(bool &failure) {
failure = false;
if(!fitting()) {
return;
}
if(_status == COMPASS_CAL_RUNNING_STEP_ONE) {
if (_fit_step >= 10) {
if(is_equal(_fitness,_initial_fitness) || isnan(_fitness)) { //if true, means that fitness is diverging instead of converging
set_status(COMPASS_CAL_FAILED);
failure = true;
}
set_status(COMPASS_CAL_RUNNING_STEP_TWO);
} else {
if (_fit_step == 0) {
calc_initial_offset();
}
run_sphere_fit();
_fit_step++;
}
} else if(_status == COMPASS_CAL_RUNNING_STEP_TWO) {
if (_fit_step >= 35) {
if(fit_acceptable()) {
set_status(COMPASS_CAL_SUCCESS);
} else {
set_status(COMPASS_CAL_FAILED);
failure = true;
}
} else if (_fit_step < 15) {
run_sphere_fit();
_fit_step++;
} else {
run_ellipsoid_fit();
_fit_step++;
}
}
}