std::vector<double> RainWaterHarvestingOptions::substractVectors(std::vector<double> & vec1, std::vector<double> & vec2){
std::vector<double> res_vec(vec1.size());
for (int i = 0; i < vec1.size(); i++)
res_vec[i] = vec1[i] - vec2[i];
return res_vec;
}
std::vector<double> WaterDemandModel::mutiplyVector(std::vector<double> &vec, double multiplyer)
{
std::vector<double> res_vec(vec.size());
for (int i = 0; i < vec.size(); i++)
res_vec[i] = vec[i] * multiplyer;
return res_vec;
}
void m3math_test_object_t::test<6>()
{
LLMatrix3 llmat_obj1;
LLVector3 llvec(1, 3, 5);
LLVector3 res_vec(0, 0, 0);
LLVector3 llvec1(1, 3, 5);
LLVector3 llvec2(3, 6, 1);
LLVector3 llvec3(4, 6, 9);
llmat_obj1.setRows(llvec1, llvec2, llvec3);
res_vec = llvec * llmat_obj1;
LLVector3 expected_result(30, 51, 53);
ensure("LLMatrix3::operator*(const LLVector3 &a, const LLMatrix3 &b) failed", res_vec == expected_result);
}
bool
FrictionalContactProblem::calculateSlip(const NumericVector<Number>& ghosted_solution,
std::vector<SlipData> * iterative_slip)
{
NonlinearSystem & nonlinear_sys = getNonlinearSystem();
unsigned int dim = nonlinear_sys.subproblem().mesh().dimension();
MooseVariable * residual_x_var = &getVariable(0,_residual_x);
MooseVariable * residual_y_var = &getVariable(0,_residual_y);
MooseVariable * residual_z_var = NULL;
MooseVariable * diag_stiff_x_var = &getVariable(0,_diag_stiff_x);
MooseVariable * diag_stiff_y_var = &getVariable(0,_diag_stiff_y);
MooseVariable * diag_stiff_z_var = NULL;
MooseVariable * inc_slip_x_var = &getVariable(0,_inc_slip_x);
MooseVariable * inc_slip_y_var = &getVariable(0,_inc_slip_y);
MooseVariable * inc_slip_z_var = NULL;
if (dim == 3)
{
residual_z_var = &getVariable(0,_residual_z);
diag_stiff_z_var = &getVariable(0,_diag_stiff_z);
inc_slip_z_var = &getVariable(0,_inc_slip_z);
}
bool updatedSolution = false;
_slip_residual = 0.0;
_it_slip_norm = 0.0;
_inc_slip_norm = 0.0;
TransientNonlinearImplicitSystem & system = getNonlinearSystem().sys();
if (iterative_slip)
iterative_slip->clear();
if (getDisplacedProblem() && _interaction_params.size() > 0)
{
computeResidual(system, ghosted_solution, *system.rhs);
_num_contact_nodes = 0;
_num_slipping = 0;
_num_slipped_too_far = 0;
GeometricSearchData & displaced_geom_search_data = getDisplacedProblem()->geomSearchData();
std::map<std::pair<unsigned int, unsigned int>, PenetrationLocator *> * penetration_locators = &displaced_geom_search_data._penetration_locators;
AuxiliarySystem & aux_sys = getAuxiliarySystem();
const NumericVector<Number> & aux_solution = *aux_sys.currentSolution();
for (std::map<std::pair<unsigned int, unsigned int>, PenetrationLocator *>::iterator plit = penetration_locators->begin();
plit != penetration_locators->end();
++plit)
{
PenetrationLocator & pen_loc = *plit->second;
std::set<dof_id_type> & has_penetrated = pen_loc._has_penetrated;
bool frictional_contact_this_interaction = false;
std::map<std::pair<int,int>,InteractionParams>::iterator ipit;
std::pair<int,int> ms_pair(pen_loc._master_boundary,pen_loc._slave_boundary);
ipit = _interaction_params.find(ms_pair);
if (ipit != _interaction_params.end())
frictional_contact_this_interaction = true;
if (frictional_contact_this_interaction)
{
InteractionParams & interaction_params = ipit->second;
Real slip_factor = interaction_params._slip_factor;
Real slip_too_far_factor = interaction_params._slip_too_far_factor;
Real friction_coefficient = interaction_params._friction_coefficient;
std::vector<dof_id_type> & slave_nodes = pen_loc._nearest_node._slave_nodes;
for (unsigned int i=0; i<slave_nodes.size(); i++)
{
dof_id_type slave_node_num = slave_nodes[i];
if (pen_loc._penetration_info[slave_node_num])
{
PenetrationInfo & info = *pen_loc._penetration_info[slave_node_num];
const Node * node = info._node;
if (node->processor_id() == processor_id())
{
std::set<dof_id_type>::iterator hpit = has_penetrated.find(slave_node_num);
if (hpit != has_penetrated.end())
{
_num_contact_nodes++;
VectorValue<dof_id_type> residual_dofs(node->dof_number(aux_sys.number(), residual_x_var->number(), 0),
node->dof_number(aux_sys.number(), residual_y_var->number(), 0),
(residual_z_var ? node->dof_number(aux_sys.number(), residual_z_var->number(), 0) : 0));
VectorValue<dof_id_type> diag_stiff_dofs(node->dof_number(aux_sys.number(), diag_stiff_x_var->number(), 0),
node->dof_number(aux_sys.number(), diag_stiff_y_var->number(), 0),
(diag_stiff_z_var ? node->dof_number(aux_sys.number(), diag_stiff_z_var->number(), 0) : 0));
VectorValue<dof_id_type> inc_slip_dofs(node->dof_number(aux_sys.number(), inc_slip_x_var->number(), 0),
node->dof_number(aux_sys.number(), inc_slip_y_var->number(), 0),
(inc_slip_z_var ? node->dof_number(aux_sys.number(), inc_slip_z_var->number(), 0) : 0));
RealVectorValue res_vec;
RealVectorValue stiff_vec;
RealVectorValue slip_inc_vec;
for (unsigned int i=0; i<dim; ++i)
{
res_vec(i) = aux_solution(residual_dofs(i));
stiff_vec(i) = aux_solution(diag_stiff_dofs(i));
slip_inc_vec(i) = aux_solution(inc_slip_dofs(i));
}
RealVectorValue slip_iterative(0.0,0.0,0.0);
Real interaction_slip_residual = 0.0;
// _console<<"inc slip: "<<slip_inc_vec<<std::endl;
// _console<<"info slip: "<<info._incremental_slip<<std::endl;
// ContactState state = calculateInteractionSlip(slip_iterative, interaction_slip_residual, info._normal, res_vec, info._incremental_slip, stiff_vec, friction_coefficient, slip_factor, slip_too_far_factor, dim);
ContactState state = calculateInteractionSlip(slip_iterative, interaction_slip_residual, info._normal, res_vec, slip_inc_vec, stiff_vec, friction_coefficient, slip_factor, slip_too_far_factor, dim);
// _console<<"iter slip: "<<slip_iterative<<std::endl;
_slip_residual += interaction_slip_residual*interaction_slip_residual;
if (state == SLIPPING || state == SLIPPED_TOO_FAR)
{
_num_slipping++;
if (state == SLIPPED_TOO_FAR)
_num_slipped_too_far++;
for (unsigned int i=0; i<dim; ++i)
{
SlipData sd(node,i,slip_iterative(i));
if (iterative_slip)
iterative_slip->push_back(sd);
_it_slip_norm += slip_iterative(i)*slip_iterative(i);
_inc_slip_norm += (slip_inc_vec(i)+slip_iterative(i))*(slip_inc_vec(i)+slip_iterative(i));
}
}
}
}
}
}
}
}
_communicator.sum(_num_contact_nodes);
_communicator.sum(_num_slipping);
_communicator.sum(_num_slipped_too_far);
_communicator.sum(_slip_residual);
_slip_residual = std::sqrt(_slip_residual);
_communicator.sum(_it_slip_norm);
_it_slip_norm = std::sqrt(_it_slip_norm);
_communicator.sum(_inc_slip_norm);
_inc_slip_norm = std::sqrt(_inc_slip_norm);
if (_num_slipping > 0)
updatedSolution = true;
}
return updatedSolution;
}
void
ContactMaster::computeContactForce(PenetrationInfo * pinfo)
{
std::map<unsigned int, Real> & lagrange_multiplier = _penetration_locator._lagrange_multiplier;
const Node * node = pinfo->_node;
RealVectorValue res_vec;
// Build up residual vector
for (unsigned int i=0; i<_mesh_dimension; ++i)
{
long int dof_number = node->dof_number(0, _vars(i), 0);
res_vec(i) = _residual_copy(dof_number);
}
const Real area = nodalArea(*pinfo);
RealVectorValue distance_vec(_mesh.node(node->id()) - pinfo->_closest_point);
RealVectorValue pen_force(_penalty * distance_vec);
if (_normalize_penalty)
pen_force *= area;
RealVectorValue tan_residual(0,0,0);
if (_model == CM_FRICTIONLESS)
{
switch (_formulation)
{
case CF_DEFAULT:
pinfo->_contact_force = -pinfo->_normal * (pinfo->_normal * res_vec);
break;
case CF_PENALTY:
pinfo->_contact_force = pinfo->_normal * (pinfo->_normal * pen_force);
break;
case CF_AUGMENTED_LAGRANGE:
pinfo->_contact_force = (pinfo->_normal * (pinfo->_normal *
//( pen_force + (lagrange_multiplier[node->id()]/distance_vec.size())*distance_vec)));
( pen_force + lagrange_multiplier[node->id()] * pinfo->_normal)));
break;
default:
mooseError("Invalid contact formulation");
break;
}
pinfo->_mech_status=PenetrationInfo::MS_SLIPPING;
}
else if (_model == CM_COULOMB && _formulation == CF_PENALTY)
{
distance_vec = pinfo->_incremental_slip + (pinfo->_normal * (_mesh.node(node->id()) - pinfo->_closest_point)) * pinfo->_normal;
pen_force = _penalty * distance_vec;
if (_normalize_penalty)
pen_force *= area;
// Frictional capacity
// const Real capacity( _friction_coefficient * (pen_force * pinfo->_normal < 0 ? -pen_force * pinfo->_normal : 0) );
const Real capacity( _friction_coefficient * (res_vec * pinfo->_normal > 0 ? res_vec * pinfo->_normal : 0) );
// Elastic predictor
pinfo->_contact_force = pen_force + (pinfo->_contact_force_old - pinfo->_normal*(pinfo->_normal*pinfo->_contact_force_old));
RealVectorValue contact_force_normal( (pinfo->_contact_force*pinfo->_normal) * pinfo->_normal );
RealVectorValue contact_force_tangential( pinfo->_contact_force - contact_force_normal );
// Tangential magnitude of elastic predictor
const Real tan_mag( contact_force_tangential.size() );
if ( tan_mag > capacity )
{
pinfo->_contact_force = contact_force_normal + capacity * contact_force_tangential / tan_mag;
pinfo->_mech_status=PenetrationInfo::MS_SLIPPING;
}
else
{
pinfo->_mech_status=PenetrationInfo::MS_STICKING;
}
}
else if (_model == CM_GLUED ||
(_model == CM_COULOMB && _formulation == CF_DEFAULT))
{
switch (_formulation)
{
case CF_DEFAULT:
pinfo->_contact_force = -res_vec;
break;
case CF_PENALTY:
pinfo->_contact_force = pen_force;
break;
case CF_AUGMENTED_LAGRANGE:
pinfo->_contact_force = pen_force +
lagrange_multiplier[node->id()]*distance_vec/distance_vec.size();
break;
default:
mooseError("Invalid contact formulation");
break;
}
pinfo->_mech_status=PenetrationInfo::MS_STICKING;
}
else
{
mooseError("Invalid or unavailable contact model");
}
}
// For lots of subsets of size nwhich, compute the exact fit to those data
// points and the residuals from all the data points.
// copied with modification from MASS/src/lqs.c
// Copyright (C) 1998-2007 B. D. Ripley
// Copyright (C) 1999 R Development Core Team
// TODO: rewrite
void LQSEstimator::operator()(const Data& data, double* coef_ptr,
double* fitted_ptr, double* resid_ptr,
double* scale_ptr) {
int nnew = nwhich, pp = p;
int i, iter, nn = n, trial;
int rank, info, n100 = 100;
int firsttrial = 1;
double a = 0.0, tol = 1.0e-7, sum, thiscrit, best = DBL_MAX, target, old,
newp, dummy, k0 = pk0;
const arma::vec& y = data.y;
const arma::mat& x = data.x;
double coef[p];
arma::vec coef_vec(coef, p, false, true);
double qraux[p];
double work[2*p];
double res[n];
arma::vec res_vec(res, n, false, true);
double yr[nwhich];
double xr[nwhich * p];
arma::vec yr_vec(yr, nwhich, false, true);
arma::mat xr_mat(xr, nwhich, p, false, true);
double bestcoef[p];
int pivot[p];
arma::uvec which_vec(nwhich);
//int bestone[nwhich];
target = (nn - pp)* (beta);
for(trial = 0; trial < ntrials; trial++) {
R_CheckUserInterrupt();
// get this trial's subset
which_vec = indices.col(trial);
yr_vec = y.elem(which_vec);
xr_mat = x.rows(which_vec);
/* compute fit, find residuals */
F77_CALL(dqrdc2)(xr, &nnew, &nnew, &pp, &tol, &rank, qraux, pivot, work);
if(rank < pp) { sing++; continue; }
F77_CALL(dqrsl)(xr, &nnew, &nnew, &rank, qraux, yr, &dummy, yr, coef,
&dummy, &dummy, &n100, &info);
res_vec = y - x * coef_vec;
/* S estimation */
if(firsttrial) {
for(i = 0; i < nn; i ++) res[i] = fabs(res[i]);
rPsort(res, nn, nn/2);
old = res[nn/2]/0.6745; /* MAD provides the initial scale */
firsttrial = 0;
} else {
/* only find optimal scale if it will be better than
existing best solution */
sum = 0.0;
for(i = 0; i < nn; i ++) sum += chi(res[i], k0 * best);
if(sum > target) continue;
old = best;
}
/* now solve for scale S by re-substitution */
for(iter = 0; iter < 30; iter++) {
/*printf("iter %d, s = %f sum = %f %f\n", iter, old, sum, target);*/
sum = 0.0;
for(i = 0; i < nn; i ++) sum += chi(res[i], k0 * old);
newp = sqrt(sum/target) * old;
if(fabs(sum/target - 1.) < 1e-4) break;
old = newp;
}
thiscrit = newp;
/* first trial might be singular, so use fence */
if(thiscrit < best) {
sum = 0.0;
for(i = 0; i < nn; i ++) sum += chi(res[i], k0 * best);
best = thiscrit;
for(i = 0; i < pp; i++) bestcoef[i] = coef[i];
bestcoef[0] += a;
}
} /* for(trial in 0:ntrials) */
crit = (best < 0.0) ? 0.0 : best;
if(sample) PutRNGstate();
/* lqs_free(); */
// output
arma::vec coef_out(coef_ptr, p, false, true);
arma::vec fitted_out(fitted_ptr, n, false, true);
arma::vec resid_out(resid_ptr, n, false, true);
arma::vec scale_out(scale_ptr, 1, false, true);
for (i = 0; i < p; i++) coef_out[i] = bestcoef[i];
fitted_out = x * coef_out;
resid_out = y - fitted_out;
scale_out = crit;
}
void
MechanicalContactConstraint::computeContactForce(PenetrationInfo * pinfo)
{
const Node * node = pinfo->_node;
RealVectorValue res_vec;
// Build up residual vector
for (unsigned int i=0; i<_mesh_dimension; ++i)
{
dof_id_type dof_number = node->dof_number(0, _vars(i), 0);
res_vec(i) = _residual_copy(dof_number);
}
RealVectorValue distance_vec(_mesh.node(node->id()) - pinfo->_closest_point);
const Real penalty = getPenalty(*pinfo);
RealVectorValue pen_force(penalty * distance_vec);
switch (_model)
{
case CM_FRICTIONLESS:
switch (_formulation)
{
case CF_KINEMATIC:
pinfo->_contact_force = -pinfo->_normal * (pinfo->_normal * res_vec);
break;
case CF_PENALTY:
pinfo->_contact_force = pinfo->_normal * (pinfo->_normal * pen_force);
break;
case CF_AUGMENTED_LAGRANGE:
pinfo->_contact_force = (pinfo->_normal * (pinfo->_normal *
( pen_force + pinfo->_lagrange_multiplier * pinfo->_normal)));
break;
default:
mooseError("Invalid contact formulation");
break;
}
pinfo->_mech_status=PenetrationInfo::MS_SLIPPING;
break;
case CM_COULOMB:
switch (_formulation)
{
case CF_KINEMATIC:
pinfo->_contact_force = -res_vec;
break;
case CF_PENALTY:
{
distance_vec = pinfo->_incremental_slip + (pinfo->_normal * (_mesh.node(node->id()) - pinfo->_closest_point)) * pinfo->_normal;
pen_force = penalty * distance_vec;
// Frictional capacity
// const Real capacity( _friction_coefficient * (pen_force * pinfo->_normal < 0 ? -pen_force * pinfo->_normal : 0) );
const Real capacity( _friction_coefficient * (res_vec * pinfo->_normal > 0 ? res_vec * pinfo->_normal : 0) );
// Elastic predictor
pinfo->_contact_force = pen_force + (pinfo->_contact_force_old - pinfo->_normal*(pinfo->_normal*pinfo->_contact_force_old));
RealVectorValue contact_force_normal( (pinfo->_contact_force*pinfo->_normal) * pinfo->_normal );
RealVectorValue contact_force_tangential( pinfo->_contact_force - contact_force_normal );
// Tangential magnitude of elastic predictor
const Real tan_mag( contact_force_tangential.size() );
if ( tan_mag > capacity )
{
pinfo->_contact_force = contact_force_normal + capacity * contact_force_tangential / tan_mag;
pinfo->_mech_status=PenetrationInfo::MS_SLIPPING;
}
else
pinfo->_mech_status=PenetrationInfo::MS_STICKING;
break;
}
case CF_AUGMENTED_LAGRANGE:
pinfo->_contact_force = pen_force +
pinfo->_lagrange_multiplier*distance_vec/distance_vec.size();
break;
default:
mooseError("Invalid contact formulation");
break;
}
break;
case CM_GLUED:
switch (_formulation)
{
case CF_KINEMATIC:
pinfo->_contact_force = -res_vec;
break;
case CF_PENALTY:
pinfo->_contact_force = pen_force;
break;
case CF_AUGMENTED_LAGRANGE:
pinfo->_contact_force = pen_force +
pinfo->_lagrange_multiplier*distance_vec/distance_vec.size();
break;
default:
mooseError("Invalid contact formulation");
break;
}
pinfo->_mech_status=PenetrationInfo::MS_STICKING;
break;
default:
mooseError("Invalid or unavailable contact model");
break;
}
}
void
MultiDContactConstraint::updateContactSet()
{
std::set<dof_id_type> & has_penetrated = _penetration_locator._has_penetrated;
std::map<dof_id_type, PenetrationInfo *>::iterator
it = _penetration_locator._penetration_info.begin(),
end = _penetration_locator._penetration_info.end();
for (; it!=end; ++it)
{
PenetrationInfo * pinfo = it->second;
// Skip this pinfo if there are no DOFs on this node.
if ( ! pinfo || pinfo->_node->n_comp(_sys.number(), _vars(_component)) < 1 )
continue;
const Node * node = pinfo->_node;
dof_id_type slave_node_num = it->first;
std::set<dof_id_type>::iterator hpit = has_penetrated.find(slave_node_num);
RealVectorValue res_vec;
// Build up residual vector
for (unsigned int i=0; i<_mesh_dimension; ++i)
{
dof_id_type dof_number = node->dof_number(0, _vars(i), 0);
res_vec(i) = _residual_copy(dof_number);
}
// Real resid = 0;
switch (_model)
{
case CM_FRICTIONLESS:
// resid = pinfo->_normal * res_vec;
break;
case CM_GLUED:
// resid = pinfo->_normal * res_vec;
break;
default:
mooseError("Invalid or unavailable contact model");
break;
}
// if (hpit != has_penetrated.end() && resid < 0)
// Moose::err<<resid<<std::endl;
/*
if (hpit != has_penetrated.end() && resid < -.15)
{
Moose::err<<std::endl<<"Unlocking node "<<node->id()<<" because resid: "<<resid<<std::endl<<std::endl;
has_penetrated.erase(hpit);
unlocked_this_step[slave_node_num] = true;
}
else*/
if (pinfo->_distance > 0 && hpit == has_penetrated.end())// && !unlocked_this_step[slave_node_num])
{
// Moose::err<<std::endl<<"Locking node "<<node->id()<<" because distance: "<<pinfo->_distance<<std::endl<<std::endl;
has_penetrated.insert(slave_node_num);
}
}
}
Real
MultiDContactConstraint::computeQpResidual(Moose::ConstraintType type)
{
PenetrationInfo * pinfo = _penetration_locator._penetration_info[_current_node->id()];
const Node * node = pinfo->_node;
RealVectorValue res_vec;
// Build up residual vector
for (unsigned int i=0; i<_mesh_dimension; ++i)
{
dof_id_type dof_number = node->dof_number(0, _vars(i), 0);
res_vec(i) = _residual_copy(dof_number);
}
const RealVectorValue distance_vec(_mesh.node(node->id()) - pinfo->_closest_point);
const RealVectorValue pen_force(_penalty * distance_vec);
Real resid = 0;
switch (type)
{
case Moose::Slave:
switch (_model)
{
case CM_FRICTIONLESS:
resid = pinfo->_normal(_component) * (pinfo->_normal * ( pen_force - res_vec ));
break;
case CM_GLUED:
resid = pen_force(_component)
- res_vec(_component)
;
break;
default:
mooseError("Invalid or unavailable contact model");
break;
}
return _test_slave[_i][_qp] * resid;
case Moose::Master:
switch (_model)
{
case CM_FRICTIONLESS:
resid = pinfo->_normal(_component) * (pinfo->_normal * res_vec);
break;
case CM_GLUED:
resid = res_vec(_component);
break;
default:
mooseError("Invalid or unavailable contact model");
break;
}
return _test_master[_i][_qp] * resid;
}
return 0;
}
int main(int argc,char** argv){
if (argc < 2){
std::cout<<"Please enter <input.pcd> file"<<std::endl;
return 0;
}
if (pcl::io::loadPCDFile (argv[1], *model) < 0)
{
std::cout << "Error loading model cloud." << std::endl;
return (-1);
}
model_name = argv[1];
model_name = model_name.substr(0,model_name.size()-4);
if(pcl::console::find_switch(argc,argv,"-vfh")){
vfh = true;
}
else if(pcl::console::find_switch(argc,argv,"-rv")){
std::cout<<"performing Resultant vector feature calculation"<<std::endl;
rv = true;
}else{
std::cout<<"no algorithm specified using default algorithm vfh"<<std::endl;
vfh = true;
}
if (pcl::console::find_switch (argc, argv, "-ds"))
{
_downsample = true;
std::cout<<"performing downsampling on the input file"<<std::endl;
}
if (pcl::console::find_switch (argc, argv, "-sn"))
{
show_normals = true;
std::cout<<"showing calclated normals"<<std::endl;
}
if(_downsample){
rec.setInputCloud(model,_leaf_size);
std::cout<<"saving downsampled file to model_down.pcd"<<std::endl;
pcl::io::savePCDFileASCII ("model_down.pcd", *(rec.getCloud()));
}
else{
rec.setInputCloud(model);
std::cout<<"setting input model without further downsampling"<<std::endl;
}
if(pcl::console::find_switch(argc,argv,"--showaxis")){
_show_axis = true;
}
if(vfh){
std::cout<<"estimating VFH features"<<std::endl;
rec.Estimate_VFH_Features();
}
else if(rv){
std::cout<<"estimating features using the resultant vector"<<std::endl;
rec.Estimate_RV_Features();
PointNormalCloudT::Ptr cloud (new PointNormalCloudT);
pcl::PointCloud<pcl::Normal>::Ptr norm_cloud (new pcl::PointCloud<pcl::Normal>);
cloud = rec.getPointNormalCloud();
norm_cloud = rec.getNormalCloud();
pcl::PointCloud<pcl::PointXYZ>::Ptr plaincloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud,*plaincloud);
//pcl::PointXYZ mass_center(rec.rv_centroid.x,rec.rv_centroid.y,rec.rv_centroid.z);
pcl::PointXYZ mass_center(0,0,0);
pcl::PointXYZ kinectZ(0,0,-1);
pcl::PointXYZ res_vec (rec.rv_resultant.normal_x,rec.rv_resultant.normal_y,rec.rv_resultant.normal_z);
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZ> rgb(plaincloud);
//viewer.addPointCloud<pcl::PointXYZ> (plaincloud, rgb, "model_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointNormal> single_color(cloud, double(0), double(255), double(0));
viewer.addPointCloud(cloud,single_color,"sample cloud");
if(_show_axis){
viewer.addLine(mass_center,res_vec,1.0f,0.0f,0.0f,"resultantvector");
viewer.addLine(mass_center,kinectZ,1.0f,1.0f,0.0f,"KinectZ");
}
std::cout<<"resultant vector :"<<res_vec.x<<" i"<<" + "<<res_vec.y<<" j"<<" + "<<res_vec.z<<" k"<<std::endl;
if(show_normals){
std::cout<<"showing 1 in "<<normalsratio<<" normals"<<std::endl;
viewer.addPointCloudNormals<pcl::PointNormal,pcl::Normal>(cloud, norm_cloud,normalsratio, 0.05, "normalscloud");
}
while(!viewer.wasStopped())
viewer.spinOnce();
}
std::cout<<"feature calculation complete"<<std::endl;
ofstream myfile;
if (vfh){
std::stringstream ss;
ss<<"vfh_"<<model_name<<".txt";
myfile.open(ss.str().c_str());
for(size_t k =0 ;k<308;k++){
if(k != 307)
myfile << rec._vfh_features->points[0].histogram[k]<<",";
else
myfile << rec._vfh_features->points[0].histogram[k];
}
std::cout<<"wrote the histogram to file :" <<ss.str()<<std::endl;
myfile.close();
}else if(rv){
std::stringstream ss;
ss<<"rv_"<<model_name<<".txt";
myfile.open(ss.str().c_str());
for(int k = 0;k<181;k++){
if(k != 180)
myfile << rec._rv_features_181[k]<<",";
else
myfile << rec._rv_features_181[k];
}
std::cout<<"wrote the histogram to file: "<< ss.str()<<std::endl;
//myfile.open(ss.c_str());
}
}
