void ALLModel::calculateXY(Eigen::VectorXd& w, Eigen::MatrixXd& res)
{
res.resize(descriptor_matrix_.cols(), Y_.cols());
res.setZero();
for (int i = 0; i < descriptor_matrix_.cols(); i++)
{
for (int j = 0; j < Y_.cols(); j++)
{
for (int s = 0; s < descriptor_matrix_.rows(); s++)
{
res(i, j) += w(s)*descriptor_matrix_(s, i)*Y_(s, j);
}
}
}
}
void LDAModel::train()
{
if (descriptor_matrix_.cols() == 0)
{
throw Exception::InconsistentUsage(__FILE__, __LINE__, "Data must be read into the model before training!");
}
readLabels();
// map values of Y to their index
map<int, unsigned int> label_to_pos;
for (unsigned int i = 0; i < labels_.size(); i++)
{
label_to_pos.insert(make_pair(labels_[i], i));
}
// calculate sigma_ = covariance matrix of descriptors
sigma_.resize(descriptor_matrix_.cols(), descriptor_matrix_.cols());
for (int i = 0; i < descriptor_matrix_.cols(); i++)
{
double mi = Statistics::getMean(descriptor_matrix_, i);
for (int j = 0; j < descriptor_matrix_.cols(); j++)
{
sigma_(i, j) = Statistics::getCovariance(descriptor_matrix_, i, j, mi);
}
}
Eigen::MatrixXd I(sigma_.cols(), sigma_.cols()); I.setIdentity();
I *= lambda_;
sigma_ += I;
sigma_ = sigma_.inverse();
mean_vectors_.resize(Y_.cols());
no_substances_.clear();
no_substances_.resize(labels_.size(), 0);
for (int c = 0; c < Y_.cols(); c++)
{
vector < int > no_substances_c(labels_.size(), 0); // no of substances in each class for activitiy c
mean_vectors_[c].resize(labels_.size(), descriptor_matrix_.cols());
mean_vectors_[c].setZero();
for (int i = 0; i < descriptor_matrix_.rows(); i++) // calculate sum vector of each class
{
int yi = static_cast < int > (Y_(i, c)); // Y_ will contains only ints for classifications
int pos = label_to_pos.find(yi)->second;
for (int j = 0; j < descriptor_matrix_.cols(); j++)
{
mean_vectors_[c](pos, j) += descriptor_matrix_(i, j);
}
if (c == 0) no_substances_c[pos]++;
}
for (int i = 0; i < mean_vectors_[c].rows(); i++) // calculate mean vector of each class
{
if (no_substances_c[i] == 0)
{
mean_vectors_[c].row(i).setConstant(std::numeric_limits<double>::infinity()); // set mean of classes NOT occurring for activitiy c to inf
}
for (int j = 0; j < descriptor_matrix_.cols(); j++)
{
mean_vectors_[c](i, j) = mean_vectors_[c](i, j)/no_substances_c[i];
}
}
for (unsigned int i = 0; i < no_substances_.size(); i++) // update overall number of substances per class
{
no_substances_[i] += no_substances_c[i];
}
}
}
geometry_msgs::Twist generate_smooth_trajectory(geometry_msgs::Twist p_ini, geometry_msgs::Twist p_fin, ros::Publisher pub_pos, double tiempo, bool respecto_robot, geometry_msgs::Twist vel_ant){
//Encontrar las posiciones de la trayectoria con ayuda de un polinomio de quinto orden
double p_ini_x_;
double p_ini_y_;
double a_ini_z_;
double p_fin_x_;
double p_fin_y_;
double a_fin_z_;
double r_time_;
p_ini_x_ = p_ini.linear.x;//10;
p_ini_y_ = p_ini.linear.y;//10;
a_ini_z_ = p_ini.angular.z;//10;
p_fin_x_ = p_fin.linear.x;///10;
p_fin_y_ = p_fin.linear.y;//10;
a_fin_z_ = p_fin.angular.z;//10;
r_time_ = p_fin.angular.x;
Eigen::MatrixXf Mat(6,6);
Mat <<0,0,0,0,0,1,
pow(r_time_,5),pow(r_time_,4),pow(r_time_,3),pow(r_time_,2),r_time_,1,
0,0,0,0,1,0,
5*pow(r_time_,4),4*pow(r_time_,3),3*pow(r_time_,2),2*r_time_,1,0,
0,0,0,2,0,0,
20*pow(r_time_,3),12*pow(r_time_,2),6*r_time_,2,0,0;
//if (!respecto_robot)
// std::cout << "Matriz de polinomio quinto orden" <<Mat << std::endl<< r_time_<<std::endl<<tiempo<<std::endl;
Eigen::MatrixXf X_(6,1);
X_ <<p_ini_x_,p_fin_x_,vel_ant.linear.x,0,0,0;
Eigen::MatrixXf Y_(6,1);
Y_ <<p_ini_y_,p_fin_y_,vel_ant.linear.y,0,0,0;
Eigen::MatrixXf Z_(6,1);
Z_ <<a_ini_z_,a_fin_z_,vel_ant.angular.z,0,0,0;
//std::cout << "Vector X" <<X_ << std::endl;
Eigen::MatrixXf coeff_X(6,1);
coeff_X = Mat.colPivHouseholderQr().solve(X_);
//std::cout << "Vector X" <<coeff_X << std::endl;
Eigen::MatrixXf coeff_Y(6,1);
coeff_Y = Mat.colPivHouseholderQr().solve(Y_);
Eigen::MatrixXf coeff_Z(6,1);
coeff_Z = Mat.colPivHouseholderQr().solve(Z_);
double i =.02;// 1/rate_hz;
geometry_msgs::Twist pos_aux,vel_aux;
//pos_aux.linear.x =coeff_X(0,0)*pow(i,5)+coeff_X(1,0)*pow(i,4)+coeff_X(2,0)*pow(i,3)+coeff_X(3,0)*pow(i,2)+coeff_X(4,0)*i+coeff_X(5,0);
pos_aux.linear.x =coeff_X(0,0)*pow(i,5)+
coeff_X(1,0)*pow(i,4)+
coeff_X(2,0)*pow(i,3)+
coeff_X(3,0)*pow(i,2)+
coeff_X(4,0)*i+
coeff_X(5,0);
pos_aux.linear.y =coeff_Y(0,0)*pow(i,5)+coeff_Y(1,0)*pow(i,4)+coeff_Y(2,0)*pow(i,3)+coeff_Y(3,0)*pow(i,2)+coeff_Y(4,0)*i+coeff_Y(5,0);
pos_aux.angular.z=coeff_Z(0,0)*pow(i,5)+coeff_Z(1,0)*pow(i,4)+coeff_Z(2,0)*pow(i,3)+coeff_Z(3,0)*pow(i,2)+coeff_Z(4,0)*i+coeff_Z(5,0);
/*vel_aux.linear.x = 5*coeff_X(0,0)*pow(i,4)
+4*coeff_X(1,0)*pow(i,3)
+3*coeff_X(2,0)*pow(i,2)
+2*coeff_X(3,0)*i
+coeff_X(4,0);
vel_aux.linear.y = 5*coeff_Y(0,0)*pow(i,4)
+4*coeff_Y(1,0)*pow(i,3)
+3*coeff_Y(2,0)*pow(i,2)
+2*coeff_Y(3,0)*i
+coeff_Y(4,0);
vel_aux.angular.z =5*coeff_Z(0,0)*pow(i,4)
+4*coeff_Z(1,0)*pow(i,3)
+3*coeff_Z(2,0)*pow(i,2)
+2*coeff_Z(3,0)*i
+coeff_Z(4,0);*/
//vel_aux.angular.z = 0;
// Opcion 1
vel_aux.linear.x = (p_fin_x_-p_ini_x_)/(r_time_-tiempo);
vel_aux.linear.y = (p_fin_y_-p_ini_y_)/(r_time_-tiempo);
//vel_aux.angular.z = (a_fin_z_-a_ini_z_)/(r_time_-tiempo)*10;
//vel_aux.linear.x = (p_fin_x_-p_ini_x_)/(r_time_);
//vel_aux.linear.y = (p_fin_y_-p_ini_y_)/(r_time_);
vel_aux.angular.z = 0;
//Opcion 2
/*vel_aux.linear.x = abs(pos_aux.linear.x-p_fin_x_)/(1/(rate_hz));
vel_aux.linear.y = abs(pos_aux.linear.y-p_fin_y_)/(1/(rate_hz));
vel_aux.angular.z = 0;*/
if (respecto_robot){//Publicar la posicion respecto al mundo para tener una idea con el
pos_aux = destransformacion_homogenea(pos_aux, goalie_position.angular.z, goalie_position);
pub_pos.publish(pos_aux);
}
/* ROS_INFO_STREAM("pos esperada sin transf:"
<<" linear X ="<<pos_aux.linear.x
<<" linear Y ="<<pos_aux.linear.y
<<" angular ="<<pos_aux.angular.z);*/
/*}
else
{ Esto es respecto al mundo, no sirve mas que para saber que si se esta haciendo bien la transformacion lineal
pos_aux = destransformacion_homogenea(pos_aux, goalie_position.angular.z, goalie_position);
pub_pos.publish(pos_aux);
ROS_INFO_STREAM("pos final esp:"
<<" linear X ="<<p_fin_x_
<<" linear Y ="<<p_fin_y_
<<" angular ="<<a_fin_z_);
ROS_INFO_STREAM("pos inicial esp:"
<<" linear X ="<<p_ini_x_
<<" linear Y ="<<p_ini_y_
<<" angular ="<<a_ini_z_);
}*/
return vel_aux;
}
void FitModel::train()
{
if (descriptor_matrix_.cols() == 0)
{
throw Exception::InconsistentUsage(__FILE__, __LINE__, "Data must be read into the model before training!");
}
if (allEquations_.empty())
{
cout<<"ERROR: No equations specified! Use method setEquations first."<<endl;
return;
}
training_result_.resize(descriptor_matrix_.cols(), Y_.cols());
for (c = 0; c < (unsigned int)Y_.cols(); c++)
{
fitY = new Eigen::MatrixXd(Y_.rows(), 1);
for (int n = 0; n < Y_.rows(); n++)
{
(*fitY)(n, 0) = Y_(n, c);
}
fitX = &descriptor_matrix_;
equation = &allEquations_[c];
if (allDiffEquations_.size() < c)
{
diffEquations = &allDiffEquations_[c];
}
else
{
diffEquations = NULL;
}
const gsl_multifit_fdfsolver_type* T = gsl_multifit_fdfsolver_lmsder;
gsl_multifit_fdfsolver* s = gsl_multifit_fdfsolver_alloc(T, fitX->rows(), fitX->cols());
const size_t n = descriptor_matrix_.rows();
const size_t p = descriptor_matrix_.cols();
gsl_multifit_function_fdf fdf;
fdf = make_fdf(&setF, &setDf, &setFdf, n, p, 0);
double* g = new double[initial_guess_.size()];
for (unsigned int m = 0; m < initial_guess_.size(); m++)
{
g[m] = initial_guess_[m];
}
gsl_vector_view ini = gsl_vector_view_array (g, p);
gsl_multifit_fdfsolver_set(s, &fdf, &ini.vector);
int status;
for (unsigned int i = 0; i < 50; i++)
{
status = gsl_multifit_fdfsolver_iterate(s);
}
// save the predicted coefficients
for (unsigned int m = 0; m < s->x->size; m++)
{
training_result_(m, c) = gsl_vector_get(s->x, m);
}
delete fitY;
delete [] g;
gsl_multifit_fdfsolver_free(s);
}
cout <<training_result_<<endl;
}
