void LiftingLine::solve_for_best_gamma(double cL)
{
int matsize = this->segments.size() + 1;
Eigen::MatrixXd matrix;
Eigen::VectorXd rhs;
Eigen::VectorXd result;
matrix.resize(matsize, matsize);
matrix.setZero();
rhs.resize(matsize);
rhs.setZero();
result.resize(matsize);
result.setZero();
// adding the main min-function
for (int i = 0; i < (matsize - 1); i++)
{
for (int j = 0; j < (matsize - 1); j++)
{
matrix(i, j) += this->segments[i].b() * this->segments[j].ind_influence(this->segments[i]);
matrix(i, j) += this->segments[j].b() * this->segments[i].ind_influence(this->segments[j]);
}
// adding lagrange multiplicator
matrix(i, matsize - 1) += this->segments[i].lift_factor;
}
for (int i = 0; i < (matsize -1); i++)
{
matrix(matsize - 1, i) += this->segments[i].lift_factor;
}
rhs(matsize - 1) += cL;
result = matrix.fullPivHouseholderQr().solve(rhs);
for (int i = 0; i < matsize - 1; i++)
{
this->segments[i].best_gamma = result[i];
}
}
segment_info(unsigned int nc) {
E.resize(6, nc);
E_tilde.resize(6, nc);
G.resize(nc);
M.resize(nc, nc);
EZ.resize(nc);
E.setZero();
E_tilde.setZero();
M.setZero();
G.setZero();
EZ.setZero();
};
segment_info(unsigned int nc):
D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0)
{
E.resize(6, nc);
E_tilde.resize(6, nc);
G.resize(nc);
M.resize(nc, nc);
EZ.resize(nc);
E.setZero();
E_tilde.setZero();
M.setZero();
G.setZero();
EZ.setZero();
};
//==============================================================================
void Spline::evaluateDerivative(
double _t, int _derivative, Eigen::VectorXd& _tangentVector) const
{
if (mSegments.empty())
throw std::logic_error("Unable to evaluate empty trajectory.");
if (_derivative < 1)
throw std::logic_error("Derivative must be positive.");
const auto targetSegmentInfo = getSegmentForTime(_t);
const auto& targetSegment = mSegments[targetSegmentInfo.first];
const auto evaluationTime = _t - targetSegmentInfo.second;
// Return zero for higher-order derivatives.
if (_derivative < targetSegment.mCoefficients.cols())
{
// TODO: We should transform this into the body frame using the adjoint
// transformation.
_tangentVector = evaluatePolynomial(
targetSegment.mCoefficients, evaluationTime, _derivative);
}
else
{
_tangentVector.resize(mStateSpace->getDimension());
_tangentVector.setZero();
}
}
Eigen::VectorXd KronProdSPMat2(
Eigen::SparseMatrix<double, Eigen::RowMajor> a0,
Eigen::SparseMatrix<double, Eigen::RowMajor> a1,
Eigen::VectorXd y) {
signal(SIGSEGV, handler); // install our handler
Eigen::VectorXd retvec;
retvec.setZero( a0.rows() * a1.rows() );
//loop rows a0
for (int row_idx0=0; row_idx0<a0.outerSize(); ++row_idx0) {
int row_offset1 = row_idx0;
row_offset1 *= a1.rows();
// loop rows a1
for (int row_idx1=0; row_idx1<a1.outerSize(); ++row_idx1) {
// loop cols a0 (non-zero elements only)
for (Eigen::SparseMatrix<double,RowMajor>::InnerIterator it0(a0,row_idx0); it0; ++it0) {
int col_offset1 = it0.index();
col_offset1 *= a1.innerSize();
double factor1 = it0.value();
for (Eigen::SparseMatrix<double,RowMajor>::InnerIterator it1(a1,row_idx1); it1; ++it1) {
retvec( row_offset1 + row_idx1 ) += factor1 * it1.value() * y( col_offset1 + it1.index() );
}
}
}
}
return retvec;
}
void log_gradient_and_output(size_t variable_idx, const Eigen::VectorXd& inputs, const Eigen::VectorXd& parameters, Eigen::VectorXd& outputs, Eigen::VectorXd& gradient_vector)
{
// TODO: Check concept for InputIterator
//double N = std::distance(first_input, last_input);
//double scaling_factor = 1. / view.size();
gradient_vector.setZero();
// DEBUG
//std::cout << gradient_ << std::endl;
//for (unsigned int i = 0; i < view.size(); i++) {
forward_propagation(parameters, inputs, outputs, true);
// DEBUG: Look for NaN
/*for (size_t idx = 0; idx < static_cast<size_t>(outputs.size()); idx++) {
if (std::isnan(outputs[idx]))
std::cout << "NaN: Output[" << idx << "] from forward propagation" << std::endl;
}*/
// TODO: Check that outputs isn't overwritten by back propagation!
// back_propagation_output(size_t variable_idx, const Eigen::VectorXd& parameters, const Eigen::MatrixBase<T>& inputs, Eigen::VectorXd& outputs, Eigen::VectorXd& gradient, double scaling_factor)
back_propagation_log_output(variable_idx, parameters, inputs, inputs_to_output_activation_, gradient_vector, 1.);
//}
// DEBUG
//std::cout << gradient_ << std::endl;
}
Eigen::VectorXd KronProdSPMat4(
Eigen::SparseMatrix<double, Eigen::RowMajor> a0,
Eigen::SparseMatrix<double, Eigen::RowMajor> a1,
Eigen::SparseMatrix<double, Eigen::RowMajor> a2,
Eigen::SparseMatrix<double, Eigen::RowMajor> a3,
Eigen::VectorXd y) {
signal(SIGSEGV, handler); // install our handler
Eigen::VectorXd retvec;
retvec.setZero( a0.rows() * a1.rows() * a2.rows() * a3.rows() );
//loop rows a0
for (int row_idx0=0; row_idx0<a0.outerSize(); ++row_idx0) {
int row_offset1 = row_idx0;
row_offset1 *= a1.rows();
// loop rows a1
for (int row_idx1=0; row_idx1<a1.outerSize(); ++row_idx1) {
int row_offset2 = row_offset1 + row_idx1;
row_offset2 *= a2.rows();
// loop rows a2
for (int row_idx2=0; row_idx2<a2.outerSize(); ++row_idx2) {
int row_offset3 = row_offset2 + row_idx2;
row_offset3 *= a3.rows();
// loop rows a3
for (int row_idx3=0; row_idx3<a3.outerSize(); ++row_idx3) {
// loop cols a0 (non-zero elements only)
for (Eigen::SparseMatrix<double,RowMajor>::InnerIterator it0(a0,row_idx0); it0; ++it0) {
int col_offset1 = it0.index();
col_offset1 *= a1.innerSize();
double factor1 = it0.value();
// loop cols a1
for (Eigen::SparseMatrix<double,RowMajor>::InnerIterator it1(a1,row_idx1); it1; ++it1) {
int col_offset2 = col_offset1 + it1.index();
col_offset2 *= a2.innerSize();
double factor2 = factor1 * it1.value();
//loop cols a2
for (Eigen::SparseMatrix<double,RowMajor>::InnerIterator it2(a2,row_idx2); it2; ++it2) {
int col_offset3 = col_offset2 + it2.index();
col_offset3 *= a3.innerSize();
double factor3 = factor2 * it2.value();
for (Eigen::SparseMatrix<double,RowMajor>::InnerIterator it3(a3,row_idx3); it3; ++it3) {
retvec( row_offset3 + row_idx3 ) += factor3 * it3.value() * y( col_offset3 + it3.index() );
}
}
}
}
}
}
}
}
return retvec;
}
void SubSystem::getParams(Eigen::VectorXd &xOut)
{
if (xOut.size() != psize)
xOut.setZero(psize);
for (int i=0; i < psize; i++)
xOut[i] = pvals[i];
}
template <typename PointSource, typename PointTarget> void
pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::estimateRigidTransformation (
const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
Eigen::Matrix4f &transformation_matrix)
{
// <cloud_src,cloud_src> is the source dataset
if (cloud_src.points.size () != cloud_tgt.points.size ())
{
PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] ");
PCL_ERROR ("Number or points in source (%zu) differs than target (%zu)!\n",
cloud_src.points.size (), cloud_tgt.points.size ());
return;
}
if (cloud_src.points.size () < 4) // need at least 4 samples
{
PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] ");
PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %zu points!\n",
cloud_src.points.size ());
return;
}
// If no warp function has been set, use the default (WarpPointRigid6D)
if (!warp_point_)
warp_point_.reset (new WarpPointRigid6D<PointSource, PointTarget>);
int n_unknowns = warp_point_->getDimension ();
Eigen::VectorXd x (n_unknowns);
x.setZero ();
// Set temporary pointers
tmp_src_ = &cloud_src;
tmp_tgt_ = &cloud_tgt;
OptimizationFunctor functor (static_cast<int> (cloud_src.points.size ()), this);
Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm (num_diff);
int info = lm.minimize (x);
// Compute the norm of the residuals
PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
PCL_DEBUG ("Final solution: [%f", x[0]);
for (int i = 1; i < n_unknowns; ++i)
PCL_DEBUG (" %f", x[i]);
PCL_DEBUG ("]\n");
// Return the correct transformation
Eigen::VectorXf params = x.cast<float> ();
warp_point_->setParam (params);
transformation_matrix = warp_point_->getTransform ();
tmp_src_ = NULL;
tmp_tgt_ = NULL;
}
void SubSystem::getParams(VEC_pD ¶ms, Eigen::VectorXd &xOut)
{
if (xOut.size() != int(params.size()))
xOut.setZero(params.size());
for (int j=0; j < int(params.size()); j++) {
MAP_pD_pD::const_iterator
pmapfind = pmap.find(params[j]);
if (pmapfind != pmap.end())
xOut[j] = *(pmapfind->second);
}
}
void buildProblem(std::vector<T>& coefficients, Eigen::VectorXd& b, int n)
{
b.setZero();
Eigen::ArrayXd boundary = Eigen::ArrayXd::LinSpaced(n, 0,M_PI).sin().pow(2);
for(int j=0; j<n; ++j)
{
for(int i=0; i<n; ++i)
{
int id = i+j*n;
insertCoefficient(id, i-1,j, -1, coefficients, b, boundary);
insertCoefficient(id, i+1,j, -1, coefficients, b, boundary);
insertCoefficient(id, i,j-1, -1, coefficients, b, boundary);
insertCoefficient(id, i,j+1, -1, coefficients, b, boundary);
insertCoefficient(id, i,j, 4, coefficients, b, boundary);
}
}
}
void SubSystem::calcGrad(VEC_pD ¶ms, Eigen::VectorXd &grad)
{
assert(grad.size() == int(params.size()));
grad.setZero();
for (int j=0; j < int(params.size()); j++) {
MAP_pD_pD::const_iterator
pmapfind = pmap.find(params[j]);
if (pmapfind != pmap.end()) {
// assert(p2c.find(pmapfind->second) != p2c.end());
std::vector<Constraint *> constrs=p2c[pmapfind->second];
for (std::vector<Constraint *>::const_iterator constr = constrs.begin();
constr != constrs.end(); ++constr)
grad[j] += (*constr)->error() * (*constr)->grad(pmapfind->second);
}
}
}
void gradient(View& view, const Eigen::VectorXd& parameters, Eigen::VectorXd& gradient_vector)
{
// TODO: Check concept for InputIterator
//double N = std::distance(first_input, last_input);
double scaling_factor = 1. / view.size();
gradient_vector.setZero();
// DEBUG
//std::cout << gradient_ << std::endl;
for (unsigned int i = 0; i < view.size(); i++) {
forward_propagation(parameters, view.first(i), outputs());
back_propagation_error(parameters, view.first(i), outputs(), view.second(i), gradient_vector, scaling_factor);
}
// DEBUG
//std::cout << gradient_ << std::endl;
}
SimuEuro(const option & o, long path, const std::vector<double> & RN){
opt=o;
N= path;
asset_price.resize(N);
asset_price.setZero();
option_value= asset_price;
for (long i=0; i< N; i++) {
asset_price(i)=opt.S* exp((opt.r- opt.q)*opt.T-.5*opt.sigma*opt.sigma*opt.T+ opt.sigma* sqrt(opt.T)* RN[i]);
if(opt.Call) option_value(i)= fmax(asset_price(i)- opt.K,0.0);
else option_value(i)= fmax(-asset_price(i)+opt.K, 0.0);
}
mean= option_value.sum()/ option_value.size() * exp(-opt.T*opt.r);
stdiv= option_value.squaredNorm()/ option_value.size()* exp(-opt.r*opt.T *2);
stdiv= stdiv- pow(mean,2.0);
stdiv= sqrt(stdiv/ N);
};
// Computes the difference vector of all design variables between the linearization point at marginalization and the current guess, on the tangent space (i.e. log(x_bar - x))
Eigen::VectorXd MarginalizationPriorErrorTerm::getDifferenceSinceMarginalization()
{
Eigen::VectorXd diff = Eigen::VectorXd(_dimensionDesignVariables);
diff.setZero();
int index = 0;
std::vector<Eigen::MatrixXd>::iterator it_marg = _designVariableValuesAtMarginalization.begin();
std::vector<aslam::backend::DesignVariable*>::iterator it_current = _designVariables.begin();
for(;it_current != _designVariables.end(); ++it_current, ++it_marg)
{
// retrieve current value (xbar) and value at marginalization(xHat)
Eigen::MatrixXd xHat = *it_marg;
//get minimal difference in tangent space
Eigen::VectorXd diffVector;
(*it_current)->minimalDifference(xHat, diffVector);
//int base = index;
int dim = diffVector.rows();
diff.segment(index, dim) = diffVector;
index += dim;
}
return diff;
}
void KernelBand<NLSSystemObject>::CalculatesNormOfJacobianRows(Eigen::VectorXd& row_norms)
{
row_norms.setZero();
for (int group = 1; group <= ngroups_; group++)
{
for (int j = group - 1; j < static_cast<int>(this->ne_); j += width_)
{
double* col_j = BAND_COL(J_, j);
int i1 = std::max(0, j - J_->nUpper());
int i2 = std::min(j + J_->nLower(), static_cast<int>(this->ne_ - 1));
for (int i = i1; i <= i2; i++)
{
const double J = BAND_COL_ELEM(col_j, i, j);
row_norms(i) += J*J;
}
}
}
for (unsigned int i = 0; i < this->ne_; i++)
row_norms(i) = std::sqrt(row_norms(i));
}
SimuEuro(const option &o, long path, unsigned int seed= int(time(0))) {
opt=o;
N= path;
asset_price.resize(N);
asset_price.setZero();
option_value= asset_price;
boost::mt19937 eng(seed);
boost::normal_distribution<double> normal(0.0, 1.0);
boost::variate_generator<boost::mt19937&, boost::normal_distribution<double>> rng(eng, normal);
for (long i=0; i< N; i++) {
asset_price(i)=opt.S* exp((opt.r- opt.q)*opt.T-.5*opt.sigma*opt.sigma*opt.T+ opt.sigma* sqrt(opt.T)* rng());
if(opt.Call) option_value(i)= fmax(asset_price(i)- opt.K,0.0);
else option_value(i)= fmax(-asset_price(i)+opt.K, 0.0);
}
mean= option_value.sum()/ option_value.size() * exp(-opt.T*opt.r);
stdiv= option_value.squaredNorm()/ option_value.size()* exp(-opt.r*opt.T *2);
stdiv= stdiv- pow(mean,2.0);
stdiv= sqrt(stdiv/ N);
};
IGL_INLINE double igl::polyvector_field_poisson_reconstruction(
const Eigen::PlainObjectBase<DerivedV> &Vcut,
const Eigen::PlainObjectBase<DerivedF> &Fcut,
const Eigen::PlainObjectBase<DerivedS> &sol3D_combed,
Eigen::PlainObjectBase<DerivedSF> &scalars,
Eigen::PlainObjectBase<DerivedS> &sol3D_recon,
Eigen::PlainObjectBase<DerivedE> &max_error )
{
Eigen::SparseMatrix<typename DerivedV::Scalar> gradMatrix;
igl::grad(Vcut, Fcut, gradMatrix);
Eigen::VectorXd FAreas;
igl::doublearea(Vcut, Fcut, FAreas);
FAreas = FAreas.array() * .5;
int nf = FAreas.rows();
Eigen::SparseMatrix<typename DerivedV::Scalar> M,M1;
Eigen::VectorXi II = igl::colon<int>(0, nf-1);
igl::sparse(II, II, FAreas, M1);
igl::repdiag(M1, 3, M) ;
int half_degree = sol3D_combed.cols()/3;
sol3D_recon.setZero(sol3D_combed.rows(),sol3D_combed.cols());
int numF = Fcut.rows();
scalars.setZero(Vcut.rows(),half_degree);
Eigen::SparseMatrix<typename DerivedV::Scalar> Q = gradMatrix.transpose()* M *gradMatrix;
//fix one point at Ik=fix, value at fixed xk=0
int fix = 0;
Eigen::VectorXi Ik(1);Ik<<fix;
Eigen::VectorXd xk(1);xk<<0;
//unknown indices
Eigen::VectorXi Iu(Vcut.rows()-1,1);
Iu<<igl::colon<int>(0, fix-1), igl::colon<int>(fix+1,Vcut.rows()-1);
Eigen::SparseMatrix<typename DerivedV::Scalar> Quu, Quk;
igl::slice(Q, Iu, Iu, Quu);
igl::slice(Q, Iu, Ik, Quk);
Eigen::SimplicialLDLT<Eigen::SparseMatrix<typename DerivedV::Scalar> > solver;
solver.compute(Quu);
Eigen::VectorXd vec; vec.setZero(3*numF,1);
for (int i =0; i<half_degree; ++i)
{
vec<<sol3D_combed.col(i*3+0),sol3D_combed.col(i*3+1),sol3D_combed.col(i*3+2);
Eigen::VectorXd b = gradMatrix.transpose()* M * vec;
Eigen::VectorXd bu = igl::slice(b, Iu);
Eigen::VectorXd rhs = bu-Quk*xk;
Eigen::MatrixXd yu = solver.solve(rhs);
Eigen::VectorXi index = i*Eigen::VectorXi::Ones(Iu.rows(),1);
igl::slice_into(yu, Iu, index, scalars);scalars(Ik[0],i)=xk[0];
}
// evaluate gradient of found scalar function
for (int i =0; i<half_degree; ++i)
{
Eigen::VectorXd vec_poisson = gradMatrix*scalars.col(i);
sol3D_recon.col(i*3+0) = vec_poisson.segment(0*numF, numF);
sol3D_recon.col(i*3+1) = vec_poisson.segment(1*numF, numF);
sol3D_recon.col(i*3+2) = vec_poisson.segment(2*numF, numF);
}
max_error.setZero(numF,1);
for (int i =0; i<half_degree; ++i)
{
Eigen::VectorXd diff = (sol3D_recon.block(0, i*3, numF, 3)-sol3D_combed.block(0, i*3, numF, 3)).rowwise().norm();
diff = diff.array() / sol3D_combed.block(0, i*3, numF, 3).rowwise().norm().array();
max_error = max_error.cwiseMax(diff.cast<typename DerivedE::Scalar>());
}
return max_error.mean();
}
void init(){
//find the path of config files
std::string selfpath = get_selfpath();
//select using normal control mode or psudogravity control mode
right_rmt = NormalMode;
//initialize FT sensor ptr
ft_gama = new gamaFT;
ft.setZero(6);
tool_vec_g.setZero();
axis_end_vec.setZero();
//show toolname
tn = hingedtool;
StopFlag = false;
//initialize the axis vec
local_hinged_axis_vec.setZero();
local_hinged_axis_vec(0) = -0.9472;
local_hinged_axis_vec(1) = 0.0494;
local_hinged_axis_vec(2) = -0.3166;
des_tm.setZero();
des_vec.setZero();
//declare the cb function
boost::function<void(boost::shared_ptr<std::string>)> button_sdh_moveto(sdh_moveto_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdhaxisvec_moveto(sdhaxisvec_moveto_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdhmaintainF(sdhmaintainF_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdhslideX(sdhslideX_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdhslideY(sdhslideY_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdhfoldtool(sdhfoldtool_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_gamaftcalib(gamaftcalib_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdh_grav_comp_ctrl(sdh_grav_comp_ctrl_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_sdh_normal_ctrl(sdh_normal_ctrl_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_brake(brake_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_nobrake(nobrake_cb);
boost::function<void(boost::shared_ptr<std::string>)> button_closeprog(closeprog_cb);
//specify controller configure file in order to load it(right kuka + shadow hand)
std::string config_filename = selfpath + "/etc/right_arm_param.xml";
std::cout<<"right arm config file name is: "<<config_filename<<std::endl;
//load controller parameters
right_pm = new ParameterManager(config_filename);
//initialize ptr to right kuka com okc
com_okc_right = new ComOkc(kuka_right,OKC_HOST,OKC_PORT);
//connect kuka right
com_okc_right->connect();
//initialize the kuka robot and let it stay in the init pose
kuka_right_arm = new KukaLwr(kuka_right,*com_okc_right,tn);
//initialize the robot state
right_rs = new RobotState(kuka_right_arm);
//get the initialize state of kuka right
kuka_right_arm->get_joint_position_act();
kuka_right_arm->update_robot_state();
right_rs->updated(kuka_right_arm);
right_ac_vec.push_back(new ProActController(*right_pm));
right_task_vec.push_back(new KukaSelfCtrlTask(RP_NOCONTROL));
right_task_vec.back()->mt = JOINTS;
right_task_vec.back()->mft = GLOBAL;
Eigen::Vector3d p,o;
p.setZero();
o.setZero();
//get start point position in cartesian space
p(0) = initP_x = right_rs->robot_position["eef"](0);
p(1) = initP_y= right_rs->robot_position["eef"](1);
p(2) = initP_z= right_rs->robot_position["eef"](2);
o = tm2axisangle(right_rs->robot_orien["eef"]);
initO_x = o(0);
initO_y = o(1);
initO_z = o(2);
right_task_vec.back()->set_desired_p_eigen(p);
right_task_vec.back()->set_desired_o_ax(o);
kuka_right_arm->setAxisStiffnessDamping(right_ac_vec.back()->pm.stiff_ctrlpara.axis_stiffness, \
right_ac_vec.back()->pm.stiff_ctrlpara.axis_damping);
com_rsb = new ComRSB();
rdtschunkjs = SchunkJS;
com_rsb->add_msg(rdtschunkjs);
gama_f_filter = new TemporalSmoothingFilter<Eigen::Vector3d>(10,Average,Eigen::Vector3d(0,0,0));
gama_t_filter = new TemporalSmoothingFilter<Eigen::Vector3d>(10,Average,Eigen::Vector3d(0,0,0));
//register cb function
com_rsb->register_external("/foo/sdhmoveto",button_sdh_moveto);
com_rsb->register_external("/foo/sdhaxisvecmoveto",button_sdhaxisvec_moveto);
com_rsb->register_external("/foo/sdhmaintainF",button_sdhmaintainF);
com_rsb->register_external("/foo/sdhslideX",button_sdhslideX);
com_rsb->register_external("/foo/sdhslideY",button_sdhslideY);
com_rsb->register_external("/foo/sdhfoldtool",button_sdhfoldtool);
com_rsb->register_external("/foo/gamaftcalib",button_gamaftcalib);
com_rsb->register_external("/foo/sdh_grav_comp_ctrl",button_sdh_grav_comp_ctrl);
com_rsb->register_external("/foo/sdh_normal_ctrl",button_sdh_normal_ctrl);
com_rsb->register_external("/foo/closeprog",button_closeprog);
#ifdef HAVE_ROS
std::string left_kuka_arm_name="la";
std::string right_kuka_arm_name="ra";
std::string left_schunk_hand_name ="lh";
js_la.name.push_back(left_kuka_arm_name+"_arm_0_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_1_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_2_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_3_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_4_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_5_joint");
js_la.name.push_back(left_kuka_arm_name+"_arm_6_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_0_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_1_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_2_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_3_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_4_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_5_joint");
js_ra.name.push_back(right_kuka_arm_name+"_arm_6_joint");
//for schunk
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_knuckle_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_finger_22_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_finger_23_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_thumb_2_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_thumb_3_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_finger_12_joint");
js_schunk.name.push_back(left_schunk_hand_name+"_sdh_finger_13_joint");
js_la.position.resize(7);
js_la.velocity.resize(7);
js_la.effort.resize(7);
js_ra.position.resize(7);
js_ra.velocity.resize(7);
js_ra.effort.resize(7);
js_schunk.position.resize(7);
js_schunk.velocity.resize(7);
js_schunk.effort.resize(7);
js_la.header.frame_id="frame_la";
js_ra.header.frame_id="frame_ra";
js_ra.header.frame_id="frame_lh";
gamma_force_marker_pub = nh->advertise<visualization_msgs::Marker>("gamma_force_marker", 2);
hingedtool_axis_marker_pub = nh->advertise<visualization_msgs::Marker>("hingedtool_axis_marker", 2);
jsPub_la = nh->advertise<sensor_msgs::JointState> ("/la/joint_states", 2);
jsPub_ra = nh->advertise<sensor_msgs::JointState> ("/ra/joint_states", 2);
jsPub_schunk = nh->advertise<sensor_msgs::JointState> ("/lh/joint_states", 2);
ros::spinOnce();
br = new tf::TransformBroadcaster();
std::cout<<"ros init finished"<<std::endl;
#endif
}
TEST(PinholeCamera, functions)
{
const size_t NUM_POINTS = 100;
// instantiate all possible versions of test cameras
std::vector<std::shared_ptr<okvis::cameras::CameraBase> > cameras;
cameras.push_back(
okvis::cameras::PinholeCamera<okvis::cameras::NoDistortion>::createTestObject());
cameras.push_back(
okvis::cameras::PinholeCamera<
okvis::cameras::RadialTangentialDistortion>::createTestObject());
cameras.push_back(
okvis::cameras::PinholeCamera<okvis::cameras::EquidistantDistortion>::createTestObject());
cameras.push_back(
okvis::cameras::PinholeCamera<okvis::cameras::RadialTangentialDistortion8>::createTestObject());
for (size_t c = 0; c < cameras.size(); ++c) {
//std::cout << "Testing " << cameras.at(c)->type() << std::endl;
// try quite a lot of points:
for (size_t i = 0; i < NUM_POINTS; ++i) {
// create a random point in the field of view:
Eigen::Vector2d imagePoint = cameras.at(c)->createRandomImagePoint();
// backProject
Eigen::Vector3d ray;
EXPECT_TRUE(cameras.at(c)->backProject(imagePoint, &ray));
// randomise distance
ray.normalize();
ray *= (0.2 + 8 * (Eigen::Vector2d::Random()[0] + 1.0));
// project
Eigen::Vector2d imagePoint2;
Eigen::Matrix<double, 2, 3> J;
Eigen::Matrix2Xd J_intrinsics;
EXPECT_TRUE(
cameras.at(c)->project(ray, &imagePoint2, &J, &J_intrinsics)
== okvis::cameras::CameraBase::ProjectionStatus::Successful);
// check they are the same
EXPECT_TRUE((imagePoint2 - imagePoint).norm() < 0.01);
// check point Jacobian vs. NumDiff
const double dp = 1.0e-7;
Eigen::Matrix<double, 2, 3> J_numDiff;
for (size_t d = 0; d < 3; ++d) {
Eigen::Vector3d point_p = ray
+ Eigen::Vector3d(d == 0 ? dp : 0, d == 1 ? dp : 0,
d == 2 ? dp : 0);
Eigen::Vector3d point_m = ray
- Eigen::Vector3d(d == 0 ? dp : 0, d == 1 ? dp : 0,
d == 2 ? dp : 0);
Eigen::Vector2d imagePoint_p;
Eigen::Vector2d imagePoint_m;
cameras.at(c)->project(point_p, &imagePoint_p);
cameras.at(c)->project(point_m, &imagePoint_m);
J_numDiff.col(d) = (imagePoint_p - imagePoint_m) / (2 * dp);
}
EXPECT_TRUE((J_numDiff - J).norm() < 0.0001);
// check intrinsics Jacobian
const int numIntrinsics = cameras.at(c)->noIntrinsicsParameters();
Eigen::VectorXd intrinsics;
cameras.at(c)->getIntrinsics(intrinsics);
Eigen::Matrix2Xd J_numDiff_intrinsics;
J_numDiff_intrinsics.resize(2,numIntrinsics);
for (int d = 0; d < numIntrinsics; ++d) {
Eigen::VectorXd di;
di.resize(numIntrinsics);
di.setZero();
di[d] = dp;
Eigen::Vector2d imagePoint_p;
Eigen::Vector2d imagePoint_m;
Eigen::VectorXd intrinsics_p = intrinsics+di;
Eigen::VectorXd intrinsics_m = intrinsics-di;
cameras.at(c)->projectWithExternalParameters(ray, intrinsics_p, &imagePoint_p);
cameras.at(c)->projectWithExternalParameters(ray, intrinsics_m, &imagePoint_m);
J_numDiff_intrinsics.col(d) = (imagePoint_p - imagePoint_m) / (2 * dp);
}
/*std::cout<<J_numDiff_intrinsics<<std::endl;
std::cout<<"----------------"<<std::endl;
std::cout<<J_intrinsics<<std::endl;
std::cout<<"================"<<std::endl;*/
EXPECT_TRUE((J_numDiff_intrinsics - J_intrinsics).norm() < 0.0001);
}
}
}
void KernelBand<NLSSystemObject>::QuasiNewton(const Eigen::VectorXd& dxi, const Eigen::VectorXd& dfi)
{
const double tstart = OpenSMOKE::OpenSMOKEGetCpuTime();
{
// The auxiliary vector named x_plus is used here
Eigen::VectorXd* normSquared = &x_plus_;
normSquared->setZero();
for (int group = 1; group <= ngroups_; group++)
{
for (int j = group - 1; j < static_cast<int>(this->ne_); j += width_)
{
int i1 = std::max(0, j - J_->nUpper());
int i2 = std::min(j + J_->nLower(), static_cast<int>(this->ne_ - 1));
for (int i = i1; i <= i2; i++)
(*normSquared)(i) += dxi(j)*dxi(j);
}
}
// The auxiliary vector named x_plus is used here
Eigen::VectorXd* sum_vector = &f_plus_;
(*sum_vector) = dfi;
for (int group = 1; group <= ngroups_; group++)
{
for (int j = group - 1; j < static_cast<int>(this->ne_); j += width_)
{
double* col_j = BAND_COL(J_, j);
int i1 = std::max(0, j - J_->nUpper());
int i2 = std::min(j + J_->nLower(), static_cast<int>(this->ne_ - 1));
for (int i = i1; i <= i2; i++)
(*sum_vector)(i) -= BAND_COL_ELEM(col_j, i, j)*dxi(j);
}
}
for (int group = 1; group <= ngroups_; group++)
{
for (int j = group - 1; j < static_cast<int>(this->ne_); j += width_)
{
double* col_j = BAND_COL(J_, j);
int i1 = std::max(0, j - J_->nUpper());
int i2 = std::min(j + J_->nLower(), static_cast<int>(this->ne_ - 1));
}
}
const double eps = 1.e-10;
for (int j = 0; j < static_cast<int>(this->ne_); j++)
(*sum_vector)(j) /= ((*normSquared)(j) + eps);
for (int group = 1; group <= ngroups_; group++)
{
for (int j = group - 1; j < static_cast<int>(this->ne_); j += width_)
{
double* col_j = BAND_COL(J_, j);
int i1 = std::max(0, j - J_->nUpper());
int i2 = std::min(j + J_->nLower(), static_cast<int>(this->ne_ - 1));
for (int i = i1; i <= i2; i++)
BAND_COL_ELEM(col_j, i, j) += (*sum_vector)(i)*dxi(j);
}
}
}
const double tend = OpenSMOKE::OpenSMOKEGetCpuTime();
numberOfJacobianQuasiAssembling_++;
cpuTimeSingleJacobianQuasiAssembling_ = tend - tstart;
cpuTimeJacobianQuasiAssembling_ += cpuTimeSingleJacobianQuasiAssembling_;
}
void display()
{
using namespace Eigen;
using namespace igl;
using namespace std;
if(!trackball_on && tot_num_samples < 10000)
{
if(S.size() == 0)
{
S.resize(V.rows());
S.setZero();
}
VectorXd Si;
const int num_samples = 20;
ambient_occlusion(ei,V,N,num_samples,Si);
S *= (double)tot_num_samples;
S += Si*(double)num_samples;
tot_num_samples += num_samples;
S /= (double)tot_num_samples;
}
// Convert to 1-intensity
C.conservativeResize(S.rows(),3);
if(ao_on)
{
C<<S,S,S;
C.array() = (1.0-ao_factor*C.array());
}else
{
C.setConstant(1.0);
}
if(ao_normalize)
{
C.col(0) *= ((double)C.rows())/C.col(0).sum();
C.col(1) *= ((double)C.rows())/C.col(1).sum();
C.col(2) *= ((double)C.rows())/C.col(2).sum();
}
C.col(0) *= color(0);
C.col(1) *= color(1);
C.col(2) *= color(2);
glClearColor(back[0],back[1],back[2],0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// All smooth points
glEnable( GL_POINT_SMOOTH );
glDisable(GL_LIGHTING);
if(lights_on)
{
lights();
}
push_scene();
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
glEnable(GL_NORMALIZE);
glEnable(GL_COLOR_MATERIAL);
glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
push_object();
// Draw the model
// Set material properties
glEnable(GL_COLOR_MATERIAL);
draw_mesh(V,F,N,C);
pop_object();
// Draw a nice floor
glPushMatrix();
const double floor_offset =
-2./bbd*(V.col(1).maxCoeff()-mid(1));
glTranslated(0,floor_offset,0);
const float GREY[4] = {0.5,0.5,0.6,1.0};
const float DARK_GREY[4] = {0.2,0.2,0.3,1.0};
draw_floor(GREY,DARK_GREY);
glPopMatrix();
pop_scene();
report_gl_error();
TwDraw();
glutSwapBuffers();
glutPostRedisplay();
}
/**
* Take as input the full constraint matrix CI for a dimension ny, and
* the associated ci vector with the 2-norm of each row scaled by delta.
* Apply the quadratic program and test against all the constraints.
* Return true if all constraints match, false otherwise.
*/
int rational_fitter_parsec_multi::solve_wrapper( const gesvdm2_args_t *args, subproblem_t *pb, int M, int ny,
double *CIptr, double *ciptr )
{
const vertical_segment *d = (const vertical_segment*)(args->dataptr);
rational_function *rf = (rational_function*)(pb->rfptr);
rational_function_1d *rf1d = rf->get(ny);
const int np = pb->np;
const int nq = pb->nq;
const int N = np + nq;
// Compute the solution
Eigen::MatrixXd CE(N, 0);
Eigen::VectorXd ce(0);
Eigen::MatrixXd G (N, N); G.setIdentity();
Eigen::VectorXd g (N); g.setZero();
Eigen::VectorXd x (0.0, N);
Eigen::MatrixXd CI = Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>::Map(CIptr, N, M);
Eigen::Map<Eigen::VectorXd> ci(ciptr, M);
// Select the size of the result vector to
// be equal to the dimension of p + q
double cost = QuadProgPP::solve_quadprog(G, g, CE, ce, CI, ci, x);
bool solves_qp = !(cost == std::numeric_limits<double>::infinity());
if(solves_qp) {
std::cout << "<<INFO>> got solution for pb with np=" << pb->np << ", nq=" << pb->nq << std::endl;
// Recopy the vector d
vec p(np), q(nq);
double norm = 0.0;
for(int i=0; (i<N) & solves_qp; ++i) {
const double v = x[i];
solves_qp = solves_qp && !isnan(v)
&& (v != std::numeric_limits<double>::infinity());
norm += v*v;
if(i < np) {
p[i] = v;
}
else {
q[i - np] = v;
}
}
if (solves_qp) {
std::cout << "<<INFO>> got solution to second step for pb with np=" << pb->np << ", nq=" << pb->nq << std::endl;
// Rq: doesn't need protection in // since it should be working on independant vectors
rf1d->update(p, q);
solves_qp = (test_all_constraint( d, rf1d, ny ) == 1);
}
}
else {
std::cerr << "<<DEBUG>> Didn't get solution to the pb with np=" << pb->np << ", nq=" << pb->nq << std::endl;
}
return solves_qp;
}
int main()
{
//initialisation
Eigen::ArrayXXd R = Eigen::ArrayXXd::Zero(FEATURE_SIZE, NO_OF_POSTAGS); //matrix R, dimension: Nf x Nw
std::default_random_engine generator;
std::normal_distribution<double> distr(0.0, 0.1);
//initialise the matrix R with small, random, non-zero numbers
for (int i = 0; i < R.rows(); i++) {
for (int j = 0; j < R.cols(); j++) {
double d = distr(generator);
//avoid having 0's as the weight, try to get all the coefficients initialised with small numbers
while (d == 0.0) {
d = distr(generator);
}
R(i, j) = d;
}
}
//initialise the matrix Q, of size feature_size x (no_of_postags * no_of_distance)
Eigen::ArrayXXd Q = Eigen::ArrayXXd::Zero(FEATURE_SIZE, NO_OF_POSTAGS);
for (int i = 0; i < Q.rows(); i++) {
for (int j = 0; j < Q.cols(); j++) {
double d = distr(generator);
//avoid having 0's as the weight, try to get all the coefficients initialised with small numbers
while (d == 0.0) {
d = distr(generator);
}
Q(i, j) = d;
}
}
//create an array of Ci (where each Ci is a matrix of Nf x Nf that governs the interaction between the ith feature and vn)
std::vector<Eigen::ArrayXXd> Ci;
for (int i = 0; i < 5; i++) {
Eigen::ArrayXXd Ci_temp = Eigen::ArrayXXd::Zero(FEATURE_SIZE, FEATURE_SIZE);
std::default_random_engine generator;
std::normal_distribution<double> distr(0.0, 0.1);
for (int j = 0; j < Ci_temp.rows(); j++) {
for (int k = 0; k < Ci_temp.cols(); k++) {
double d = distr(generator);
//avoid having 0's as the weight, try to get all the coefficients initialised with small numbers
while (d == 0.0) {
d = distr(generator);
}
Ci_temp(j, k) = d;
}
}
Ci.push_back(Ci_temp);
}
//initialise vi. We use a context of 5 (the postag of the head itself, the postag before the modifier, the postag after the modifier, the postag before the head, and the postag after the head)
//initialise vn. Both vi and vn are column vectors with all 0's and a single 1, which indicates the POS-tag we desire to get
//FOR NOW, we ignore the term br^transpose * R^transpose*vn, but find out what it is
//initialise the bias term bv
Eigen::ArrayXd bv;
//print the array of biases into an external file
std::ofstream final_postag_bias("final_postag_bias.txt");
if (!final_postag_bias.is_open()) {
std::cout << "fail to create final postag bias file" << std::endl;
}
/*std::map<std::string, int> big_pos_tag_map = get_postag_bias(bv, "distance_count.txt", "fine_pos_tags.txt");
std::cout << "size of big pos tag map = " << big_pos_tag_map.size() << std::endl;
for (std::map<std::string, int>::iterator it = big_pos_tag_map.begin(); it != big_pos_tag_map.end(); it++) {
final_postag_bias << "Pos tag " << it->first << " has bias value " << bv(it->second) << std::endl;
} */
/* create a pair between each "target pos tag" with a bias value (tested and correct)*/
//Eigen::ArrayXd word_bias_pair = Eigen::ArrayXd::Zero(NO_OF_POSTAGS_WITHOUT_DUMMY * NO_OF_DISTANCE);
int i_3 = -1;
std::ifstream fine_pos_tags("fine_pos_tags.txt");
std::string temp_string;
/*
while (getline(fine_pos_tags, temp_string)) {
for (int i = -MAX_DISTANCE; i <= MAX_DISTANCE; i++) {
if (i != 0) {
if (i == -49 || i == -45 || i == -40 || i == -35 || i == -30 || i == -25 || i == -20 || i == -15 || i == -10 || i == -5 || i == -4 || i == -3 || i == -2 || i == -1 || i == 1 || i == 2 || i == 3 || i == 4 || i == 5 || i == 6 || i == 11 || i == 16 || i == 21 || i == 26 || i == 31 || i == 36|| i == 41 || i == 46) {
i_3++;
std::stringstream sstm;
sstm << temp_string << i;
if (big_pos_tag_map.find(sstm.str()) == big_pos_tag_map.end()) {
std::cout << "ERROR" << std::endl;
} else {
int pos_tag_idx = big_pos_tag_map[sstm.str()];
//word_bias_pair(i_3) = bv(pos_tag_idx);
}
}
}
}
} */
/* end */
//main training part
std::map<std::string, int> mapping = postag_mapping("fine_pos_tags.txt");
fine_pos_tags.close();
//print out both the small and big mapping
//std::ofstream big_pos_tag_mapping("big_pos_tag_map.txt");
std::ofstream small_pos_tag_mapping("small_pos_tag_map.txt");
/*
for (std::map<std::string, int>::iterator it = big_pos_tag_map.begin(); it != big_pos_tag_map.end(); it++) {
big_pos_tag_mapping << it->first << " " << it->second << std::endl;
} */
for (std::map<std::string, int>::iterator it = mapping.begin(); it != mapping.end(); it++) {
small_pos_tag_mapping << it->first << " " << it->second << std::endl;
}
small_pos_tag_mapping.close();
//create the pairs
std::ifstream pairs ("real_pairs_medium.txt");
//initialise the gradient matrix
std::vector<Eigen::MatrixXd> dCi;
for (int i = 0; i < 5; i++) {
Eigen::MatrixXd temp = Eigen::MatrixXd::Zero(FEATURE_SIZE, FEATURE_SIZE);
dCi.push_back(temp);
}
Eigen::MatrixXd dR = Eigen::MatrixXd::Zero(FEATURE_SIZE, NO_OF_POSTAGS);
Eigen::MatrixXd dQ = Eigen::MatrixXd::Zero(FEATURE_SIZE, NO_OF_POSTAGS);
std::ofstream parameters ("parameters_each_epoch_small_trial_1_check_R.txt");
if (!parameters.is_open()) {
std::cout << "Error: output file not opening" << std::endl;
}
//initialise all the temporary variables outside the loop to save time on memory allocation on the heap (expensive)
//all these temporary variables are reset to zero using .setZero() method at the end of each loop
Eigen::MatrixXd tempC = Eigen::MatrixXd::Zero(FEATURE_SIZE, FEATURE_SIZE);
Eigen::MatrixXd temp_R_Model = Eigen::MatrixXd::Zero(FEATURE_SIZE, NO_OF_POSTAGS);
Eigen::MatrixXd temp_R_Data = Eigen::MatrixXd::Zero(FEATURE_SIZE, NO_OF_POSTAGS);
Eigen::MatrixXd temp_Q = Eigen::MatrixXd::Zero(FEATURE_SIZE, NO_OF_POSTAGS);
Eigen::MatrixXd temp_Q_Model = Eigen::MatrixXd::Zero(FEATURE_SIZE, NO_OF_POSTAGS);
Eigen::MatrixXd temp_Q_Data = Eigen::MatrixXd::Zero(FEATURE_SIZE, NO_OF_POSTAGS);
Eigen::VectorXd temp_Qw = Eigen::VectorXd::Zero(FEATURE_SIZE);
//Eigen::VectorXd dbias = Eigen::VectorXd::Zero(NO_OF_POSTAGS_WITHOUT_DUMMY * NO_OF_DISTANCE);
//Eigen::VectorXd temp_dbias = Eigen::VectorXd::Zero(NO_OF_POSTAGS_WITHOUT_DUMMY * NO_OF_DISTANCE);
Eigen::VectorXd tempCr = Eigen::VectorXd::Zero(FEATURE_SIZE);
Eigen::ArrayXd temp_e = Eigen::ArrayXd::Zero(NO_OF_POSTAGS);
/*
Eigen::MatrixXd temp_matrix = Eigen::MatrixXd::Zero(NO_OF_POSTAGS, FEATURE_SIZE);
Eigen::MatrixXd temp_matrix_2 = Eigen::MatrixXd::Zero(NO_OF_POSTAGS, FEATURE_SIZE); */
//calculate the normalising constant for the first time
std::ofstream gradient_check_R("gradient_check_R_big.txt");
Eigen::MatrixXd curr_dR = Eigen::MatrixXd::Zero(FEATURE_SIZE, NO_OF_POSTAGS); //DELETE THIS LATER
int no_of_iter = 0;
for (int i = 0; i < NO_OF_EPOCH; i++) {
if (!pairs.is_open()) {
std::cout << "Fail to open input file" << std::endl;
} else {
std::vector<std::string> temp;
std::string line;
while (getline(pairs, line)) {
if (line.size() > 1) {
no_of_iter++;
temp = split(line, ' ');
//initialise an array with the elements as the indices of: postag of the modifier, postag of the head, postag of before modifier, postag of after modifier, postag of before head, postag of after head
int arr_temp[6] = {mapping[temp[0]], mapping[temp[1]], mapping[temp[2]], mapping[temp[3]], mapping[temp[4]], mapping[temp[5]]};
if (no_of_iter % 10000 == 0) {
std::cout << no_of_iter << std::endl;
}
//calculate the normalising constant
double normalising_constant = 0.0;
for (int iter2 = 0; iter2 < 5; iter2++) {
tempCr += (Ci[iter2].matrix() * R.col(arr_temp[iter2+1]).matrix()); //prediction
}
for (int iter = 0; iter < Q.cols(); iter++) {
double temp = exp((tempCr.transpose() * Q.col(iter).matrix()));//dot product
temp_e(iter) = temp;
normalising_constant += temp;
}
//calculate all the gradient changes for each of the Ci, from dCi[0] to dCi[4]
//this operation is already efficient
for (int j = 0; j < 5; j++) {
for (int k = 0; k < Q.cols(); k++) {
tempC += (temp_e(k) * (Q.col(k).matrix() * R.col(arr_temp[j+1]).transpose().matrix()));
}
//normalise
tempC /= normalising_constant;
dCi[j] += (Q.col(arr_temp[0]).matrix() * R.col(arr_temp[j+1]).transpose().matrix() - tempC);
//reset the matrix tempC to prepare to calculate the next dCi[j]
tempC.setZero();
}
//calculate dQ VERIFY
for (int j = 0; j < 5; j++) {
temp_Qw += (Ci[j].matrix() * R.col(arr_temp[j+1]).matrix());
}
for (int j = 0; j < Q.cols(); j++) {
temp_Q_Model.col(j) += (temp_e(j) * temp_Qw);
}
temp_Q_Model /= normalising_constant;
temp_Q_Data.col(arr_temp[0]) = temp_Qw;
dQ += (temp_Q_Data - temp_Q_Model);
temp_Qw.setZero();
temp_Q_Model.setZero();
temp_Q_Data.setZero();
//calculate dR
//calculate dR with respect to data
for (int j = 0; j < 5; j++) {
temp_R_Data.col(arr_temp[j+1]) += Ci[j].transpose().matrix() * Q.col(arr_temp[0]).matrix();
}
//calculate dR with respect to model
for (int k = 0; k < Q.cols(); k++) {
for (int j = 0; j < 5; j++) {
temp_R_Model.col(arr_temp[j+1]) += (temp_e(k) * (Ci[j].transpose().matrix() * Q.col(k).matrix()));
}
}
temp_R_Model /= normalising_constant;
dR += (temp_R_Data - temp_R_Model);
if (no_of_iter % 1000 == 0) {
curr_dR = temp_R_Data - temp_R_Model;
}
temp_R_Data.setZero();
temp_R_Model.setZero();
//calculate dbias
//temp_dbias(arr_temp[0]) = 1.0;
//dbias += (temp_dbias - temp_e.matrix() / normalising_constant);
//temp_dbias(arr_temp[0]) = 0.0;
tempCr.setZero();
temp_e.setZero();
if (no_of_iter % 1000 == 0) {
Eigen::VectorXd temp = Eigen::VectorXd::Zero(FEATURE_SIZE);
gradient_check_R << "current data point = " << no_of_iter <<std::endl;
for (int j = 0; j < R.rows(); j++) {
for (int k = 0; k < R.cols(); k++) {
Eigen::ArrayXXd R_copy = R;
double result_1 = 0.0;
double result_2 = 0.0;
double temp_double = 0.0;
gradient_check_R << curr_dR(j, k) << " ";
R_copy(j, k) += EPSILON;
for (int iter2 = 0; iter2 < 5; iter2++) {
temp += (Ci[iter2].matrix() * R_copy.col(arr_temp[iter2+1]).matrix());
}
result_1 += temp.transpose() * Q.col(arr_temp[0]).matrix();
for (int l = 0; l < Q.cols(); l++) {
temp_double += exp(temp.transpose() * Q.col(l).matrix());
}
result_1 -= log(temp_double);
temp_double = 0.0;
//calculate result_2
R_copy(j, k) -= (2 * EPSILON);
temp.setZero();
for (int iter2 = 0; iter2 < 5; iter2++) {
temp += (Ci[iter2].matrix() * R_copy.col(arr_temp[iter2 + 1]).matrix());
}
result_2 += temp.transpose() * Q.col(arr_temp[0]).matrix();
for (int l = 0; l < Q.cols(); l++) {
temp_double += exp(temp.transpose() * Q.col(l).matrix());
}
result_2 -= log(temp_double);
temp.setZero();
gradient_check_R << (result_1 - result_2) / (2 * EPSILON) << std::endl;
}
}
curr_dR.setZero();
}
//update the gradient if no_of_iter mod mini batch size == 0, reset the gradient matrix to 0
if (no_of_iter % BATCH_SIZE == 0) {
double curr_learning_rate = LEARNING_RATE / (1 + no_of_iter * LEARNING_DECAY);
for (int j = 0; j < 5; j++) {
Ci[j] = Ci[j].matrix() + curr_learning_rate * dCi[j];
//reset the matrix dCi[j]
dCi[j].setZero();
}
//word_bias_pair = word_bias_pair + curr_learning_rate * dbias.array();
R = R.matrix() + curr_learning_rate * dR;
Q = Q.matrix() + curr_learning_rate * dQ;
dR.setZero();
dQ.setZero();
//dbias.setZero();
}
}
temp.clear();
}
pairs.close();
}
parameters << "Epoch number: " << i+1 << std::endl;
for (int j = 0; j < 5; j++) {
parameters << "Matrix C" << j+1 << std::endl;
parameters << Ci[j] << std::endl;
parameters << std::endl;
parameters << "End of matrix C" << j+1 << std::endl;
}
parameters << "Matrix R" << std::endl;
parameters << R << std::endl;
parameters << "End of matrix R" << std::endl;
parameters << "Matrix Q" << std::endl;
parameters << Q << std::endl;
parameters << "End of matrix Q" << std::endl;
//parameters << "Bias vector" << std::endl;
//parameters << word_bias_pair << std::endl;
//parameters << "End of bias vector" << std::endl;
pairs.open("real_pairs_medium.txt");
}
pairs.close();
parameters.close();
return 0;
}
/**
* Simulate
*
* @param double input
* @return double u - control signal
*/
double RegulatorGPC::simulate(double input)
{
if(m_proces)
{
m_w = m_proces->simulate();
const double y = input;
m_e = m_w - y;
m_history_Y.push_front(y);
m_identify.add_sample(input, m_history_U.front());
//std::deque<double> A,B;
m_poly_A.clear();
m_poly_B.clear();
//A.push_back(-0.6);
//B.push_back(0.4);
m_identify.get_polynomial_a(m_poly_A);
m_identify.get_polynomial_b(m_poly_B);
// Return disruption
if (m_initial_steps_left > 0)
{
const double u = m_w - y;
m_history_U.push_front(u);
m_initial_steps_left--;
return u;
}
// Control algorithm
// ------------------------------------------------------------------------------------------------------
// 1. Calculating h initial conditions equal zero, delay = 0
// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
// page 27
Eigen::VectorXd h(m_H);
{
std::map<std::string, double> others;
others["k"] = 0;
others["stationary"] = 0;
others["noise"] = 0;
ARX ob;
ob.set_parameters(m_poly_A,m_poly_B,others);
for(int i=0; i<m_H; i++)
{
h[i] = ob.simulate(1.0);
}
}
// 2. Calculating Q:
// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
// page 28
Eigen::MatrixXd Q;
Q.setZero(m_H, m_L);
for(int i=0; i<m_H; i++)
{
for(int j=0; j<m_L; j++)
{
if(i-j<0)
{
Q(i,j) = 0;
}
else
{
Q(i,j) = h[i-j];
}
}
}
// 3. Calculating w0
// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
// page 8
Eigen::VectorXd w0(m_H);
w0[0] = (1-m_alpha)*m_w + m_alpha*y;
for(int i=1; i<m_H; i++)
{
w0[i] = (1-m_alpha)*m_w + m_alpha*w0[i-1];
}
// 4. Calculating q
// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
// page 20
Eigen::VectorXd tmp;
tmp.setZero(m_L);
tmp[0] = 1;
Eigen::MatrixXd mIdentity;
mIdentity.setIdentity(m_L,m_L);
Eigen::VectorXd q = (tmp.transpose()
*((Q.transpose()*Q + m_ro*mIdentity).inverse())
*Q.transpose()
).transpose();
// 5. Calculating y0
// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
// page 31
Eigen::VectorXd y0(m_H);
{
ARX ob;
std::map<std::string, double> others;
others["k"] = 0;
others["stationary"] = 0;
others["noise"] = 0;
ob.set_parameters(m_poly_A,m_poly_B,others);
ob.set_initial_state(m_history_U, m_history_Y);
ob.simulate(m_history_U.front());
for (int i=0; i<m_H; i++)
{
y0[i] = ob.simulate(m_history_U.front());
}
}
// 6. Calculating final control
// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
// page 35
const double du = q.transpose() * (w0-y0);
const double u = m_history_U.front() + du;
m_history_U.push_front(u);
return u;
}
else
return 0.0;
}
// [[Rcpp::export]]
List predictLong_cpp(Rcpp::List in_list)
{
//**********************************
// basic parameter
//**********************************
int nSim = Rcpp::as< int > (in_list["nSim"]);
int nBurnin = Rcpp::as< int > (in_list["nBurnin"] );
int silent = Rcpp::as< int > (in_list["silent"]);
//**********************************
// setting up the main data
//**********************************
//if(silent == 0){
// Rcpp::Rcout << " Setup data\n";
//}
Rcpp::List obs_list = Rcpp::as<Rcpp::List> (in_list["obs_list"]);
int nindv = obs_list.length(); //count number of patients
std::vector< Eigen::SparseMatrix<double,0,int> > As( nindv);
std::vector< Eigen::SparseMatrix<double,0,int> > As_pred( nindv);
std::vector< Eigen::VectorXd > Ys( nindv);
std::vector< Eigen::MatrixXd > pred_ind(nindv);
std::vector< Eigen::MatrixXd > obs_ind(nindv);
std::vector< Eigen::MatrixXd > Bfixed_pred(nindv);
std::vector< Eigen::MatrixXd > Brandom_pred(nindv);
int count;
count = 0;
for( List::iterator it = obs_list.begin(); it != obs_list.end(); ++it ) {
List obs_tmp = Rcpp::as<Rcpp::List>(*it);
As[count] = Rcpp::as<Eigen::SparseMatrix<double,0,int> >(obs_tmp["A"]);
As_pred[count] = Rcpp::as<Eigen::SparseMatrix<double,0,int> >(obs_tmp["Apred"]);
pred_ind[count] = Rcpp::as<Eigen::MatrixXd>(obs_tmp["pred_ind"]);
obs_ind[count] = Rcpp::as<Eigen::MatrixXd>(obs_tmp["obs_ind"]);
Ys[count] = Rcpp::as<Eigen::VectorXd>(obs_tmp["Y"]);
Brandom_pred[count] = Rcpp::as<Eigen::MatrixXd>(obs_tmp["Brandom_pred"]);
Bfixed_pred[count] = Rcpp::as<Eigen::MatrixXd>(obs_tmp["Bfixed_pred"]);
count++;
}
//**********************************
//operator setup
//***********************************
//if(silent == 0){
// Rcpp::Rcout << " Setup operator\n";
//}
Rcpp::List operator_list = Rcpp::as<Rcpp::List> (in_list["operator_list"]);
std::string type_operator = Rcpp::as<std::string>(operator_list["type"]);
operator_list["nIter"] = 1;
operatorMatrix* Kobj;
//Kobj = new MaternMatrixOperator;
operator_select(type_operator, &Kobj);
Kobj->initFromList(operator_list, List::create(Rcpp::Named("use.chol") = 1));
Eigen::VectorXd h = Rcpp::as<Eigen::VectorXd>( operator_list["h"]);
//Prior solver
cholesky_solver Qsolver;
Qsolver.init( Kobj->d, 0, 0, 0);
Qsolver.analyze( Kobj->Q);
Qsolver.compute( Kobj->Q);
//Create solvers for each patient
std::vector< cholesky_solver > Solver( nindv);
Eigen::SparseMatrix<double, 0, int> Q;
count = 0;
for( List::iterator it = obs_list.begin(); it != obs_list.end(); ++it ) {
List obs_tmp = Rcpp::as<Rcpp::List>( *it);
Solver[count].init(Kobj->d, 0, 0, 0);
Q = Eigen::SparseMatrix<double,0,int>(Kobj->Q.transpose());
Q = Q * Kobj->Q;
Q = Q + As[count].transpose()*As[count];
Solver[count].analyze(Q);
Solver[count].compute(Q);
count++;
}
//**********************************
// mixed effect setup
//***********************************
//if(silent == 0){
// Rcpp::Rcout << " Setup mixed effect\n";
//}
Rcpp::List mixedEffect_list = Rcpp::as<Rcpp::List> (in_list["mixedEffect_list"]);
std::string type_mixedEffect = Rcpp::as<std::string> (mixedEffect_list["noise"]);
MixedEffect *mixobj;
if(type_mixedEffect == "Normal")
mixobj = new NormalMixedEffect;
else
mixobj = new NIGMixedEffect;
mixobj->initFromList(mixedEffect_list);
//**********************************
// measurement setup
//***********************************
//if(silent == 0){
// Rcpp::Rcout << " Setup noise\n";
//}
MeasurementError *errObj;
Rcpp::List measurementError_list = Rcpp::as<Rcpp::List> (in_list["measurementError_list"]);
std::string type_MeasurementError= Rcpp::as <std::string> (measurementError_list["noise"]);
if(type_MeasurementError == "Normal")
errObj = new GaussianMeasurementError;
else
errObj = new NIGMeasurementError;
errObj->initFromList(measurementError_list);
//**********************************
// stochastic processes setup
//***********************************
//if(silent == 0){
// Rcpp::Rcout << " Setup process\n";
//}
Rcpp::List processes_list = Rcpp::as<Rcpp::List> (in_list["processes_list"]);
Rcpp::List V_list = Rcpp::as<Rcpp::List> (processes_list["V"]);
std::string type_processes = Rcpp::as<std::string> (processes_list["noise"]);
Process *process;
if (type_processes != "Normal"){
process = new GHProcess;
}else{ process = new GaussianProcess;}
process->initFromList(processes_list, h);
/*
Simulation objects
*/
std::mt19937 random_engine;
std::normal_distribution<double> normal;
std::default_random_engine gammagenerator;
random_engine.seed(std::chrono::high_resolution_clock::now().time_since_epoch().count());
gig rgig;
rgig.seed(std::chrono::high_resolution_clock::now().time_since_epoch().count());
Eigen::VectorXd z;
z.setZero(Kobj->d);
Eigen::VectorXd b, Ysim;
b.setZero(Kobj->d);
std::vector<int> longInd;
for (int i=0; i< nindv; i++) longInd.push_back(i);
Eigen::SparseMatrix<double,0,int> K = Eigen::SparseMatrix<double,0,int>(Kobj->Q);
K *= sqrt(Kobj->tau);
std::vector< Eigen::MatrixXd > WVec(nindv);
std::vector< Eigen::MatrixXd > XVec(nindv);
for(int i = 0; i < nindv; i++ ) {
if(silent == 0){
Rcpp::Rcout << " Compute prediction for patient " << i << "\n";
}
XVec[i].resize(As_pred[i].rows(), nSim);
WVec[i].resize(As_pred[i].rows(), nSim);
Eigen::MatrixXd random_effect = mixobj->Br[i];
Eigen::MatrixXd fixed_effect = mixobj->Bf[i];
for(int ipred = 0; ipred < pred_ind[i].rows(); ipred++){
//if(silent == 0){
// Rcpp::Rcout << " location = " << ipred << ": "<< obs_ind[i](ipred,0)<< " " << obs_ind[i](ipred,1) << "\n";
// Rcpp::Rcout << " A size = " << As[i].rows() << " " << As[i].cols() << "\n";
// Rcpp::Rcout << " Y size = " << Ys[i].size() << ", re = " << random_effect.rows() << ", fe = "<< fixed_effect.size() << "\n";
//}
//extract data to use for prediction:
Eigen::SparseMatrix<double,0,int> A = As[i].middleRows(obs_ind[i](ipred,0),obs_ind[i](ipred,1));
Eigen::VectorXd Y = Ys[i].segment(obs_ind[i](ipred,0),obs_ind[i](ipred,1));
mixobj->Br[i] = random_effect.middleRows(obs_ind[i](ipred,0),obs_ind[i](ipred,1));
mixobj->Bf[i] = fixed_effect.middleRows(obs_ind[i](ipred,0),obs_ind[i](ipred,1));
//Rcpp::Rcout << "here1111\n";
for(int ii = 0; ii < nSim + nBurnin; ii ++){
//Rcpp::Rcout << "iter = " << ii << "\n";
Eigen::VectorXd res = Y;
//***************************************
//***************************************
// building the residuals and sampling
//***************************************
//***************************************
// removing fixed effect from Y
mixobj->remove_cov(i, res);
res -= A * process->Xs[i];
//***********************************
// mixobj sampling
//***********************************
//Rcpp::Rcout << "here2\n";
if(type_MeasurementError == "Normal")
mixobj->sampleU( i, res, 2 * log(errObj->sigma));
else
//errObj->Vs[i] = errObj->Vs[i].head(ipred+1);
mixobj->sampleU2( i,
res,
errObj->Vs[i].segment(obs_ind[i](ipred,0),obs_ind[i](ipred,1)).cwiseInverse(),
2 * log(errObj->sigma));
mixobj->remove_inter(i, res);
//***********************************
// sampling processes
//***********************************
//Rcpp::Rcout << "here3\n";
Eigen::SparseMatrix<double,0,int> Q = Eigen::SparseMatrix<double,0,int>(K.transpose());
Eigen::VectorXd iV(process->Vs[i].size());
iV.array() = process->Vs[i].array().inverse();
Q = Q * iV.asDiagonal();
Q = Q * K;
for(int j =0; j < Kobj->d; j++)
z[j] = normal(random_engine);
res += A * process->Xs[i];
//Sample X|Y, V, sigma
if(type_MeasurementError == "Normal")
process->sample_X(i,
z,
res,
Q,
K,
A,
errObj->sigma,
Solver[i]);
else
process->sample_Xv2( i,
z,
res,
Q,
K,
A,
errObj->sigma,
Solver[i],
errObj->Vs[i].segment(obs_ind[i](ipred,0),obs_ind[i](ipred,1)).cwiseInverse());
res -= A * process->Xs[i];
//if(res.cwiseAbs().sum() > 1e16){
// throw("res outof bound\n");
//}
// sample V| X
process->sample_V(i, rgig, K);
//***********************************
// random variance noise sampling
//***********************************
//Rcpp::Rcout << "here4\n";
if(type_MeasurementError != "Normal"){
errObj->sampleV(i, res,obs_ind[i](ipred,0)+obs_ind[i](ipred,1));
}
// save samples
if(ii >= nBurnin){
//if(silent == 0){
// Rcpp::Rcout << " save samples << " << i << ", " << ipred << ", " << ii << "\n";
//}
//Rcpp::Rcout << "here 1\n";
Eigen::SparseMatrix<double,0,int> Ai = As_pred[i];
Ai = Ai.middleRows(pred_ind[i](ipred,0),pred_ind[i](ipred,1));
//Rcpp::Rcout << "here 2\n";
Eigen::VectorXd random_effect = Bfixed_pred[i]*mixobj->beta_fixed;
//Rcpp::Rcout << "here 3\n";
random_effect += Brandom_pred[i]*(mixobj->U.col(i)+mixobj->beta_random);
//Rcpp::Rcout << "here 4\n";
Eigen::VectorXd random_effect_c = random_effect.segment(pred_ind[i](ipred,0),pred_ind[i](ipred,1));
Eigen::VectorXd AX = Ai * process->Xs[i];
//Rcpp::Rcout << "here 5\n";
WVec[i].block(pred_ind[i](ipred,0), ii - nBurnin, pred_ind[i](ipred,1), 1) = AX;
XVec[i].block(pred_ind[i](ipred,0), ii - nBurnin, pred_ind[i](ipred,1), 1) = random_effect_c + AX;
//Rcpp::Rcout << "here 6\n";
}
}
}
}
Rcpp::Rcout << "store results\n";
// storing the results
Rcpp::List out_list;
out_list["XVec"] = XVec;
out_list["WVec"] = WVec;
return(out_list);
}
