bool localizerGVG::loadLocalizerMap(localizer::LoadLocalizerMap::Request &req, localizer::LoadLocalizerMap::Response &res) {
mapLoadLocalization = false;
nL = req.nL;
X = VectorXd(3*nL+3);
P = MatrixXd::Zero(3*nL+3, 3*nL+3);
X_init = VectorXd(3*nL+3);
P_init = MatrixXd::Zero(3*nL+3, 3*nL+3);
for (int i = 0; i < req.ids.size(); i++) {
idList.push_back(req.ids.at(i));
}
for(int i = 0; i < req.state.size(); i++) {
X(i) = req.state.at(i);
X_init(i) = req.state.at(i);
}
std::vector<double>::const_iterator it = req.cov.begin();
int covSize = 3*nL + 3;
for (int i = 0; i < covSize; i++) {
for (int j = 0; j < covSize; j++) {
P(i,j) = *it;
P_init(i,j) = *it;
it++;
}
}
P.block(0,0,3,3) = MatrixXd::Zero(3,3);
P.block(0,3,3,3*nL) = MatrixXd::Zero(3, 3*nL);
P.block(3,0,3*nL,3) = MatrixXd::Zero(3*nL, 3);
return true;
}
void linear::chem_pot_force(void){
gradient(kind::CHEMPOT, kind::GRADCHEMPOT);
VectorXd al = field_to_vctr( kind::ALPHA );
VectorXd grad_cp_x = vfield_to_vctr( kind::GRADCHEMPOT , 0 );
VectorXd f_x = vfield_to_vctr( kind::FORCE , 0 );
f_x -= VectorXd( al.array() * grad_cp_x.array() ); // array: for el-wise *
vctr_to_vfield( f_x , kind::FORCE , 0 );
VectorXd f_y = vfield_to_vctr( kind::FORCE , 1 );
VectorXd grad_cp_y = vfield_to_vctr( kind::GRADCHEMPOT , 1 );
f_y -= VectorXd( al.array() * grad_cp_y.array() ); // array: for el-wise *
vctr_to_vfield( f_y , kind::FORCE , 1 );
return;
}
void ConstraintDynamics::addConstraint(Constraint *_constr) {
mConstraints.push_back(_constr);
mTotalRows += _constr->getNumRows();
for (int i = 0; i < mSkels.size(); i++) {
mJ[i].conservativeResize(mTotalRows, mSkels[i]->getNumDofs());
mJ[i].bottomRows(_constr->getNumRows()).setZero();
}
mC = VectorXd(mTotalRows);
mCDot = VectorXd(mTotalRows);
mGInv = MatrixXd::Zero(mTotalRows, mTotalRows);
mTauHat = VectorXd(mTotalRows);
}
void test_nullary()
{
CALL_SUBTEST_1( testMatrixType(Matrix2d()) );
CALL_SUBTEST_2( testMatrixType(MatrixXcf(50,50)) );
CALL_SUBTEST_3( testMatrixType(MatrixXf(5,7)) );
CALL_SUBTEST_4( testVectorType(VectorXd(51)) );
CALL_SUBTEST_5( testVectorType(VectorXd(41)) );
CALL_SUBTEST_6( testVectorType(Vector3d()) );
CALL_SUBTEST_7( testVectorType(VectorXf(51)) );
CALL_SUBTEST_8( testVectorType(VectorXf(41)) );
CALL_SUBTEST_9( testVectorType(Vector3f()) );
}
void test_nullary()
{
CALL_SUBTEST_1( testMatrixType(Matrix2d()) );
CALL_SUBTEST_2( testMatrixType(MatrixXcf(internal::random<int>(1,300),internal::random<int>(1,300))) );
CALL_SUBTEST_3( testMatrixType(MatrixXf(internal::random<int>(1,300),internal::random<int>(1,300))) );
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_4( testVectorType(VectorXd(internal::random<int>(1,300))) );
CALL_SUBTEST_5( testVectorType(VectorXd(internal::random<int>(1,300))) );
CALL_SUBTEST_6( testVectorType(Vector3d()) );
CALL_SUBTEST_7( testVectorType(VectorXf(internal::random<int>(1,300))) );
CALL_SUBTEST_8( testVectorType(VectorXf(internal::random<int>(1,300))) );
CALL_SUBTEST_9( testVectorType(Vector3f()) );
}
}
void CVarianceDecomposition::aestimateHeritability(VectorXd* out, const MatrixXd& Y, const MatrixXd& fixed, const MatrixXd& K)
{
/*
* estimates the genetic and the noise variance and creates a matrirx object to return them
*/
MatrixXd covs;
if(isnull(fixed))
covs = MatrixXd::Ones(Y.rows(),1);
else
covs = fixed;
//use mixed model code to estimate heritabiltiy
CLMM lmm;
lmm.setK(K);
lmm.setSNPs(MatrixXd::Zero(K.rows(),1));
lmm.setPheno(Y);
lmm.setCovs(covs);
lmm.setVarcompApprox0(-20, 20, 1000);
lmm.process();
mfloat_t delta0 = exp(lmm.getLdelta0()(0,0));
mfloat_t Vtotal = exp(lmm.getLSigma()(0,0));
VectorXd rv = VectorXd(2);
rv(0) = Vtotal;
rv(1) = Vtotal*delta0;
(*out) =rv;
}
void test_redux()
{
// the max size cannot be too large, otherwise reduxion operations obviously generate large errors.
int maxsize = (std::min)(100,EIGEN_TEST_MAX_SIZE);
EIGEN_UNUSED_VARIABLE(maxsize);
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( matrixRedux(Matrix<float, 1, 1>()) );
CALL_SUBTEST_1( matrixRedux(Array<float, 1, 1>()) );
CALL_SUBTEST_2( matrixRedux(Matrix2f()) );
CALL_SUBTEST_2( matrixRedux(Array2f()) );
CALL_SUBTEST_3( matrixRedux(Matrix4d()) );
CALL_SUBTEST_3( matrixRedux(Array4d()) );
CALL_SUBTEST_4( matrixRedux(MatrixXcf(internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_4( matrixRedux(ArrayXXcf(internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_5( matrixRedux(MatrixXd (internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_5( matrixRedux(ArrayXXd (internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_6( matrixRedux(MatrixXi (internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_6( matrixRedux(ArrayXXi (internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
}
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_7( vectorRedux(Vector4f()) );
CALL_SUBTEST_7( vectorRedux(Array4f()) );
CALL_SUBTEST_5( vectorRedux(VectorXd(internal::random<int>(1,maxsize))) );
CALL_SUBTEST_5( vectorRedux(ArrayXd(internal::random<int>(1,maxsize))) );
CALL_SUBTEST_8( vectorRedux(VectorXf(internal::random<int>(1,maxsize))) );
CALL_SUBTEST_8( vectorRedux(ArrayXf(internal::random<int>(1,maxsize))) );
}
}
void test_nullary()
{
CALL_SUBTEST_1( testMatrixType(Matrix2d()) );
CALL_SUBTEST_2( testMatrixType(MatrixXcf(internal::random<int>(1,300),internal::random<int>(1,300))) );
CALL_SUBTEST_3( testMatrixType(MatrixXf(internal::random<int>(1,300),internal::random<int>(1,300))) );
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_4( testVectorType(VectorXd(internal::random<int>(1,300))) );
CALL_SUBTEST_5( testVectorType(Vector4d()) ); // regression test for bug 232
CALL_SUBTEST_6( testVectorType(Vector3d()) );
CALL_SUBTEST_7( testVectorType(VectorXf(internal::random<int>(1,300))) );
CALL_SUBTEST_8( testVectorType(Vector3f()) );
CALL_SUBTEST_8( testVectorType(Vector4f()) );
CALL_SUBTEST_8( testVectorType(Matrix<float,8,1>()) );
CALL_SUBTEST_8( testVectorType(Matrix<float,1,1>()) );
CALL_SUBTEST_9( testVectorType(VectorXi(internal::random<int>(1,300))) );
CALL_SUBTEST_9( testVectorType(Matrix<int,1,1>()) );
}
#ifdef EIGEN_TEST_PART_6
// Assignment of a RowVectorXd to a MatrixXd (regression test for bug #79).
VERIFY( (MatrixXd(RowVectorXd::LinSpaced(3, 0, 1)) - RowVector3d(0, 0.5, 1)).norm() < std::numeric_limits<double>::epsilon() );
#endif
}
ACovarianceFunction::ACovarianceFunction(muint_t numberParams)
{
this->numberParams =numberParams;
this->params = VectorXd(numberParams);
//default: 0 dimensions
this->numberDimensions = 0;
}
void test_stdvector()
{
// some non vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Vector2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix3f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Matrix2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Vector4f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix4f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST_3(check_stdvector_matrix(VectorXd(20)));
CALL_SUBTEST_3(check_stdvector_matrix(RowVectorXf(20)));
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST_4(check_stdvector_transform(Projective2f()));
CALL_SUBTEST_4(check_stdvector_transform(Projective3f()));
CALL_SUBTEST_4(check_stdvector_transform(Projective3d()));
//CALL_SUBTEST(heck_stdvector_transform(Projective4d()));
// some Quaternion
CALL_SUBTEST_5(check_stdvector_quaternion(Quaternionf()));
CALL_SUBTEST_5(check_stdvector_quaternion(Quaterniond()));
}
void test_mapped_matrix()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( map_class_vector(Matrix<float, 1, 1>()) );
CALL_SUBTEST_1( check_const_correctness(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( map_class_vector(Vector4d()) );
CALL_SUBTEST_2( map_class_vector(VectorXd(13)) );
CALL_SUBTEST_2( check_const_correctness(Matrix4d()) );
CALL_SUBTEST_3( map_class_vector(RowVector4f()) );
CALL_SUBTEST_4( map_class_vector(VectorXcf(8)) );
CALL_SUBTEST_5( map_class_vector(VectorXi(12)) );
CALL_SUBTEST_5( check_const_correctness(VectorXi(12)) );
CALL_SUBTEST_1( map_class_matrix(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( map_class_matrix(Matrix4d()) );
CALL_SUBTEST_11( map_class_matrix(Matrix<float,3,5>()) );
CALL_SUBTEST_4( map_class_matrix(MatrixXcf(internal::random<int>(1,10),internal::random<int>(1,10))) );
CALL_SUBTEST_5( map_class_matrix(MatrixXi(internal::random<int>(1,10),internal::random<int>(1,10))) );
CALL_SUBTEST_6( map_static_methods(Matrix<double, 1, 1>()) );
CALL_SUBTEST_7( map_static_methods(Vector3f()) );
CALL_SUBTEST_8( map_static_methods(RowVector3d()) );
CALL_SUBTEST_9( map_static_methods(VectorXcd(8)) );
CALL_SUBTEST_10( map_static_methods(VectorXf(12)) );
CALL_SUBTEST_11( map_not_aligned_on_scalar<double>() );
}
}
void test_qtvector()
{
// some non vectorizable fixed sizes
CALL_SUBTEST(check_qtvector_matrix(Vector2f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix3f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST(check_qtvector_matrix(Matrix2f()));
CALL_SUBTEST(check_qtvector_matrix(Vector4f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix4f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST(check_qtvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST(check_qtvector_matrix(VectorXd(20)));
CALL_SUBTEST(check_qtvector_matrix(RowVectorXf(20)));
CALL_SUBTEST(check_qtvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST(check_qtvector_transform(Transform2f()));
CALL_SUBTEST(check_qtvector_transform(Transform3f()));
CALL_SUBTEST(check_qtvector_transform(Transform3d()));
//CALL_SUBTEST(check_qtvector_transform(Transform4d()));
// some Quaternion
CALL_SUBTEST(check_qtvector_quaternion(Quaternionf()));
CALL_SUBTEST(check_qtvector_quaternion(Quaternionf()));
}
/**
* Constructs the MetropolisHastingsSimulation object.
*
* @param pModel Pointer to the Model.
* @param pData Pointer to the Data.
*/
MetropolisHastingsSimulation::MetropolisHastingsSimulation(Model* pModel,
Data* pData) :
Simulation(pModel, pData), //
lScores(1, VectorXd(nStatistics())), //
lScoreTargets(VectorXd::Zero(nStatistics())), //
lNeedsDerivative(false), //
lDerivative(1, MatrixXd(nParameters(), nParameters())), //
lMeanScoresMinusTargets(), //
lNRunMH(nPeriods()), //
lResult(&lTheta, &lScoreTargets, 0/*ptr*/, &lScores, 0/*ptr*/,
&lMeanScoresMinusTargets, 0/*ptr*/, 0/*ptr*/, &lDerivative, 0/*ptr*/)
{
// Conditional is not possible with maximum likelihood
// R: sienaModelCreate.r sienaModelCreate() (line ~56)
assert(!pModel->conditional());
// Calculate the number of simulation steps per period.
// R: initializeFran.r initializeFRAN() (line ~504)
const MatrixXd targets = calculateTargetStatistics();
const ArrayXb basicRates = rStatisticEffects().array()
== &Simulation::RATE_EFFECT;
const VectorXd zero = VectorXd::Zero(nStatistics());
for (int m = 0; m < nPeriods(); ++m) {
lNRunMH[m] = N_RUN_MH_MULTIPLICATOR_DEFAULT
* basicRates.select(targets.row(m).transpose(), zero).sum();
if (lNRunMH[m] > 100) {
lNRunMH[m] = round((double) lNRunMH[m] / 100.0) * 100;
}
}
}
void test_stdvector_overload()
{
// some non vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Vector2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix3f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Matrix2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Vector4f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix4f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST_3(check_stdvector_matrix(VectorXd(20)));
CALL_SUBTEST_3(check_stdvector_matrix(RowVectorXf(20)));
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST_4(check_stdvector_transform(Affine2f())); // does not need the specialization (2+1)^2 = 9
CALL_SUBTEST_4(check_stdvector_transform(Affine3f()));
CALL_SUBTEST_4(check_stdvector_transform(Affine3d()));
// some Quaternion
CALL_SUBTEST_5(check_stdvector_quaternion(Quaternionf()));
CALL_SUBTEST_5(check_stdvector_quaternion(Quaterniond()));
}
/*
* Here we have another version of the method run.
* In contrast of the above one, here we run the algorithm with a fixed dimensions among all iterations.
* @param dimFixed vector of integer with length the number of cluster.
* Each component contains the number of dimensions of the corresponding cluster.
* @param nbIter The maximum number of iterations.
*/
void SimpleStrategy::run(VectorXi dimFixed, int nbIter){
// iter will count the number of iterations of the algorithm.
int iter=1;
//as its name indicates, logliklihood_prev will store the previous liklihood; ie logliklihood befor runnig alg
double logliklihood_prev=m_algo->getModel()->getLoglik();
//logliklihood_max stores the maximum liklihood among the iterations of the algorithm.
double logliklihood_max=m_algo->getModel()->getLoglik();
// m_logliktotal and m_Rtotal stores respectively loglik and dimensions among the iteration of the algorithm
m_loglikTotal=VectorXd(nbIter+1);
m_loglikTotal(0)=m_algo->getModel()->getLoglik();
m_Rtotal=MatrixXi(m_algo->getModel()->getNbClust(),nbIter+1);
m_Rtotal.col(0)=m_algo->getModel()->getParamobj().r;
//criterion stop: small increase in consecutive loglikelihood or max iterations
do {
// storing the loglik befor running algo.
logliklihood_prev=m_algo->getModel()->getLoglik();
/*
run algo. here we use the version of running algo with a fixed dimensions among all iterations.
*/
m_algo -> run(dimFixed);
//update clusters and chck if there is an empty class. here is the only place that the clusters membership is calculate
m_algo->getModel()->updateClusters();
//stores the new loglikelihood and the news dimensions
m_loglikTotal(iter)=m_algo->getModel()->getLoglik();
m_Rtotal.col(iter)=m_algo->getModel()->getParamobj().r;
//if we have increased the loglikelihood, this models is copied in bestModels
if (m_algo->getModel()->getLoglik() > logliklihood_max){
//update the logliklihood_max.
logliklihood_max=m_algo->getModel()->getLoglik();
//update clusters. here is the only place that the clusters membership is calculate
m_algo->getModel()->updateClusters();
// we stores the current model which make increasing the logliklihood in the bestModel.
*m_bestModel=*(m_algo->getModel());
}
iter+=1;
} while (fabs(m_algo->getModel()->getLoglik()-logliklihood_prev) > m_epsilon && iter <= nbIter
&& m_algo->getModel()->getEmpty()==false);
// we stores only the liklihood and dimension which have been calculated.
m_loglikTotal.conservativeResize(iter);
m_Rtotal.conservativeResize(m_algo->getModel()->getNbClust(),iter);
//we update the actual model with the best model
*(m_algo->getModel())=*m_bestModel;
//compute the new criterion
m_algo->getModel()->aic();
m_algo->getModel()->bic();
m_algo->getModel()->icl();
}
/**
* Calculates the score regression coefficient.
*
* @param rFullStatistics Matrix with all simulated statistics (rowwise).
* @param rFullScores Matrix with all simulated scores (rowwise).
* @return Score regression coefficient.
*/
Eigen::VectorXd DolbyModificator::regressionCoefficient( const
Eigen::MatrixXd& rFullStatistics, const Eigen::MatrixXd& rFullScores) {
assert(rFullStatistics.cols() == rFullScores.cols());
VectorXd reg = VectorXd(rFullStatistics.cols());
for (int i = 0; i < rFullStatistics.cols(); ++i) {
reg[i] = covariance(rFullStatistics.col(i), rFullScores.col(i)) / variance(rFullScores.col(i));
}
return reg;
}
State::State(trackingC *obj1, int id1, trackingC *obj2, int id2):
t_object1(obj1), s_object1(id1), t_object2(obj2), s_object2(id2)
{
marked = false;
strength = 0;
relation = -1;
relationsVector = VectorXd(4);
}
void test_stable_norm()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( stable_norm(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( stable_norm(Vector4d()) );
CALL_SUBTEST_3( stable_norm(VectorXd(internal::random<int>(10,2000))) );
CALL_SUBTEST_4( stable_norm(VectorXf(internal::random<int>(10,2000))) );
CALL_SUBTEST_5( stable_norm(VectorXcd(internal::random<int>(10,2000))) );
}
}
void test_vectorwiseop()
{
CALL_SUBTEST_1( vectorwiseop_array(Array22cd()) );
CALL_SUBTEST_2( vectorwiseop_array(Array<double, 3, 2>()) );
CALL_SUBTEST_3( vectorwiseop_array(ArrayXXf(3, 4)) );
CALL_SUBTEST_4( vectorwiseop_matrix(Matrix4cf()) );
CALL_SUBTEST_5( vectorwiseop_matrix(Matrix<float,4,5>()) );
CALL_SUBTEST_6( vectorwiseop_matrix(MatrixXd(internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
CALL_SUBTEST_7( vectorwiseop_matrix(VectorXd(internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
CALL_SUBTEST_7( vectorwiseop_matrix(RowVectorXd(internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
}
localizerGVG::localizerGVG(std::string& robot_name):
X(3),P(3,3) {
ROS_INFO("localizerGVG::localizerGVG ");
std::string output_frame;
std::string theMap_frame;
odomSub = nh.subscribe("/indoor/odom",1,&localizerGVG::handleOdom, this);
startService = nh.advertiseService("StartFilter" , &localizerGVG::processStart,this);
updateService = nh.advertiseService("UpdateFilter" , &localizerGVG::processMeetpoint,this);
maxUncService = nh.advertiseService("MaxUncertainty", &localizerGVG::processMaxUnc,this);
minUncService = nh.advertiseService("MinUncertainty", &localizerGVG::processMinUnc,this);
loadMapService = nh.advertiseService("LoadLocalizerMap", &localizerGVG::loadLocalizerMap, this);
initLoadMapTransService = nh.advertiseService("InitLoadMapTransform", &localizerGVG::initLoadMapTransform, this);
nh.param("output_frame", output_frame, std::string("odom_combined"));
posePub = nh.advertise<nav_msgs::Odometry>(output_frame, 10);
nh.param("theMap_frame", theMap_frame, std::string("theMap"));
mapPub = nh.advertise<localizer::GVGmap>(theMap_frame, 10);
nh.param("filterOn",this->filterOn);
initOdom=false;
mapLoadLocalization = true;
bearing_angle = 0;
nL=0; // number of landmarks
// EKF constants
X=VectorXd(3); // The state vector initialized to the pose of the robot.
X<< 0,0,0;
P=MatrixXd::Zero(3,3); // The covariance matrix
// The model noise covariance (linear and angular velocity) defined in localizer launch files
nh.getParam("/indoor/gvg/localizerGVGNode/Wvv", this->_Wvv);
nh.getParam("/indoor/gvg/localizerGVGNode/Www", this->_Www);
nh.getParam("/indoor/gvg/localizerGVGNode/Wvw", this->_Wvw);
Qr<< _Wvv, _Wvw,
_Wvw, _Www;
CF << 1,0,0,
0,1,0,
0,0,1;
J << 0,-1,
1, 0;
R<< 0.02527455743763, 0.000014869773007,0,
0.000014869773007, 0.02476827355463,0,
0,0,0.025;
}
ModelParameters* ModelParametersGMR::clone(void) const
{
std::vector<double> priors;
std::vector<VectorXd> means_x;
std::vector<VectorXd> means_y;
std::vector<MatrixXd> covars_x;
std::vector<MatrixXd> covars_y;
std::vector<MatrixXd> covars_y_x;
for (size_t i = 0; i < priors_.size(); i++)
{
priors.push_back(priors_[i]);
means_x.push_back(VectorXd(means_x_[i]));
means_y.push_back(VectorXd(means_x_[i]));
covars_x.push_back(MatrixXd(means_x_[i]));
covars_y.push_back(MatrixXd(means_y_[i]));
covars_y_x.push_back(MatrixXd(covars_y_x_[i]));
}
return new ModelParametersGMR(priors, means_x, means_y, covars_x, covars_y, covars_y_x);
}
void DataSet::findFeatRange() {
m_minFeatRange = VectorXd(m_numFeatures);
m_maxFeatRange = VectorXd(m_numFeatures);
double minVal, maxVal;
for (int nFeat = 0; nFeat < m_numFeatures; nFeat++) {
minVal = m_samples[0].x(nFeat);
maxVal = m_samples[0].x(nFeat);
for (int nSamp = 1; nSamp < m_numSamples; nSamp++) {
if (m_samples[nSamp].x(nFeat) < minVal) {
minVal = m_samples[nSamp].x(nFeat);
}
if (m_samples[nSamp].x(nFeat) > maxVal) {
maxVal = m_samples[nSamp].x(nFeat);
}
}
m_minFeatRange(nFeat) = minVal;
m_maxFeatRange(nFeat) = maxVal;
}
}
void test_nullary()
{
CALL_SUBTEST_1( testMatrixType(Matrix2d()) );
CALL_SUBTEST_2( testMatrixType(MatrixXcf(internal::random<int>(1,300),internal::random<int>(1,300))) );
CALL_SUBTEST_3( testMatrixType(MatrixXf(internal::random<int>(1,300),internal::random<int>(1,300))) );
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_4( testVectorType(VectorXd(internal::random<int>(1,300))) );
CALL_SUBTEST_5( testVectorType(Vector4d()) ); // regression test for bug 232
CALL_SUBTEST_6( testVectorType(Vector3d()) );
CALL_SUBTEST_7( testVectorType(VectorXf(internal::random<int>(1,300))) );
CALL_SUBTEST_8( testVectorType(Vector3f()) );
CALL_SUBTEST_8( testVectorType(Matrix<float,1,1>()) );
}
}
void DataSet::load(const string& x_filename, const string& y_filename) {
ifstream xfp(x_filename.c_str(), ios::binary);
if (!xfp) {
cout << "Could not open input file " << x_filename << endl;
exit(EXIT_FAILURE);
}
ifstream yfp(y_filename.c_str(), ios::binary);
if (!yfp) {
cout << "Could not open input file " << y_filename << endl;
exit(EXIT_FAILURE);
}
cout << "Loading data file: " << x_filename << " ... " << endl;
// Reading the header
int tmp;
xfp >> m_numSamples;
xfp >> m_numFeatures;
yfp >> tmp;
if (tmp != m_numSamples) {
cout << "Number of samples in data and labels file is different" << endl;
exit(EXIT_FAILURE);
}
yfp >> tmp;
m_samples.clear();
set<int> labels;
for (int nSamp = 0; nSamp < m_numSamples; nSamp++) {
Sample sample;
sample.x = VectorXd(m_numFeatures);
sample.id = nSamp;
sample.w = 1.0;
yfp >> sample.y;
labels.insert(sample.y);
for (int nFeat = 0; nFeat < m_numFeatures; nFeat++) {
xfp >> sample.x(nFeat);
}
m_samples.push_back(sample); // push sample into dataset
}
xfp.close();
yfp.close();
m_numClasses = labels.size();
// Find the data range
findFeatRange();
cout << "Loaded " << m_numSamples << " samples with " << m_numFeatures;
cout << " features and " << m_numClasses << " classes." << endl;
}
void test_eigen2_sum()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( matrixSum(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( matrixSum(Matrix2f()) );
CALL_SUBTEST_3( matrixSum(Matrix4d()) );
CALL_SUBTEST_4( matrixSum(MatrixXcf(3, 3)) );
CALL_SUBTEST_5( matrixSum(MatrixXf(8, 12)) );
CALL_SUBTEST_6( matrixSum(MatrixXi(8, 12)) );
}
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_5( vectorSum(VectorXf(5)) );
CALL_SUBTEST_7( vectorSum(VectorXd(10)) );
CALL_SUBTEST_5( vectorSum(VectorXf(33)) );
}
}
void test_eigen2_visitor()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( matrixVisitor(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( matrixVisitor(Matrix2f()) );
CALL_SUBTEST_3( matrixVisitor(Matrix4d()) );
CALL_SUBTEST_4( matrixVisitor(MatrixXd(8, 12)) );
CALL_SUBTEST_5( matrixVisitor(Matrix<double,Dynamic,Dynamic,RowMajor>(20, 20)) );
CALL_SUBTEST_6( matrixVisitor(MatrixXi(8, 12)) );
}
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_7( vectorVisitor(Vector4f()) );
CALL_SUBTEST_4( vectorVisitor(VectorXd(10)) );
CALL_SUBTEST_4( vectorVisitor(RowVectorXd(10)) );
CALL_SUBTEST_8( vectorVisitor(VectorXf(33)) );
}
}
bool DynamicMovementPrimitive::setup(const VectorXd &goal, const double movementDuration, const double samplingFrequency)
{
VectorXd start = VectorXd(params_.numTransformationSystems_);
for (int i = 0; i < params_.numTransformationSystems_; i++)
{
start(i) = transformationSystems_[i].initialY0_;
}
if (!setup(start, goal, movementDuration, samplingFrequency))
{
params_.isSetup_ = false;
return params_.isSetup_;
}
//start has not been specified, need to be set before the DMP can be propagated
params_.isStartSet_ = false;
params_.isSetup_ = true;
return params_.isSetup_;
}
Manipulator::Manipulator(const string &name, const string &path)
{
this->kukaArm = new YouBotManipulator(name, path);
/* Enable control and initialise the arm */
this->kukaArm->doJointCommutation();
this->kukaArm->calibrateManipulator();
/* The arm has 5 angles and no desired position */
this->latestDesiredPosition = VectorXd(ARMJOINTS);
/* Drive arm to home position, to get definied value */
this->setPose(HOME_POSITION);
/* Create a new kinematics solver */
this->solver = new KinematicsSolver();
JointVelocitySetpoint data;
data.angularVelocity = 0.001 * radian_per_second;
this->kukaArm->getArmJoint(1).setData(data);
}
H2_2D_Tree::H2_2D_Tree(const unsigned short nChebNodes, double* const charge, const vector<Point>& location, const unsigned long N, const unsigned m, bool print){
this->nChebNodes = nChebNodes;
this->rank = nChebNodes*nChebNodes;
this->N = N;
this->m = m;
this->maxLevels = 0;
this->chargeTree = Map<MatrixXd>(charge, N, m);// charge should be N..N..N...
this->locationTree = location;
this->print = print;
// Get Chebyshev nodes
cNode = VectorXd(nChebNodes);
get_Standard_Chebyshev_Nodes(nChebNodes,cNode);
// Get Chebyshev polynomials evaluated at Chebyshev nodes
TNode = MatrixXd::Zero(nChebNodes,nChebNodes);
get_Standard_Chebyshev_Polynomials(nChebNodes,nChebNodes,cNode,TNode);
// Gets transfer matrices
get_Transfer(nChebNodes,cNode,TNode,R);
nLevels = 0;
get_Center_Radius(location, center, radius);
// Create root
root = new H2_2D_Node(0, 0);
root->nNeighbor = 0;
root->nInteraction = 0;
root->N = N;
root->center = center;
root->radius = radius;
root->index.setLinSpaced(N,0,N-1);
if(print)
std::cout << "Assigning children..." << std::endl;
assign_Children(root);
if(print)
std::cout << "Assigned children." << std::endl;
build_Tree(root);
if(print)
std::cout << "Maximum levels is: " << this->maxLevels << std::endl;
}
void test_matrix_power()
{
typedef Matrix<double,3,3,RowMajor> Matrix3dRowMajor;
typedef Matrix<long double,Dynamic,Dynamic> MatrixXe;
typedef Matrix<long double,Dynamic,1> VectorXe;
CALL_SUBTEST_2(test2dRotation<double>(1e-13));
CALL_SUBTEST_1(test2dRotation<float>(2e-5)); // was 1e-5, relaxed for clang 2.8 / linux / x86-64
CALL_SUBTEST_9(test2dRotation<long double>(1e-13));
CALL_SUBTEST_2(test2dHyperbolicRotation<double>(1e-14));
CALL_SUBTEST_1(test2dHyperbolicRotation<float>(1e-5));
CALL_SUBTEST_9(test2dHyperbolicRotation<long double>(1e-14));
CALL_SUBTEST_2(testMatrixVector(Matrix2d(), Vector2d(), 1e-13));
CALL_SUBTEST_7(testMatrixVector(Matrix3dRowMajor(), MatrixXd(3,5), 1e-13));
CALL_SUBTEST_3(testMatrixVector(Matrix4cd(), Vector4cd(), 1e-13));
CALL_SUBTEST_4(testMatrixVector(MatrixXd(8,8), VectorXd(8), 2e-12));
CALL_SUBTEST_1(testMatrixVector(Matrix2f(), Vector2f(), 1e-4));
CALL_SUBTEST_5(testMatrixVector(Matrix3cf(), Vector3cf(), 1e-4));
CALL_SUBTEST_8(testMatrixVector(Matrix4f(), Vector4f(), 1e-4));
CALL_SUBTEST_6(testMatrixVector(MatrixXf(8,8), VectorXf(8), 1e-3));
CALL_SUBTEST_9(testMatrixVector(MatrixXe(7,7), VectorXe(7), 1e-13));
}
