float * nlfit
(
int ts_length, /* length of time series data */
float * ts_array, /* input time series array */
char ** label /* label output for this voxel */
)
{
float * fit; /* nonlinear fit of time series data */
/*----- declare input option variables -----*/
int nabs; /* use absolute constraints for noise parameters */
int nrand; /* number of random vectors to generate */
int nbest; /* number of random vectors to keep */
float rms_min; /* minimum rms error to reject reduced model */
/*----- declare time series variables -----*/
int im1; /* index of 1st image in time series for analysis */
float ** x_array = NULL; /* independent variable matrix */
char * tfilename = NULL; /* file name of time points */
/*----- declare reduced (noise) model variables -----*/
char * nname = NULL; /* noise model name */
vfp nmodel; /* pointer to noise model */
int r; /* number of parameters in the noise model */
char ** npname = NULL; /* noise parameter labels */
float * par_rdcd = NULL; /* estimated parameters for the reduced model */
float sse_rdcd; /* error sum of squares for the reduced model */
float * min_nconstr = NULL; /* min parameter constraints for noise model */
float * max_nconstr = NULL; /* max parameter constraints for noise model */
/*----- declare full (signal+noise) model variables -----*/
char * sname = NULL; /* signal model name */
vfp smodel; /* pointer to signal model */
int p; /* number of parameters in the signal model */
char ** spname = NULL; /* signal parameter labels */
float * par_full = NULL; /* estimated parameters for the full model */
float sse_full; /* error sum of squares for the full model */
float * tpar_full = NULL; /* t-statistic of parameters in full model */
float freg; /* f-statistic for the full regression model */
float rmsreg; /* rms for the full regression model */
float rsqr; /* R^2 (coef. of multiple determination) */
float smax; /* signed maximum of signal */
float tmax; /* epoch of signed maximum of signal */
float pmax; /* percentage change due to signal */
float area; /* area between signal and baseline */
float parea; /* percent area between signal and baseline */
float * min_sconstr = NULL; /* min parameter constraints for signal model */
float * max_sconstr = NULL; /* max parameter constraints for signal model */
int novar; /* flag for insufficient variation in the data */
/*----- program initialization -----*/
initialize_program (&im1, &nname, &sname, &nmodel, &smodel,
&r, &p, &npname, &spname,
&min_nconstr, &max_nconstr, &min_sconstr, &max_sconstr,
&nabs, &nrand, &nbest, &rms_min,
&par_rdcd, &par_full, &tpar_full,
ts_length, &tfilename, &x_array, &fit);
/*----- calculate the reduced (noise) model -----*/
calc_reduced_model (ts_length, r, x_array, ts_array,
par_rdcd, &sse_rdcd);
/*----- calculate the full (signal+noise) model -----*/
calc_full_model (nmodel, smodel, r, p,
min_nconstr, max_nconstr, min_sconstr, max_sconstr,
ts_length, x_array, ts_array, par_rdcd, sse_rdcd,
nabs, nrand, nbest, rms_min, par_full, &sse_full, &novar);
/*----- create estimated time series using the full model parameters -----*/
full_model (nmodel, smodel, par_full, par_full + r,
ts_length, x_array, fit);
/*----- calculate statistics for the full model -----*/
analyze_results (nmodel, smodel, r, p, novar,
min_nconstr, max_nconstr, min_sconstr, max_sconstr,
ts_length, x_array,
par_rdcd, sse_rdcd, par_full, sse_full,
&rmsreg, &freg, &rsqr, &smax, &tmax, &pmax, &area, &parea,
tpar_full);
/*----- report results for this voxel -----*/
report_results (nname, sname, r, p, npname, spname, ts_length,
par_rdcd, sse_rdcd, par_full, sse_full, tpar_full,
rmsreg, freg, rsqr, smax, tmax, pmax, area, parea, label);
printf ("\nVoxel Results: \n");
printf ("%s \n", *label);
/*----- end of program -----*/
terminate_program (r, p, ts_length, &x_array,
&par_rdcd, &min_nconstr, &max_nconstr,
&par_full, &tpar_full,
&min_sconstr, &max_sconstr);
return (fit);
}
pcl::PointCloud<PointT>::Ptr Meshing(const std::vector< pcl::PointCloud<PointT>::Ptr > &cloud_set,
const std::vector< KEYSET > &keypt_set,
const std::vector< cv::Mat > &keydescr_set)
{
int frame_num = cloud_set.size();
if( frame_num <= 0 )
return pcl::PointCloud<PointT>::Ptr (new pcl::PointCloud<PointT>());
PointT dummy;
dummy.x = 0;dummy.y = 0;dummy.z = 0;
std::vector< pcl::PointCloud<PointT>::Ptr > keypt_cloud(frame_num);
std::vector< pcl::PointCloud<PointT>::Ptr > cloud_f_set(frame_num);
std::vector< cv::Mat > key_active_set(frame_num);
for(int i = 0 ; i < frame_num ; i++ )
{
int w = cloud_set[i]->width;
int h = cloud_set[i]->height;
keypt_cloud[i] = pcl::PointCloud<PointT>::Ptr (new pcl::PointCloud<PointT>());
std::vector< cv::Mat > mat_tmp;
for( size_t j = 0 ; j < keypt_set[i].size() ; j++ )
{
int cur_y = round(keypt_set[i][j].pt.y);
int cur_x = round(keypt_set[i][j].pt.x);
if( cur_y < h && cur_y >= 0 && cur_x < w && cur_x >= 0)
{
int idx = cur_y*w + cur_x;
if( pcl_isfinite(cloud_set[i]->at(idx).z) == true )
{
keypt_cloud[i]->push_back(cloud_set[i]->at(idx));
mat_tmp.push_back(keydescr_set[i].row(j));
}
}
else
std::cerr << cur_y << " " << cur_x << " " << h << " " << w << std::endl;
}
cv::vconcat(mat_tmp, key_active_set[i]);
cloud_f_set[i] = FilterBoundary(cloud_set[i]);
//std::cerr << keypt_cloud[i]->size() << "***" << key_active_set[i].rows << std::endl;
}
std::vector< Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > tran_set(frame_num);
tran_set[0] = Eigen::Matrix4f::Identity();
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer());
viewer->initCameraParameters();
viewer->addCoordinateSystem(0.1);
viewer->setCameraPosition(0, 0, 0.1, 0, 0, 1, 0, -1, 0);
pcl::registration::CorrespondenceRejectorSampleConsensus<PointT> crsc;
crsc.setInlierThreshold( 0.005 );
crsc.setMaximumIterations( 2000 );
std::vector< pcl::PointCloud<PointT>::Ptr > tran_model_set(frame_num);
pcl::PointCloud<PointT>::Ptr full_model(new pcl::PointCloud<PointT>());
pcl::copyPointCloud(*cloud_f_set[0], *full_model);
tran_model_set[0] = cloud_f_set[0];
viewer->removePointCloud("full");
viewer->addPointCloud(full_model, "full");
viewer->spin();
for(int i = 1 ; i < frame_num ; i++ )
{
std::cerr << "Frame --- " << i << std::endl;
pcl::CorrespondencesPtr cur_corrs = matchSIFT(key_active_set[i], key_active_set[i-1]);
crsc.setInputSource( keypt_cloud[i] );
crsc.setInputTarget( keypt_cloud[i-1] );
crsc.setInputCorrespondences( cur_corrs );
pcl::CorrespondencesPtr in_corr (new pcl::Correspondences());
crsc.getCorrespondences( *in_corr );
Eigen::Matrix4f init_guess = crsc.getBestTransformation();
init_guess = tran_set[i-1] * init_guess;
tran_set[i] = DenseColorICP(cloud_f_set[i], tran_model_set[i-1], init_guess);
pcl::PointCloud<PointT>::Ptr Final (new pcl::PointCloud<PointT>);
pcl::transformPointCloud(*cloud_f_set[i], *Final, tran_set[i]);
tran_model_set[i] = Final;
full_model->insert(full_model->end(), Final->begin(), Final->end());
viewer->removePointCloud("full");
viewer->addPointCloud(full_model, "full");
viewer->spinOnce(1);
}
std::cerr << "Registration Done!!!" << std::endl;
pcl::PointCloud<PointT>::Ptr down_model(new pcl::PointCloud<PointT>());
pcl::VoxelGrid<PointT> sor;
sor.setInputCloud(full_model);
sor.setLeafSize(0.001, 0.001, 0.001);
sor.filter(*down_model);
/*
//*
pcl::search::KdTree<PointT>::Ptr mls_tree (new pcl::search::KdTree<PointT>);
pcl::MovingLeastSquares<PointT, PointT> mls;
// Set parameters
mls.setInputCloud (down_model);
mls.setComputeNormals(false);
mls.setPolynomialFit(true);
mls.setSearchMethod (mls_tree);
mls.setSearchRadius (0.01);
// Reconstruct
pcl::PointCloud<PointT>::Ptr down_model_f(new pcl::PointCloud<PointT>());
mls.process (*down_model_f);
*/
//std::cerr << "Smoothing Done!!!" << std::endl;
//*/
//viewer->removePointCloud("full");
//viewer->addPointCloud(down_model_f, "down");
//viewer->spin();
return down_model;
}
