void CybGaussianNaiveBayes::initData()
{
for(int i=0; i < this->getVariablesNumber();i++) //X, Y, Z
{
float mean = 0; //mean variable
for(int j=0; j < this->getData()->size(); j++) //from 0 to size do
mean += this->getData()->pos(j)->operator[](i); //add value to mean
setMean(mean/this->getData()->size(), i); //divide mean by its size
}
for(int i=0; i < this->getVariablesNumber();i++) //X, Y, Z
{
float variance = 0; //variance variable
for(int j=0; j<this->getData()->size(); j++) //from 0 to size do
{
//variancia = variancia + pow((float_aux[j] - media), 2);
variance += pow((this->getData()->pos(j)->operator[](i) - this->getMean(i)), 2); //add to variance
}
setVariance(variance/this->getData()->size(), i); //divide variance by its size
}
}
void Separatrices::redefine(const QList< Number >& rol, const QList< StatClass >& statisticClasses,
const uint modalClassIndex, const Number& h)
{
clear();
setAverageBySum(rol);
setAverageByHalfPoint(statisticClasses,rol.size());
setMedian(statisticClasses,modalClassIndex,rol.size(),h);
setMode(statisticClasses,modalClassIndex,h);
setVariance(rol);
m_rol = &rol;
m_statClasses = &statisticClasses;
quartil(2);
decil(2);
percentis(63);
}
void
PersonTracker::computeStateLoop(const ros::TimerEvent& event) {
ROS_DEBUG("[person tracker] Last callback took %f seconds", event.profile.last_duration.toSec());
// Set variance based on time since last acquisition
double dt = timeSinceLastDetect().toSec();
double current_var = std::min(var_increase_rate_*dt, max_var_);
setVariance(person_pos_, current_var);
// person_pos_ is in the frame "map" which is constant. Lie and say the pose was acquired right now
person_pos_.header.stamp = ros::Time::now();
// Low-pass filter on the person position
PoseWithCovarianceStamped pub_pos;
pub_pos.header = person_pos_.header;
pub_pos.pose.covariance = person_pos_.pose.covariance;
pub_pos.pose.pose.orientation = person_pos_.pose.pose.orientation;
pub_pos.pose.pose.position.x = alpha_ *person_pos_ .pose.pose.position.x
+ (1.0-alpha_)*last_pub_pos_.pose.pose.position.x;
pub_pos.pose.pose.position.y = alpha_ *person_pos_ .pose.pose.position.y
+ (1.0-alpha_)*last_pub_pos_.pose.pose.position.y;
pub_pos.pose.pose.position.z = alpha_ *person_pos_ .pose.pose.position.z
+ (1.0-alpha_)*last_pub_pos_.pose.pose.position.z;
// Publish the goal and a stripped-down version without the covariance
goal_cov_pub_.publish( pub_pos );
goal_pub_ .publish( stripCovariance(pub_pos) );
last_pub_pos_ = pub_pos;
ROS_DEBUG_THROTTLE(2, "[person_tracker] Time since detect = %.2fs, Variance = %.2fm^2", dt, current_var);
// Update the sound player
if( hasPerson() ) {
sound_player_.setState(PTSounds::state_tracking);
}
else {
sound_player_.setState(PTSounds::state_searching);
}
sound_player_.update();
ros::spinOnce();
}
void
PersonTracker::skeletonCB(const body_msgs::Skeletons& skel_msg)
{
//ROS_INFO_THROTTLE(5,"[person tracker] Got data, %lu skeletons.", skel_msg.skeletons.size());
body_msgs::SkeletonJoint body_part;
string body_part_name;
std::vector<double> thresholds;
thresholds.push_back(0.7);
//thresholds.push_back(0.5);
//thresholds.push_back(0.3);
//thresholds.push_back(-0.1);
for( int ithresh = 0; ithresh<thresholds.size(); ithresh++ )
{
// Find the first trackable person
for( unsigned int i=0; i<skel_msg.skeletons.size(); i++ )
{
// If the person has a trackable joint, save the location
if( getFirstGoodJoint(skel_msg.skeletons.at(i), thresholds.at(ithresh), body_part, body_part_name) )
{
ROS_INFO_THROTTLE(2,"[person tracker] Player %d's %s has confidence %f", skel_msg.skeletons.at(i).playerid, body_part_name.c_str(), body_part.confidence);
// Create a new pose
PoseWithCovarianceStamped temp_pose;
temp_pose.pose.pose.position = body_part.position;
temp_pose.pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
temp_pose.header.frame_id = skel_msg.header.frame_id;
temp_pose.header.stamp = skel_msg.header.stamp;
setVariance(temp_pose, 0.0);
// Transform the goal to a global, fixed frame.
PoseWithCovarianceStamped transformed_pose;
if( poseToGlobalFrame(temp_pose, transformed_pose) )
{
person_pos_ = transformed_pose;
last_detect_ = ros::Time::now();
break;
}
}
}
}
}
// WARNING LOGO LOGO MUDAR OS PARAMETROS JA QUE ESTAMOS GUARDANDO A REFERENCIA EM m_rol E m_statClasses
Separatrices::Separatrices(const QList<Number>& rol, const QList< StatClass >& statisticClasses,
const uint modalClassIndex, const Number& h,QObject* parent): QObject(parent), m_averageBySum(0)
, m_averageByHalfPoint(0)
, m_mode(0)
, m_median(0)
, m_quartil(0)
, m_decil(0)
, m_percentil(0)
, m_variance(0)
, m_width(&h)
, m_rol(&rol)
, m_statClasses(&statisticClasses)
{
setAverageBySum(rol);
setAverageByHalfPoint(statisticClasses, rol.size());
setMedian(statisticClasses,modalClassIndex,rol.size(), h);
setMode(statisticClasses, modalClassIndex,h);
setVariance(rol);
quartil(2);
decil(2);
percentis(50);
}
Link::Link(int from,
int to,
Type type,
const Transform & transform,
double rotVariance,
double transVariance,
const cv::Mat & userData) :
from_(from),
to_(to),
transform_(transform),
type_(rtabmap::Link::kLandmark)
{
setVariance(rotVariance, transVariance);
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
NormalDistribution<1>::NormalDistribution(const std::tuple<Mean, Variance>&
parameters) :
mMean(std::get<0>(parameters)) {
setVariance(std::get<1>(parameters));
}
NormalDistribution<1>::NormalDistribution(Mean mean, Variance variance) :
mMean(mean) {
setVariance(variance);
}
void LinearModel::fitLM()
{
if (par::verbose)
{
for (int i=0; i<nind; i++)
{
cout << "VO " << i << "\t"
<< Y[i] << "\t";
for (int j=0; j<np; j++)
cout << X[i][j] << "\t";
cout << "\n";
}
}
// cout << "LM VIEW\n";
// display(Y);
// display(X);
// cout << "---\n";
coef.resize(np);
sizeMatrix(S,np,np);
if ( np==0 || nind==0 || ! all_valid )
{
return;
}
setVariance();
if ( par::standard_beta )
standardise();
sig.resize(nind, sqrt(1.0/sqrt((double)nind)) );
w.resize(np);
sizeMatrix(u,nind,np);
sizeMatrix(v,np,np);
// Perform "svdfit(C,Y,sig,b,u,v,w,chisq,function)"
int i,j;
const double TOL=1.0e-13;
double wmax,tmp,thresh,sum;
vector_t b(nind),afunc(np);
for (i=0;i<nind;i++) {
afunc = X[i];
tmp=1.0/sig[i];
for (j=0;j<np;j++)
u[i][j]=afunc[j]*tmp;
b[i]=Y[i]*tmp;
}
bool flag = svdcmp(u,w,v);
if ( ! flag )
{
all_valid = false;
return;
}
wmax=0.0;
for (j=0;j<np;j++)
if (w[j] > wmax) wmax=w[j];
thresh=TOL*wmax;
for (j=0;j<np;j++)
if (w[j] < thresh) w[j]=0.0;
svbksb(u,w,v,b,coef);
chisq=0.0;
for (i=0;i<nind;i++) {
afunc=X[i];
sum=0.0;
for (j=0;j<np;j++) sum += coef[j]*afunc[j];
chisq += (tmp=(Y[i]-sum)/sig[i],tmp*tmp);
}
/////////////////////////////////////////
// Obtain covariance matrix of estimates
// Robust cluster variance estimator
// V_cluster = (X'X)^-1 * \sum_{j=1}^{n_C} u_{j}' * u_j * (X'X)^-1
// where u_j = \sum_j cluster e_i * x_i
// Above, e_i is the residual for the ith observation and x_i is a
// row vector of predictors including the constant.
// For simplicity, I omitted the multipliers (which are close to 1)
// from the formulas for Vrob and Vclusters.
// The formula for the clustered estimator is simply that of the
// robust (unclustered) estimator with the individual ei*s replaced
// by their sums over each cluster.
// http://www.stata.com/support/faqs/stat/cluster.html
// SEE http://aje.oxfordjournals.org/cgi/content/full/kwm223v1#APP1
// Williams, R. L. 2000. A note on robust variance estimation for
// cluster-correlated data. Biometrics 56: 64
// t ( y - yhat X ) %*% ( y - yhat) / nind - np
// = variance of residuals
// j <- ( t( y- m %*% t(b) ) %*% ( y - m %*% t(b) ) ) / ( N - p )
// print( sqrt(kronecker( solve( t(m) %*% m ) , j ) ))
////////////////////////////////////////////////
// OLS variance estimator = s^2 * ( X'X )^-1
// where s^2 = (1/(N-k)) \sum_i=1^N e_i^2
// 1. Calcuate S = (X'X)^-1
matrix_t Xt;
sizeMatrix(Xt, np, nind);
for (int i=0; i<nind; i++)
for (int j=0; j<np; j++)
Xt[j][i] = X[i][j];
matrix_t S0;
multMatrix(Xt,X,S0);
flag = true;
S0 = svd_inverse(S0,flag);
if ( ! flag )
{
all_valid = false;
return;
}
if (par::verbose)
{
cout << "beta...\n";
display(coef);
cout << "Sigma(S0b)\n";
display(S0);
cout << "\n";
}
////////////////////////
// Calculate s^2 (sigma)
if (!cluster)
{
double sigma= 0.0;
for (int i=0; i<nind; i++)
{
double partial = 0.0;
for (int j=0; j<np; j++)
partial += coef[j] * X[i][j];
partial -= Y[i];
sigma += partial * partial;
}
sigma /= nind-np;
for (int i=0; i<np; i++)
for (int j=0; j<np; j++)
S[i][j] = S0[i][j] * sigma;
}
///////////////////////////
// Robust-cluster variance
if (cluster)
{
vector<vector_t> sc(nc);
for (int i=0; i<nc; i++)
sc[i].resize(np,0);
for (int i=0; i<nind; i++)
{
double partial = 0.0;
for (int j=0; j<np; j++)
partial += coef[j] * X[i][j];
partial -= Y[i];
for (int j=0; j<np; j++)
sc[clst[i]][j] += partial * X[i][j];
}
matrix_t meat;
sizeMatrix(meat, np, np);
for (int k=0; k<nc; k++)
{
for (int i=0; i<np; i++)
for (int j=0; j<np; j++)
meat[i][j] += sc[k][i] * sc[k][j];
}
matrix_t tmp1;
multMatrix( S0 , meat, tmp1);
multMatrix( tmp1 , S0, S);
}
if (par::verbose)
{
cout << "coefficients:\n";
display(coef);
cout << "var-cov matrix:\n";
display(S);
cout << "\n";
}
}
void Normal<T>::setPrecision(T p)
{
setVariance(1.0/p);
}
