//------------------------------------------------------------------------------
void HamiltonMatrix::computeMatrixElements()
{
cout << "Setting up the Hamilton matrix " << endl;
int phase;
int nStates = slaterDeterminants.size();
bitset<BITS> newState;
int i, j, k, l;
double interactionElement;
H = zeros(nStates, nStates);
Ht = zeros(nStates, nStates);
for (int st = 0; st < nStates; st++) {
for (int b = 0; b < (int)interactions.n_rows; b++) {
newState = slaterDeterminants[st];
i = interactions(b, 0);
j = interactions(b, 1);
k = interactions(b, 2);
l = interactions(b, 3);
// Using the two body operator. Mathematics: a+(i)a+(j)a-(l)a-(k)|psi>
phase = 1;
newState = removeParticle(k, newState);
phase *= sign(k, newState);
newState = removeParticle(l, newState);
phase *= sign(l, newState);
newState = addParticle(j, newState);
phase *= sign(j, newState);
newState = addParticle(i, newState);
phase *= sign(i, newState);
if (newState[BITS - 1] != 1) {
interactionElement = interactions(b, 4);
// Searching for the new state to compute the matrix elements.
for (int st2 = 0; st2 < (int)slaterDeterminants.size(); st2++) {
if (newState == slaterDeterminants[st2]) {
H(st, st2) += interactionElement * phase;
break;
}
}
}
}
// One body energies
H(st, st) += spsEnergy(st);
}
#if DEBUG
cout << H << endl;
// // Symmetrizing the Hamilton matrix
// for (int i = 0; i < H.n_rows; i++) {
// for (int j = 0; j < i; j++) {
// H(j, i) = H(i, j);
// }
// }
cout << "Completed generating the Hamilton matrix" << endl;
#endif
}
vertex_data() {
pvec = zeros(D);
weight = zeros(D);
bias = 0;
}
int main(int argc, char *argv[]) {
benchmark_t benchmarks[] = {
(benchmark_t){"sht", setup_sht, execute_sht, finish_sht, 1, 0.0},
#if HAS_PSHT
(benchmark_t){"psht", setup_psht, execute_psht, finish_psht, 1, 0.0},
#endif
{NULL, NULL, NULL, NULL, 0, 0.0}
};
benchmark_t *sht_benchmark = &benchmarks[0];
benchmark_t *psht_benchmark = &benchmarks[1];
double t0, t1, dtmin, tit0, tit1, dt;
int n, i, j, should_run;
benchmark_t *pbench;
#ifdef HAS_PPROF
char profilefile[MAXPATH];
#endif
char *resourcename;
char timestr1[MAXTIME], timestr2[MAXTIME];
char *stats_filename = NULL;
char *stats_mode;
int c;
int got_threads, miniter;
double mintime;
char *resource_path;
/* Parse options */
sht_resourcefile = NULL;
Nside = -1;
miniter = 1;
mintime = 0;
opterr = 0;
sht_nmaps = 1;
do_ffts = -1;
sht_flags = WAVEMOTH_MEASURE;
while ((c = getopt (argc, argv, "r:N:j:n:t:S:k:a:o:FEC")) != -1) {
switch (c) {
case 'r':
sht_resourcefile = optarg;
break;
case 'N':
Nside = atoi(optarg);
break;
case 'j':
N_threads = atoi(optarg);
break;
case 'n':
miniter = atoi(optarg);
break;
case 't':
mintime = atof(optarg);
break;
case 'E':
sht_flags &= ~WAVEMOTH_MEASURE;
break;
case 'C':
sht_flags |= WAVEMOTH_NO_RESOURCE_COPY;
break;
case 'a':
stats_filename = optarg;
stats_mode = "a";
break;
case 'o':
stats_filename = optarg;
stats_mode = "w";
break;
case 'S':
sht_m_stride = atoi(optarg);
do_ffts = 0;
break;
case 'F':
do_ffts = 0;
break;
case 'k':
sht_nmaps = atoi(optarg);
break;
}
}
argv += optind;
argc -= optind;
for (pbench = benchmarks; pbench->execute; ++pbench) {
pbench->should_run = (argc == 0);
}
for (j = 0; j != argc; ++j) {
for (pbench = benchmarks; pbench->execute; ++pbench) {
if (strcmp(argv[j], pbench->name) == 0) {
pbench->should_run = 1;
}
}
}
if (do_ffts == -1) do_ffts = 1;
/* Resource configuration */
resource_path = getenv("SHTRESOURCES");
if (sht_resourcefile != NULL) {
wavemoth_configure("");
wavemoth_query_resourcefile(sht_resourcefile, &Nside, &lmax);
} else {
check(resource_path != NULL, "Please define SHTRESOURCES or use -r switch");
wavemoth_configure(resource_path);
lmax = 2 * Nside;
}
fprintf(stderr, "Using %d threads, %d maps, %s, m-thinning %d\n", N_threads, sht_nmaps,
do_ffts ? "with FFTs" : "without FFTs", sht_m_stride);
/* Set up input and output */
size_t npix = 12 * Nside * Nside;
sht_input = gauss_array(((lmax + 1) * (lmax + 2)) / 2 * 2 * sht_nmaps);
sht_output = zeros(npix * sht_nmaps);
psht_output = zeros(npix * sht_nmaps);
/* Wavemoth benchmark */
for (pbench = benchmarks; pbench->execute; ++pbench) {
if (!pbench->should_run) continue;
printf("%s:\n", pbench->name);
pbench->setup();
printf(" Warming up\n");
pbench->execute(NULL);
printf(" Executing\n");
#ifdef HAS_PPROF
snprintf(profilefile, MAXPATH, "profiles/%s.prof", pbench->name);
profilefile[MAXPATH - 1] = '\0';
ProfilerStart(profilefile);
#endif
printf(" ");
pbench->min_time = benchmark(pbench->name, pbench->execute, NULL,
miniter, mintime);
#ifdef HAS_PPROF
ProfilerStop();
#endif
pbench->finish();
}
printf("Runtime sht/psht: %f\n", sht_benchmark->min_time / psht_benchmark->min_time);
printf("Speedup psht/sht: %f\n", psht_benchmark->min_time / sht_benchmark->min_time);
double rho = 0;
for (j = 0; j != sht_nmaps; ++j) {
double z = relative_error(npix, psht_output + j * npix, 1, sht_output + j, sht_nmaps);
rho = fmax(z, rho);
}
printf("Relative error: %e\n", rho);
if (stats_filename != NULL) {
/* Write result to file as a line in the format
nside lmax nmaps nthreads wavemoth_full_time wavemoth_legendre_time psht_time relative_error
*/
FILE *f = fopen(stats_filename, stats_mode);
if (!f) {
fprintf(stderr, "Could not open %s in mode %s\n", stats_filename, stats_mode);
} else {
fprintf(f, "%d %d %d %d %.15e %.15e %.15e %.15e\n",
Nside, lmax, sht_nmaps, N_threads,
sht_benchmark->min_time, min_legendre_dt, psht_benchmark->min_time,
rho);
fclose(f);
}
}
free(psht_output);
free(sht_output);
free(sht_input);
}
Matrix cgs( Matrix& A, Matrix& b, Matrix& x0, double tol )
{
//Check matrix and right hand side vector inputs have appropriate sizes
int m = size( A, 1 ); //number of rows of A
int n = size( A, 2 ); //number of columns of A
if( m != n ) //if A is not square
{
exit(1);
}
if( size( b, 1 ) != n || size( b, 2) != 1 ) //if b does not match A or is not a column vector
{
exit(1);
}
if( size( x0, 1 ) != m || size( x0, 2 ) != 1)//if x0 does not match A or is not a column vector
{
exit(1);
}
if( !iscolumn( b ) )
{
exit(1);
}
// Assign default values to unspecified parameters
//minimum tolerance is 1e-6
if( tol > 1e-6 )
{
tol = 1e-6;
}
int maxit = n < 20 ? n:20; //maximum iterations are 20
Matrix x = x0; //initial guess x0
// Check for all zero right hand side vector => all zero solution
double n2b = norm(b); // Norm of rhs vector, b
if( fabs( n2b ) <= DBL_EPSILON ) // if rhs vector is all zeros
{
x = zeros( n, 1 ); // then solution is all zeros
return x;
}
// Set up for the method
int flag = 1;
Matrix xmin = x; // Iterate which has minimal residual so far
int imin = 0; // Iteration at which xmin was computed
double tolb = tol * n2b; // Relative tolerance
Matrix r = b - A*x; // Residual vector
double normr = norm(r); // Norm of residual
double normr_act = normr;
// other column vectors needed
Matrix u;
Matrix p;
Matrix q;
Matrix vh;
Matrix rtvh;
Matrix uh;
Matrix qh;
if( normr <= tolb ) // Initial guess is a good enough solution
{
return x;
}
Matrix rt = r; // Shadow residual
Matrix resvec = zeros(maxit+1,1); // Preallocate vector for norms of residuals
resvec(1) = normr; // resvec(1) = norm(b-A*x0)
double normrmin = normr; // Norm of residual from xmin
Matrix rho; //double rho = 1;
rho = 1.0;
Matrix rho1;
double alpha;
double beta;
int stag = 0; // stagnation of the method
int moresteps = 0;
int maxmsteps = (n/50) < 5 ? (n/50):5; // maxmsteps is min( floor(n/50), 5, n-maxit )
maxmsteps = maxmsteps < n-maxit ? maxmsteps:n-maxit;
int maxstagsteps = 3;
// loop over maxit iterations (unless convergence or failure)
for( int i = 1; i <= maxit; i++ )
{
rho1 = rho;
rho = tr( rt )*r;
if( fabs( rho(1) ) <= DBL_EPSILON || fabs( rho(1) ) >= DBL_MAX )
{
flag = 4;
break;
}
if( i == 1 )
{
u = r;
p = u;
}
else
{
beta = rho(1) / rho1(1);
if( fabs( beta ) < DBL_EPSILON || fabs( beta ) > DBL_MAX )
{
flag = 4;
break;
}
u = r + beta * q;
p = u + beta * (q + beta * p);
}
vh = A*p;
rtvh = tr( rt ) * vh;
if( fabs( rtvh( 1 ) ) <= DBL_EPSILON )
{
flag = 4;
break;
}
else
{
alpha = rho(1) / rtvh(1);
}
if( fabs( alpha ) >= DBL_MAX )
{
flag = 4;
break;
}
q = u - alpha * vh;
uh = u+q;
// Check for stagnation of the method
if( fabs(alpha)*norm(uh) < DBL_EPSILON*norm(x) )
{
stag = stag + 1;
}
else
{
stag = 0;
}
x = x + alpha * uh; // form the new iterate
qh = A*uh;
r = r - alpha * qh;
normr = norm(r);
normr_act = normr;
resvec(i+1) = normr;
// check for convergence
if( normr <= tolb || stag >= maxstagsteps || moresteps )
{
r = b - A*x;
normr_act = norm(r);
resvec(i+1,1) = normr_act;
if( normr_act <= tolb )
{
flag = 0;
break;
}
else
{
if( stag >= maxstagsteps && moresteps == 0 )
{
stag = 0;
}
moresteps = moresteps + 1;
}
}
if( normr_act < normrmin ) // update minimal norm quantities
{
normrmin = normr_act;
xmin = x;
}
} // for i = 1 : maxit
if( flag == 0 )
{
//tolerance achieved
return x;
}
else
{
if( flag == 4)
{
std::cerr<<"too small or too large a number encountered."<<std::endl;
}
r = b - A*xmin;
if( norm(r) <= normr_act )
{
x = xmin;
}
return x;
}
}
vertex_data() {
pvec = zeros(D);
}
int init(PhantomState *s)
{
if(!s)
{
ROS_FATAL("Internal error. PhantomState is NULL.");
return -1;
}
node_ = ros::NodeHandlePtr(new ros::NodeHandle);
pnode_ = ros::NodeHandlePtr(new ros::NodeHandle("~"));
pnode_->param(std::string("tf_prefix"), tf_prefix_, std::string(""));
// Vertical displacement from base_link to link_0. Defaults to Omni offset.
pnode_->param(std::string("table_offset"), table_offset_, .135);
// Force feedback damping coefficient
pnode_->param(std::string("damping_k"), damping_k_, 0.001);
// On startup device will generate forces to hold end-effector at origin.
pnode_->param(std::string("locked"), locked_, false);
// Check calibration status on start up and calibrate if necessary.
pnode_->param(std::string("calibrate"), calibrate_, false);
//Frame attached to the base of the phantom (NAME/base_link)
base_link_name_ = "base_link";
//Publish on NAME/pose
std::string pose_topic_name = "pose";
pose_publisher_ = node_->advertise<geometry_msgs::PoseStamped>(pose_topic_name, 100);
//Publish button state on NAME/button
std::string button_topic = "button";
button_publisher_ = node_->advertise<sensable_phantom::PhantomButtonEvent>(button_topic, 100);
//Subscribe to NAME/force_feedback
std::string force_feedback_topic = "force_feedback";
wrench_sub_ = node_->subscribe(force_feedback_topic, 100, &PhantomROS::wrench_callback, this);
//Frame of force feedback (NAME/sensable_origin)
sensable_frame_name_ = "sensable_origin";
for (int i = 0; i < 7; i++)
{
std::ostringstream stream1;
stream1 << "link_" << i;
link_names_[i] = std::string(stream1.str());
}
state_ = s;
state_->buttons[0] = 0;
state_->buttons[1] = 0;
state_->buttons_prev[0] = 0;
state_->buttons_prev[1] = 0;
hduVector3Dd zeros(0, 0, 0);
state_->velocity = zeros;
state_->inp_vel1 = zeros; //3x1 history of velocity
state_->inp_vel2 = zeros; //3x1 history of velocity
state_->inp_vel3 = zeros; //3x1 history of velocity
state_->out_vel1 = zeros; //3x1 history of velocity
state_->out_vel2 = zeros; //3x1 history of velocity
state_->out_vel3 = zeros; //3x1 history of velocity
state_->pos_hist1 = zeros; //3x1 history of position
state_->pos_hist2 = zeros; //3x1 history of position
state_->lock = locked_;
state_->lock_pos = zeros;
state_->hd_cur_transform = hduMatrix::createTranslation(0, 0, 0);
return 0;
}
void AvlIntTree::make_balance(void)
{
if (balance == 2)
{
if (right->balance >= 0)
{
AvlKey tKey = key;
uint tValue = value;
key = right->key;
value = right->value;
right->key = tKey;
right->value = tValue;
AvlIntTree *tNode=right->right;
right->right = right->left;
right->left = left;
left = right;
right = tNode;
balance = left->balance - 1;
left->balance = 1 - left->balance;
/* correct sums (values) */
//value += left->value + (left->key.max - left->key.min + 1);
if (left) { left->value = (left->right) ? left->value - value - zeros() : 0; }
}
else
{
AvlKey tKey = key;
uint tValue = value;
key = right->left->key;
value = right->left->value;
right->left->key = tKey;
right->left->value = tValue;
AvlIntTree *tNode = right->left->right;
right->left->right = right->left->left;
right->left->left = left;
left = right->left;
right->left = tNode;
balance = 0;
right->balance = (left->balance == -1) ? (1) : (0);
left->balance = (left->balance == 1) ? (-1) : (0);
/* correct sums (value) */
//right->value = right->left->value + (right->left->key.max - right->left->key.min + 1);
//value = left->value + (left->key.max - left->key.min + 1);
int tmp;
tmp = left->value - right->value - right->zeros() - value - zeros(); // a
value = right->value + right->zeros() + value; // c
left->value = tmp;
}
}
else
{
if (balance == -2)
{
if (left->balance <= 0)
{
AvlKey tKey = key;
uint tValue = value;
key = left->key;
value = left->value;
left->key = tKey;
left->value = tValue;
AvlIntTree *tNode = left->left;
left->left = left->right;
left->right = right;
right = left;
left = tNode;
balance = 1 + right->balance;
right->balance = -1 - right->balance;
/* correct sums (values) */
//right->value = right->left->value + (right->left->key.max - right->left->key.min + 1);
if (right) { right->value = (right->right) ? value - right->value - right->zeros() : 0; }
}
else
{
AvlKey tKey = key;
uint tValue = value;
key = left->right->key;
value = left->right->value;
left->right->key = tKey;
left->right->value = tValue;
AvlIntTree *tNode = left->right->left;
left->right->left = left->right->right;
left->right->right = right;
right = left->right;
left->right = tNode;
balance = 0;
left->balance = (right->balance == 1) ? (-1) : (0);
right->balance = (right->balance == -1) ? (1) : (0);
/* correct sums (values) */
//right->value = right->left->value + (right->left->key.max - right->left->key.min + 1);
//value = left->value + (left->key.max - left->key.min + 1);
int tmp = left->value - value - zeros();
value = right->zeros() + right->value + value;
left->value = tmp;
}
}
}
//value = (right) ? right->value + right->zeros() : 0;
}
// Given a vector of steer values, calculate the lean values associated with
// static equilibrium. Also, return the indices of the steer/lean vectors
// which most nearly cause the u5^2 coefficient to go to zero.
static int staticEq(gsl_vector * lean, gsl_vector * pitch,
const gsl_vector * steer, Whipple * bike)
{
int i, N = lean->size, iter, iter_max = ITER_MAX, status;
double ftol = FTOL;
gsl_vector * x = gsl_vector_alloc(2); // vector to store the solution
gsl_vector * u5s_coefs = zeros(steer->size);
const gsl_multiroot_fdfsolver_type * T = gsl_multiroot_fdfsolver_newton;
gsl_multiroot_fdfsolver *s = gsl_multiroot_fdfsolver_alloc(T, 2);
gsl_multiroot_function_fdf f = {&static_f, &static_df, &static_fdf, 2, bike};
bike->q1 = bike->q3 = 0.0;
bike->calcPitch();
gsl_vector_set(x, 0, bike->q1);
gsl_vector_set(x, 1, bike->q2);
gsl_multiroot_fdfsolver_set(s, &f, x);
// for loop to loop over all values of steer
for (i = 0; i < N; ++i) {
bike->q3 = gsl_vector_get(steer, i); // steer as a parameter
iter = 0;
do
{
++iter;
status = gsl_multiroot_fdfsolver_iterate(s);
if (status)
iterateError(status, "staticEq()", gsl_vector_get(steer, i));
status = gsl_multiroot_test_residual(s->f, ftol);
} while (status == GSL_CONTINUE && iter < iter_max);
// Increase the tolerance by an order of magnitude to improve convergence
if (iter == iter_max) {
gsl_vector_set(x, 0, gsl_vector_get(lean, i-1));
gsl_vector_set(x, 1, gsl_vector_get(pitch, i-1));
gsl_multiroot_fdfsolver_set(s, &f, x);
increaseftol(&ftol, &i, iter_max, "staticEq()", bike->q3);
continue;
} // if
// Store the lean into the lean vector
gsl_vector_set(lean, i, gsl_vector_get(s->x, 0));
gsl_vector_set(pitch, i, gsl_vector_get(s->x, 1));
// Store the square of the coefficient of the u5^2 term;
gsl_vector_set(u5s_coefs, i, bike->F[10] * bike->F[10]);
ftol = FTOL; // reset the error tolerance
} // for
// Assign a large value to the u5s_coefs vector near steer = 0 and steer = PI
// This ensure the minimum will be near PI/2 where the two boudary curves
// cross
for (i = 0; i < 5; ++i) {
gsl_vector_set(u5s_coefs, i, 10000.0);
gsl_vector_set(u5s_coefs, u5s_coefs->size - 1 - i, 10000.0);
}
// Free dynamically allocated variables
gsl_multiroot_fdfsolver_free(s);
gsl_vector_free(x);
i = gsl_vector_min_index(u5s_coefs);
gsl_vector_free(u5s_coefs);
return i;
} // staticEq()
/**
* Vertex update function - computes the least square step
*/
void update(graphchi_vertex<VertexDataType, EdgeDataType> &vertex, graphchi_context &gcontext) {
//compute only for user nodes
if (vertex.id() >= std::min(M,(uint)end_user) || vertex.id() < (uint)start_user)
return;
vertex_data & vdata = latent_factors_inmem[vertex.id()];
int howmany = (int)(N*knn_sample_percent);
assert(howmany > 0 );
if (vertex.num_outedges() == 0){
mymutex.lock();
users_without_ratings++;
mymutex.unlock();
}
vec distances = zeros(howmany);
ivec indices = ivec::Zero(howmany);
for (int i=0; i< howmany; i++){
indices[i]= -1;
}
std::vector<bool> curratings;
curratings.resize(N);
for(int e=0; e < vertex.num_edges(); e++) {
//no need to calculate this rating since it is given in the training data reference
assert(vertex.edge(e)->vertex_id() - M >= 0 && vertex.edge(e)->vertex_id() - M < N);
curratings[vertex.edge(e)->vertex_id() - M] = true;
}
if (knn_sample_percent == 1.0){
for (uint i=M; i< M+N; i++){
if (curratings[i-M])
continue;
vertex_data & other = latent_factors_inmem[i];
double dist;
if (algo == SVDPP)
svdpp_predict(vdata, other, 0, dist);
else if (algo == BIASSGD)
biassgd_predict(vdata, other, 0, dist);
else if (algo == RBM)
rbm_predict(vdata, other, 0, dist);
else assert(false);
indices[i-M] = i-M;
distances[i-M] = dist + 1e-10;
}
}
else for (int i=0; i<howmany; i++){
int random_other = ::randi(M, M+N-1);
vertex_data & other = latent_factors_inmem[random_other];
double dist;
if (algo == SVDPP)
svdpp_predict(vdata, other, 0, dist);
else if (algo == BIASSGD)
biassgd_predict(vdata, other, 0, dist);
else if (algo == RBM)
rbm_predict(vdata, other, 0, dist);
else assert(false);
indices[i] = random_other-M;
distances[i] = dist;
}
vec out_dist(num_ratings);
ivec indices_sorted = reverse_sort_index2(distances, indices, out_dist, num_ratings);
assert(indices_sorted.size() <= num_ratings);
assert(out_dist.size() <= num_ratings);
vdata.ids = indices_sorted;
vdata.ratings = out_dist;
if (debug)
printf("Closest is: %d with distance %g\n", (int)vdata.ids[0], vdata.ratings[0]);
if (vertex.id() % 1000 == 0)
printf("Computing recommendations for user %d at time: %g\n", vertex.id()+1, mytimer.current_time());
}
// [[Rcpp::export]]
arma::rowvec euler_hat_ds_alt(
arma::rowvec exog, arma::rowvec endog, arma::rowvec cont,
arma::mat exog_innov_integ, double betta,
double gamma, arma::mat coeffs_cont,
int n_exog, int n_endog, int n_cont, int n_fwd,
arma::rowvec rho, int n_integ, int N, arma::rowvec upper,
arma::rowvec lower, bool cheby, arma::rowvec weights,
bool print_rhs=false ){
// Computes the single-period error on the Euler equations
// double betta = params["betta"] ;
// double gamma = params["gamma"] ;
double rho_pref = - std::log( betta ) ;
// Extract coefficients
double NFA = endog( 0 ) ;
double z1 = endog( 1 ) ;
double z2 = endog( 2 ) ;
// Extract the endogenous states
double c1 = cont(0) ;
double c2 = cont(1) ;
double q = cont(15) ;
double af1 = cont(16) ;
// Extract controls
mat exog_lead = zeros( n_integ, n_exog ) ;
// Initalize the draws of the exogenous variables in the next period
exog_lead = ones(n_integ) * ( rho % exog ) + exog_innov_integ ;
// Multiply the most recent exogenous draw by the appropriate rho and
// add the innovation
rowvec integral = zeros<rowvec>( n_fwd ) ;
mat integrand = zeros( n_integ, n_fwd ) ;
// Initialize the right hand side.
// Add the extra two columns for the extra equations for the prices
// r1 and r2
for( int i = 0 ; i < n_integ ; i++ ){
integrand.row(i) = integrand_ds( endog, exog_lead.row(i), gamma,
coeffs_cont, n_exog, n_endog,
n_cont, N, upper, lower, cheby ) ;
} // Compute the integral
integral = weights * integrand ;
if( print_rhs ){
Rcout << "err: \n" << integrand << std::endl ;
Rcout << "weights:" << weights << std::endl ;
Rcout << "integral: " << integral << std::endl ;
Rcout << "integral(0) - 1 = " << integral(0) - 1 << std::endl ;
}
rowvec out(4) ;
out(0) = z1 + ( gamma * c1 - rho_pref + std::log( integral(0) ) ) ;
out(1) = z2 - ( gamma * c1 - rho_pref + std::log( integral(1) ) ) ;
out(2) = af1 + ( gamma * c2 - rho_pref + std::log( integral(2) ) ) ;
out(3) = q + ( gamma * c2 - rho_pref + std::log( integral(3) ) ) ;
// The predictors. Set up z1, Z_2 s.t. if current consumption is too high
// for the Euler equation to hold then z1 decreases. This will pull down
// on consumption in the contemporaneous block later. Similarly, for af1:
// if the expected return on asset 2 is too high then the investment share
// in asset 1 declines. Finally, if q(+1) is too high
return out ;
// Return predictors for (B11, B22, r1, r2)
}
void Controller::run()
{
mutexRun.lock();
string event="none";
// verify if any saccade is still underway
mutexCtrl.lock();
if (commData->get_isSaccadeUnderway() && (Time::now()-saccadeStartTime>=Ts))
{
if (!tiltDone)
posHead->checkMotionDone(eyesJoints[0],&tiltDone);
if (!panDone)
posHead->checkMotionDone(eyesJoints[1],&panDone);
if (!verDone)
posHead->checkMotionDone(eyesJoints[2],&verDone);
commData->get_isSaccadeUnderway()=!(tiltDone&&panDone&&verDone);
if (!commData->get_isSaccadeUnderway())
notifyEvent("saccade-done");
}
mutexCtrl.unlock();
// get data
double x_stamp;
Vector xd=commData->get_xd();
Vector x=commData->get_x(x_stamp);
Vector new_qd=commData->get_qd();
// read feedbacks
q_stamp=Time::now();
if (Robotable)
{
if (!getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData,&q_stamp))
{
printf("\nCommunication timeout detected!\n\n");
notifyEvent("comm-timeout");
suspend();
mutexRun.unlock();
return;
}
IntState->reset(fbHead);
}
fbNeck=fbHead.subVector(0,2);
fbEyes=fbHead.subVector(3,5);
double errNeck=norm(new_qd.subVector(0,2)-fbNeck);
double errEyes=norm(new_qd.subVector(3,new_qd.length()-1)-fbEyes);
bool swOffCond=(Time::now()-ctrlActiveRisingEdgeTime<GAZECTRL_SWOFFCOND_DISABLETIME) ? false :
(!commData->get_isSaccadeUnderway() &&
(errNeck<GAZECTRL_MOTIONDONE_NECK_QTHRES*CTRL_DEG2RAD) &&
(errEyes<GAZECTRL_MOTIONDONE_EYES_QTHRES*CTRL_DEG2RAD));
// verify control switching conditions
if (commData->get_isCtrlActive())
{
// switch-off condition
if (swOffCond)
{
event="motion-done";
mutexCtrl.lock();
stopLimb(false);
mutexCtrl.unlock();
}
// manage new target while controller is active
else if (port_xd->get_new())
{
event="motion-onset";
mutexData.lock();
motionOngoingEventsCurrent=motionOngoingEvents;
mutexData.unlock();
}
port_xd->get_new()=false;
}
else
{
// inhibition is cleared upon new target arrival
if (ctrlInhibited)
ctrlInhibited=!port_xd->get_new();
// switch-on condition
commData->get_isCtrlActive()=port_xd->get_new() ||
(!ctrlInhibited &&
(new_qd[0]!=qd[0]) || (new_qd[1]!=qd[1]) || (new_qd[2]!=qd[2]) ||
(!commData->get_canCtrlBeDisabled() && (norm(port_xd->get_xd()-x)>GAZECTRL_MOTIONSTART_XTHRES)));
// reset controllers
if (commData->get_isCtrlActive())
{
ctrlActiveRisingEdgeTime=Time::now();
port_xd->get_new()=false;
Vector zeros(3,0.0);
mjCtrlNeck->reset(zeros);
mjCtrlEyes->reset(zeros);
IntPlan->reset(fbNeck);
event="motion-onset";
mutexData.lock();
motionOngoingEventsCurrent=motionOngoingEvents;
mutexData.unlock();
}
}
if (event=="motion-onset")
q0deg=CTRL_RAD2DEG*fbHead;
qd=new_qd;
qdNeck=qd.subVector(0,2);
qdEyes=qd.subVector(3,5);
if (commData->get_isCtrlActive())
{
// control loop
vNeck=mjCtrlNeck->computeCmd(neckTime,qdNeck-fbNeck);
IntPlan->integrate(vNeck);
if (unplugCtrlEyes)
{
if (Time::now()-saccadeStartTime>=Ts)
vEyes=commData->get_counterv();
}
else
vEyes=mjCtrlEyes->computeCmd(eyesTime,qdEyes-fbEyes)+commData->get_counterv();
}
else
{
vNeck=0.0;
vEyes=0.0;
}
v.setSubvector(0,vNeck);
v.setSubvector(3,vEyes);
// apply bang-bang just in case to compensate
// for unachievable low velocities
if (Robotable)
{
for (size_t i=0; i<v.length(); i++)
if ((v[i]!=0.0) && (v[i]>-minAbsVel) && (v[i]<minAbsVel))
v[i]=iCub::ctrl::sign(qd[i]-fbHead[i])*minAbsVel;
}
// convert to degrees
mutexData.lock();
qddeg=CTRL_RAD2DEG*qd;
qdeg =CTRL_RAD2DEG*fbHead;
vdeg =CTRL_RAD2DEG*v;
mutexData.unlock();
// send commands to the robot
if (Robotable && commData->get_isCtrlActive())
{
mutexCtrl.lock();
if (neckPosCtrlOn)
{
Vector posdeg=(CTRL_RAD2DEG)*IntPlan->get();
posNeck->setPositions(neckJoints.size(),neckJoints.getFirst(),posdeg.data());
velEyes->velocityMove(eyesJoints.size(),eyesJoints.getFirst(),vdeg.subVector(3,5).data());
}
else
velHead->velocityMove(vdeg.data());
mutexCtrl.unlock();
}
// print info
printIter(xd,x,qddeg,qdeg,vdeg,1.0);
// send x,q through YARP ports
Vector q(nJointsTorso+nJointsHead);
int j;
for (j=0; j<nJointsTorso; j++)
q[j]=CTRL_RAD2DEG*fbTorso[j];
for (; (size_t)j<q.length(); j++)
q[j]=qdeg[j-nJointsTorso];
txInfo_x.update(x_stamp);
if (port_x.getOutputCount()>0)
{
port_x.setEnvelope(txInfo_x);
port_x.write(x);
}
txInfo_q.update(q_stamp);
if (port_q.getOutputCount()>0)
{
port_q.setEnvelope(txInfo_q);
port_q.write(q);
}
// update pose information
mutexChain.lock();
for (int i=0; i<nJointsTorso; i++)
{
chainNeck->setAng(i,fbTorso[i]);
chainEyeL->setAng(i,fbTorso[i]);
chainEyeR->setAng(i,fbTorso[i]);
}
for (int i=0; i<3; i++)
{
chainNeck->setAng(nJointsTorso+i,fbHead[i]);
chainEyeL->setAng(nJointsTorso+i,fbHead[i]);
chainEyeR->setAng(nJointsTorso+i,fbHead[i]);
}
chainEyeL->setAng(nJointsTorso+3,fbHead[3]); chainEyeR->setAng(nJointsTorso+3,fbHead[3]);
chainEyeL->setAng(nJointsTorso+4,fbHead[4]+fbHead[5]/2.0); chainEyeR->setAng(nJointsTorso+4,fbHead[4]-fbHead[5]/2.0);
txInfo_pose.update(q_stamp);
mutexChain.unlock();
if (event=="motion-onset")
notifyEvent(event);
motionOngoingEventsHandling();
if (event=="motion-done")
{
motionOngoingEventsFlush();
notifyEvent(event);
}
// update joints angles
fbHead=IntState->integrate(v);
commData->set_q(fbHead);
commData->set_torso(fbTorso);
commData->set_v(v);
mutexRun.unlock();
}
void matrix::block_mult(const matrix & B, matrix & C, int BlockX, int BlockY, int BlockZ) const {
int i, j, k, ii, jj, kk, Ax, Ay, Bx, By, Cx, Cy; i = 0; j = 0; k = 0; ii = 0; jj = 0; kk = 0;
Ax = 0; Ay = 0; Bx = 0; By = 0; Cx = 0; Cy = 0;
int Nx, Ny, Nz; Nx = 0; Ny = 0; Nz = 0;
fpp temp; temp = 0.0;
submatrix SubA, SubB, SubC;
matrix CacheA, CacheB, CacheC;
//std::cout << "Matrix mult called!" <<std::endl;
Ax = this->get_rows(); Ay = this->get_cols();
Bx = B.get_rows(); By = B.get_cols();
Cx = C.get_rows(); Cy = C.get_cols();
if ((Ay != Bx) || (By != Cy) || (Ax != Cx)){
std::cerr << "Matrix multiplication failed! Dimension incompatibility. A(" << Ax << "," << Ay << "), B(" << Bx << "," << By << "), C(" << Cx << "," << Cy << ")." << std::endl;
exit(-1);
}
Nx = Cx/BlockX; Ny = Cy/BlockY; Nz = Ay/BlockZ;
int x_fudge, y_fudge, z_fudge; x_fudge = 1; y_fudge = 1; z_fudge = 1;
if (Cx % BlockX == 0){
x_fudge = 0;
}
if (Cy % BlockY == 0){
y_fudge = 0;
}
if (Ay % BlockZ == 0){
z_fudge = 0;
}
for (i = 0; i < Nx + x_fudge; i++){
for (j = 0; j < Ny + y_fudge; j++){
// Set Up Submatrix C
if((i+1)*BlockX < Cx){
if((j+1)*BlockY < Cy){
SubC.subcreate(C, i*BlockX, j*BlockY, BlockX, BlockY);
} else {
SubC.subcreate(C, i*BlockX, j*BlockY, BlockX, Cy - j*BlockY);
}
} else {
if((j+1)*BlockY < Cy){
SubC.subcreate(C, i*BlockX, j*BlockY, Cx - i*BlockX, BlockY);
} else {
SubC.subcreate(C, i*BlockX, j*BlockY, Cx - i*BlockX, Cy - j*BlockY);
}
}
CacheC = SubC;
zeros(CacheC);
for(k = 0; k < Nz + z_fudge; k++){
if((i+1)*BlockX < Ax){
if((k+1)*BlockZ < Ay){
SubA.subcreate(*this, i*BlockX, k*BlockZ, BlockX, BlockZ);
} else {
SubA.subcreate(*this, i*BlockX, k*BlockZ, BlockX, Ay - k*BlockZ);
}
} else {
if((k+1)*BlockZ < Ay){
SubA.subcreate(*this, i*BlockX, k*BlockZ, Ax - i*BlockX, BlockZ);
} else {
SubA.subcreate(*this, i*BlockX, k*BlockZ, Ax - i*BlockX, Ay - k*BlockZ);
}
}
CacheA = SubA;
if((j+1)*BlockY < By){
if((k+1)*BlockZ < Bx){
SubB.subcreate(B, k*BlockZ, j*BlockY, BlockZ, BlockY);
} else {
SubB.subcreate(B, k*BlockZ, j*BlockY, Bx - k*BlockZ, BlockY);
}
} else {
if((k+1)*BlockZ < Bx){
SubB.subcreate(B, k*BlockZ, j*BlockY, BlockZ, By - j*BlockY);
} else {
SubB.subcreate(B, k*BlockZ, j*BlockY, Bx - k*BlockZ, By - j*BlockY);
}
}
CacheB = SubB;
for(ii = 0; ii < CacheA.get_rows(); ii++){
for(jj = 0; jj < CacheB.get_cols(); jj++){
for(kk = 0; kk < CacheA.get_cols(); kk++){
CacheC(ii,jj) += CacheA(ii,kk)*CacheB(kk,jj);
}
}
}
}
SubC = CacheC;
}
}
}
SegImage* MeanFiller::fillDynamic(int startX, int startY, int startZ, int startRadius)
{
int cols, rows, slices;
int minx, miny, minz, maxx, maxy, maxz;
cube sample, xes;
vec values;
stack<triple> historyEntity;
image->getSize(cols, rows, slices);
cube result = zeros(rows, cols, slices);
cube sphere = Utils::sphere(startRadius);
result(startY, startX, startZ, arma::size(sphere)) = sphere;
res_image = Utils::convert(result);
Utils::bounds(result, minx, miny, minz, maxx, maxy, maxz);
double thres;
history.clear();
result.reset();
if (minx < 3) minx = 3;
if (maxx > cols - 2) maxx = cols - 2;
if (miny < 3) miny = 3;
if (maxy > rows - 2) maxy = rows - 2;
if (minz < 3) minz = 3;
if (maxz > slices - 2) maxz = slices - 2;
bool flag = false;
while (!flag)
{
flag = true;
xes = Utils::convert_d(image->getRegion(minx, miny, maxx, maxy, minz, maxz));
sample = wBright * Utils::convert_d(res_image->getRegion(minx, miny, maxx, maxy, minz, maxz)) +
wCurv * xes;
sample = Utils::conv3(Utils::conv3(sample, kernel), kernel);
sample %= -xes + 1;
values = sort(nonzeros(vectorise(sample)));
thres = values(quantile * values.size());
for (int i = minx; i < maxx; i++)
{
for (int j = miny; j < maxy; j++)
{
for (int k = minz; k < maxz; k++)
{
if (image->getVoxel(i, j, k) > 0 && res_image->getVoxel(i, j, k) == 0)
{
if (sample(j-miny, i - minx, k - minz) > thres)
{
res_image->setVoxel(i, j, k, 255);
if (i <= minx && i >= 4)
minx = i - 1;
if (i >= maxx && i <= cols - 4 && maxx < i + 1)
maxx = i + 1;
if (j <= miny && j >= 4)
miny = j - 1;
if (j >= maxy && j <= rows - 4 && maxy < j + 1)
maxy = j + 1;
if (k <= minz && k >= 4)
minz = k - 1;
if (k >= maxz && k <= slices - 4 && maxz < k + 1)
maxz = k + 1;
historyEntity.push(triple(i, j, k));
flag = false;
}
}
}
}
}
if (!flag) {
if (history.size() >= h_size)
{
stack<triple> first = history.back();
while (!first.empty())
{
first.pop();
}
history.pop_back();
}
history.push_front(historyEntity);
}
}
return res_image;
}
SegImage* MeanFiller::fill(int startX, int startY, int startZ, int startRadius)
{
timer.start("Fill");
int cols, rows, slices;
stack<triple> historyEntity;
cube sample;
image->getSize(cols, rows, slices);
cube result = zeros(rows, cols, slices);
cube sphere = Utils::sphere(startRadius);
result(startY, startX, startZ, arma::size(sphere)) = sphere;
timer.stage("Init");
res_image = Utils::convert(result);
timer.stage("Convert");
Utils::bounds(result, minx, miny, minz, maxx, maxy, maxz);
timer.stage("Bounds");
result.reset();
history.clear();
if (minx < 3) minx = 3;
if (maxx > cols - 2) maxx = cols - 2;
if (miny < 3) miny = 3;
if (maxy > rows - 2) maxy = rows - 2;
if (minz < 3) minz = 3;
if (maxz > slices - 2) maxz = slices - 2;
bool flag = false;
while(!flag)
{
historyEntity = stack<triple>();
flag = true;
for (int i = minx; i <= maxx; i++)
{
for (int j = miny; j <= maxy; j++)
{
for (int k = minz; k <= maxz; k++)
{
if (image->getVoxel(i,j,k) > 0 && res_image->getVoxel(i,j,k) == 0)
{
sample = wBright * Utils::convert_d(image->getRegion(i - 2, j - 2, i + 2, j + 2, k - 2, k + 2)) +
wCurv * Utils::convert_d(res_image->getRegion(i - 2, j - 2, i + 2, j + 2, k - 2, k + 2));
double der = derivate(sample);
if (der > threshold)
{
res_image->setVoxel(i, j, k, 255);
if (i <= minx && i >= 4 && minx > i - 1)
minx = i - 1;
if (i >= maxx && i <= cols - 4 && maxx < i + 1)
maxx = i + 1;
if (j <= miny && j >= 4 && miny > j - 1)
miny = j - 1;
if (j >= maxy && j <= rows - 4 && maxy < j + 1)
maxy = j + 1;
if (k <= minz && k >= 4 && minz > k - 1)
minz = k - 1;
if (k >= maxz && k <= slices - 4 && minz < k + 1)
maxz = k + 1;
historyEntity.push(triple(i, j, k));
flag = false;
}
}
}
}
}
timer.stage("Full cycle");
if (!flag) {
if (history.size() >= h_size)
{
stack<triple> first = history.back();
while (!first.empty())
{
first.pop();
}
history.pop_back();
}
history.push_front(historyEntity);
}
}
timer.stop();
return res_image;
}
void before_iteration(int iteration, graphchi_context & gcontext){
last_validation_rmse = dvalidation_rmse;
validation_rmse_vec = zeros(num_threads);
}
int main(int argc, const char ** argv) {
mytimer.start();
print_copyright();
/* GraphChi initialization will read the command line
arguments and the configuration file. */
graphchi_init(argc, argv);
/* Metrics object for keeping track of performance counters
and other information. Currently required. */
metrics m("rating2");
knn_sample_percent = get_option_float("knn_sample_percent", 1.0);
if (knn_sample_percent <= 0 || knn_sample_percent > 1)
logstream(LOG_FATAL)<<"Sample percente should be in the range (0, 1] " << std::endl;
num_ratings = get_option_int("num_ratings", 10);
if (num_ratings <= 0)
logstream(LOG_FATAL)<<"num_ratings, the number of recomended items for each user, should be >=1 " << std::endl;
debug = get_option_int("debug", 0);
tokens_per_row = get_option_int("tokens_per_row", tokens_per_row);
std::string algorithm = get_option_string("algorithm");
/* Basic arguments for RBM algorithm */
rbm_bins = get_option_int("rbm_bins", rbm_bins);
rbm_scaling = get_option_float("rbm_scaling", rbm_scaling);
if (algorithm == "svdpp" || algorithm == "svd++")
algo = SVDPP;
else if (algorithm == "biassgd")
algo = BIASSGD;
else if (algorithm == "rbm")
algo = RBM;
else logstream(LOG_FATAL)<<"--algorithm should be svd++ or biassgd or rbm"<<std::endl;
parse_command_line_args();
/* Preprocess data if needed, or discover preprocess files */
int nshards = 0;
if (tokens_per_row == 3)
nshards = convert_matrixmarket<edge_data>(training, 0, 0, 3, TRAINING, false);
else if (tokens_per_row == 4)
nshards = convert_matrixmarket4<edge_data4>(training);
else logstream(LOG_FATAL)<<"--tokens_per_row should be either 3 or 4" << std::endl;
assert(M > 0 && N > 0);
latent_factors_inmem.resize(M+N); // Initialize in-memory vertices.
//initialize data structure to hold the matrix read from file
if (algo == RBM){
#pragma omp parallel for
for (uint i=0; i< M+N; i++){
if (i < M){
latent_factors_inmem[i].pvec = zeros(D*3);
}
else {
latent_factors_inmem[i].pvec = zeros(rbm_bins + rbm_bins * D);
}
}
}
read_factors(training);
if ((uint)num_ratings > N){
logstream(LOG_WARNING)<<"num_ratings is too big - setting it to: " << N << std::endl;
num_ratings = N;
}
srand(time(NULL));
/* Run */
if (tokens_per_row == 3){
RatingVerticesInMemProgram<VertexDataType, EdgeDataType> program;
graphchi_engine<VertexDataType, EdgeDataType> engine(training, nshards, false, m);
set_engine_flags(engine);
engine.run(program, 1);
}
else if (tokens_per_row == 4){
RatingVerticesInMemProgram<VertexDataType, edge_data4> program;
graphchi_engine<VertexDataType, edge_data4> engine(training, nshards, false, m);
set_engine_flags(engine);
engine.run(program, 1);
}
/* Output latent factor matrices in matrix-market format */
output_knn_result(training);
rating_stats();
if (users_without_ratings > 0)
logstream(LOG_WARNING)<<"Found " << users_without_ratings << " without ratings. For those users no items are recommended (item id 0)" << std::endl;
if (users_no_ratings > 0)
logstream(LOG_WARNING)<<"Failed to compute ratings for " << users_no_ratings << " Users. For those users no items are recommended (item id 0)" << std::endl;
/* Report execution metrics */
if (!quiet)
metrics_report(m);
return 0;
}
mat Poisson1D::setPhip(mat phin, bool Equilibrum) {
if (Equilibrum == true)
return ( zeros(dev1D.getSumPoint()) ); // see Device1D::chargeDensityArrayFunct
return ( dev1D.getBGArray() - phin - (fLnArray - fLpArray) );
}
function result=multiple(x,y)
global countmul countzero;
accuracy=2^-15;
result=zeros(size(x,1),size(y,2));
for a=1:size(x,1)
for c=1:size(y,2)
for b=1:size(x,2)
tempvalue=x(a,b)*y(b,c);
countmul=countmul+1;
if tempvalue<accuracy
countzero=countzero+1;
%tempvalue=0;
end
result(a,c)=result(a,c)+tempvalue;
end
end
end
end
void write(const star2d &A,char *var,char *fmt) {
int i;
double d;
matrix m,m2,T,T_odd;
if(pole||equator) {
m2=zeros(1,A.nth+pole+equator);
m2.setblock(0,0,equator,A.nth+equator-1,A.th);
if(equator) m2(0)=PI/2;
m=A.map.leg.eval_00(A.th,m2,T);
m=A.map.leg.eval_11(A.th,m2,T_odd);
} else {
T=eye(A.nth);
T_odd=T;
}
if(!strcmp(var,"nr")) {
if(fmt) fprintf(stdout,fmt,A.nr);
else {
i=A.nr;
fwrite(&i,sizeof(int),1,stdout);
}
} else if(!strcmp(var,"ndomains")) {
if(fmt) fprintf(stdout,fmt,A.ndomains);
else {
i=A.ndomains;
fwrite(&i,sizeof(int),1,stdout);
}
} else if(!strcmp(var,"npts")) {
if(fmt) {
for(i=0;i<A.ndomains;i++) {
fprintf(stdout,fmt,*(A.map.gl.npts+i));
if(i<A.ndomains-1) fprintf(stdout,",");
}
} else {
fwrite(A.map.gl.npts,sizeof(int),A.ndomains,stdout);
}
} else if(!strcmp(var,"xif")) {
if(fmt) {
for(i=0;i<A.ndomains+1;i++) {
fprintf(stdout,fmt,*(A.map.gl.xif+i));
if(i<A.ndomains) fprintf(stdout,",");
}
} else {
fwrite(A.map.gl.xif,sizeof(double),A.ndomains+1,stdout);
}
} else if(!strcmp(var,"nth")) {
if(fmt) fprintf(stdout,fmt,A.nth);
else {
i=A.nth;
fwrite(&i,sizeof(int),1,stdout);
}
} else if(!strcmp(var,"nex")) {
if(fmt) fprintf(stdout,fmt,A.nex);
else {
i=A.nex;
fwrite(&i,sizeof(int),1,stdout);
}
} else if(!strcmp(var,"ps")) {
m=A.ps;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"m")) {
if(fmt) fprintf(stdout,fmt,A.m);
else {
fwrite(&A.m,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"surff")) {
if(fmt) fprintf(stdout,fmt,A.surff);
else {
fwrite(&A.surff,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"conv")) {
if(fmt) fprintf(stdout,fmt,A.conv);
else {
fwrite(&A.conv,sizeof(int),1,stdout);
}
} else if(!strcmp(var,"Omega")) {
if(dim) d=A.Omega*A.units.Omega;
else d=A.Omega;
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"Omega_bk")) {
if(fmt) fprintf(stdout,fmt,A.Omega_bk);
else {
fwrite(&A.Omega_bk,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"Omegac")) {
if(dim) d=A.Omegac*A.units.Omega;
else d=A.Omegac;
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"Xc")) {
if(fmt) fprintf(stdout,fmt,A.Xc);
else {
fwrite(&A.Xc,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"rhoc")) {
if(fmt) fprintf(stdout,fmt,A.rhoc);
else {
fwrite(&A.rhoc,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"Tc")) {
if(fmt) fprintf(stdout,fmt,A.Tc);
else {
fwrite(&A.Tc,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"pc")) {
if(fmt) fprintf(stdout,fmt,A.pc);
else {
fwrite(&A.pc,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"R")) {
if(fmt) fprintf(stdout,fmt,A.R);
else {
fwrite(&A.R,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"Rp")) {
if(fmt) fprintf(stdout,fmt,A.R);
else {
fwrite(&A.R,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"Re")) {
d=A.map.leg.eval_00(A.r.row(A.nr-1),PI/2)(0)*A.units.r;
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"M")) {
if(fmt) fprintf(stdout,fmt,A.M);
else {
fwrite(&A.M,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"Lz")) {
d=A.Lz();
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"Mcore")) {
d=A.Mcore();
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"Lzcore")) {
d=A.Lzcore();
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"X")) {
if(fmt) fprintf(stdout,fmt,A.X0);
else {
fwrite(&A.X0,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"Y")) {
if(fmt) fprintf(stdout,fmt,A.Y0);
else {
fwrite(&A.Y0,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"Z")) {
if(fmt) fprintf(stdout,fmt,A.Z0);
else {
fwrite(&A.Z0,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"R/R_SUN")) {
if(fmt) fprintf(stdout,fmt,A.R/R_SUN);
else {
d=A.R/R_SUN;
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"Rp/R_SUN")) {
if(fmt) fprintf(stdout,fmt,A.R/R_SUN);
else {
d=A.R/R_SUN;
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"Re/R_SUN")) {
d=A.map.leg.eval_00(A.r.row(A.nr-1),PI/2)(0)*A.units.r/R_SUN;
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"M/M_SUN")) {
if(fmt) fprintf(stdout,fmt,A.M/M_SUN);
else {
d=A.M/M_SUN;
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"opa")) {
if(fmt) fprintf(stdout,fmt,A.opa.name);
else {
fwrite(A.opa.name,sizeof(char),strlen(A.opa.name)+1,stdout);
}
} else if(!strcmp(var,"eos")) {
if(fmt) fprintf(stdout,fmt,A.eos.name);
else {
fwrite(A.eos.name,sizeof(char),strlen(A.eos.name)+1,stdout);
}
} else if(!strcmp(var,"r")) {
if(dim) m=A.r*A.units.r;
else m=A.r;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"z")) {
if(dim) m=A.z*A.units.r;
else m=A.z;
if(fmt) matrix_fmt(fmt,m);
else m.write(stdout,'b');
} else if(!strcmp(var,"D")) {
if(dim) m=A.D/A.units.r;
else m=A.D;
if(fmt) matrix_fmt(fmt,m);
else m.write(stdout,'b');
} else if(!strcmp(var,"I")) {
if(dim) m=A.map.gl.I*A.units.r;
else m=A.map.gl.I;
if(fmt) matrix_fmt(fmt,m);
else m.write(stdout,'b');
} else if(!strcmp(var,"rex")) {
if(dim) m=A.map.ex.r*A.units.r;
else m=A.map.ex.r;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"phiex")) {
if(dim) m=A.phiex*A.units.phi;
else m=A.phiex;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"Dex")) {
if(dim) m=A.map.ex.D/A.units.r;
else m=A.map.ex.D;
if(fmt) matrix_fmt(fmt,m);
else m.write(stdout,'b');
} else if(!strcmp(var,"th")) {
m=zeros(1,A.nth+pole+equator);
m.setblock(0,0,equator,A.nth-1+pole,A.th);
if(equator) m(0)=PI/2;
if(pole) m(m.ncols()-1)=0;
if(fmt) matrix_fmt(fmt,m);
else m.write(stdout,'b');
} else if(!strcmp(var,"Dt")) {
m2=(A.Dt,T_odd);
m=zeros(A.nth+pole+equator,A.nth+pole+equator);
m.setblock(equator,A.nth+equator-1,0,A.nth+pole+equator-1,m2);
if(fmt) matrix_fmt(fmt,m);
else m.write(stdout,'b');
} else if(!strcmp(var,"Dtodd")) {
m2=(A.map.leg.D_11,T);
m=zeros(A.nth+pole+equator,A.nth+pole+equator);
m.setblock(equator,A.nth+equator-1,0,A.nth+pole+equator-1,m2);
if(fmt) matrix_fmt(fmt,m);
else m.write(stdout,'b');
} else if(!strcmp(var,"Dt2")) {
m2=(A.Dt2,T);
m=zeros(A.nth+pole+equator,A.nth+pole+equator);
m.setblock(equator,A.nth+equator-1,0,A.nth+pole+equator-1,m2);
if(fmt) matrix_fmt(fmt,m);
else m.write(stdout,'b');
} else if(!strcmp(var,"It")) {
m=zeros(A.nth+pole+equator,1);
m.setblock(equator,A.nth+equator-1,0,0,A.map.leg.I_00);
if(fmt) matrix_fmt(fmt,m);
else m.write(stdout,'b');
} else if(!strcmp(var,"Ts")) {
m=A.Ts;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"map.R")) {
if(dim) m=A.map.R.block(1,-1,0,-1)*A.units.r;
else m=A.map.R.block(1,-1,0,-1);
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"rho")) {
if(dim) m=A.rho*A.units.rho;
else m=A.rho;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"phi")) {
if(dim) m=A.phi*A.units.phi;
else m=A.phi;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"p")) {
if(dim) m=A.p*A.units.p;
else m=A.p;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"Xr")) {
m=A.comp.X();
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"Yr")) {
m=A.comp.Y();
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"Zr")) {
m=A.comp.Z();
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strncmp(var,"X_",2)) {
std::string elem(var+2);
if(A.comp.count(elem)) m=A.comp[elem];
else m=zeros(A.nr,A.nth);
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"w")) {
if(dim) m=A.w*A.units.Omega;
else m=A.w;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"T")) {
if(dim) m=A.T*A.units.T;
else m=A.T;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"vr")) {
if(dim) m=A.vr*A.units.v;
else m=A.vr;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"vt")) {
if(dim) m=A.vt*A.units.v;
else m=A.vt;
if(fmt) matrix_fmt(fmt,(m,T_odd));
else (m,T_odd).write(stdout,'b');
} else if(!strcmp(var,"G")) {
if(dim) m=A.G*A.units.v*A.units.r*A.units.rho;
else m=A.G;
if(fmt) matrix_fmt(fmt,(m,T_odd));
else (m,T_odd).write(stdout,'b');
} else if(!strcmp(var,"N2")) {
m=A.N2();
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"opa.k")) {
m=A.opa.k;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"opa.xi")) {
m=A.opa.xi;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"opa.dlnxi_lnT")) {
m=A.opa.dlnxi_lnT;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"opa.dlnxi_lnrho")) {
m=A.opa.dlnxi_lnrho;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"nuc.eps")) {
m=A.nuc.eps;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"nuc.pp")) {
m=A.nuc.pp;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"nuc.cno")) {
m=A.nuc.cno;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"eos.G1")) {
m=A.eos.G1;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"eos.cp")) {
m=A.eos.cp;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"eos.del_ad")) {
m=A.eos.del_ad;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"eos.G3_1")) {
m=A.eos.G3_1;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"eos.cv")) {
m=A.eos.cv;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"eos.d")) {
m=A.eos.d;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"eos.prad")) {
m=A.eos.prad;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"eos.chi_T")) {
m=A.eos.chi_T;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"eos.chi_rho")) {
m=A.eos.chi_rho;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"eos.s")) {
m=A.eos.s;
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"L")) {
d=A.luminosity();
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"L/L_SUN")) {
d=A.luminosity()/L_SUN;
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"Teff")) {
m=A.Teff();
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"Teff_eq")) {
m=A.Teff();
d=A.map.leg.eval_00(m,PI/2)(0);
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"Teff_pol")) {
m=A.Teff();
d=A.map.leg.eval_00(m,0)(0);
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"gsup")) {
m=A.gsup();
if(fmt) matrix_fmt(fmt,(m,T));
else (m,T).write(stdout,'b');
} else if(!strcmp(var,"gsup_eq")) {
m=A.gsup();
d=A.map.leg.eval_00(m,PI/2)(0);
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"gsup_pol")) {
m=A.gsup();
d=A.map.leg.eval_00(m,0)(0);
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"eps")) {
d=1-1./A.map.leg.eval_00(A.r,PI/2)(0);
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"eps_c")) {
d=0;
if(A.conv) d=1-A.map.eta(A.conv)/A.map.leg.eval_00(A.map.R.row(A.conv),PI/2)(0);
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"virial")) {
d=A.virial();
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else if(!strcmp(var,"energy_test")) {
d=A.energy_test();
if(fmt) fprintf(stdout,fmt,d);
else {
fwrite(&d,sizeof(double),1,stdout);
}
} else {
ester_err("Unknown variable %s", var);
exit(1);
}
}
void umat_plasticity_kin_iso_CCP(const vec &Etot, const vec &DEtot, vec &sigma, mat &Lt, mat &L, vec &sigma_in, const mat &DR, const int &nprops, const vec &props, const int &nstatev, vec &statev, const double &T, const double &DT, const double &Time, const double &DTime, double &Wm, double &Wm_r, double &Wm_ir, double &Wm_d, const int &ndi, const int &nshr, const bool &start, const int &solver_type, double &tnew_dt)
{
UNUSED(nprops);
UNUSED(nstatev);
UNUSED(Time);
UNUSED(DTime);
UNUSED(nshr);
UNUSED(tnew_dt);
//From the props to the material properties
double E = props(0);
double nu= props(1);
double alpha_iso = props(2);
double sigmaY = props(3);
double k=props(4);
double m=props(5);
double kX = props(6);
//definition of the CTE tensor
vec alpha = alpha_iso*Ith();
///@brief Temperature initialization
double T_init = statev(0);
//From the statev to the internal variables
double p = statev(1);
vec EP(6);
EP(0) = statev(2);
EP(1) = statev(3);
EP(2) = statev(4);
EP(3) = statev(5);
EP(4) = statev(6);
EP(5) = statev(7);
///@brief a is the internal variable associated with kinematical hardening
vec a = zeros(6);
a(0) = statev(8);
a(1) = statev(9);
a(2) = statev(10);
a(3) = statev(11);
a(4) = statev(12);
a(5) = statev(13);
//Rotation of internal variables (tensors)
EP = rotate_strain(EP, DR);
a = rotate_strain(a, DR);
///@brief Initialization
if(start)
{
//Elstic stiffness tensor
L = L_iso(E, nu, "Enu");
T_init = T;
vec vide = zeros(6);
sigma = vide;
EP = vide;
a = vide;
p = 0.;
Wm = 0.;
Wm_r = 0.;
Wm_ir = 0.;
Wm_d = 0.;
}
//Additional parameters and variables
vec X = kX*(a%Ir05());
double Hp=0.;
double dHpdp=0.;
if (p > iota) {
dHpdp = m*k*pow(p, m-1);
Hp = k*pow(p, m);
}
else {
dHpdp = 0.;
Hp = 0.;
}
//Variables values at the start of the increment
vec sigma_start = sigma;
vec EP_start = EP;
vec a_start = a;
vec X_start = X;
double A_p_start = -Hp;
vec A_a_start = -X_start;
//Variables required for the loop
vec s_j = zeros(1);
s_j(0) = p;
vec Ds_j = zeros(1);
vec ds_j = zeros(1);
///Elastic prediction - Accounting for the thermal prediction
vec Eel = Etot + DEtot - alpha*(T+DT-T_init) - EP;
sigma = el_pred(L, Eel, ndi);
//Define the plastic function and the stress
vec Phi = zeros(1);
mat B = zeros(1,1);
vec Y_crit = zeros(1);
double dPhidp=0.;
vec dPhida = zeros(6);
vec dPhidsigma = zeros(6);
double dPhidtheta = 0.;
//Compute the explicit flow direction
vec Lambdap = eta_stress(sigma-X);
vec Lambdaa = eta_stress(sigma-X);
std::vector<vec> kappa_j(1);
kappa_j[0] = L*Lambdap;
mat K = zeros(1,1);
//Loop parameters
int compteur = 0;
double error = 1.;
//Loop
for (compteur = 0; ((compteur < maxiter_umat) && (error > precision_umat)); compteur++) {
p = s_j(0);
if (p > iota) {
dHpdp = m*k*pow(p, m-1);
Hp = k*pow(p, m);
}
else {
dHpdp = 0.;
Hp = 0.;
}
dPhidsigma = eta_stress(sigma-X);
dPhidp = -1.*dHpdp;
dPhida = -1.*kX*(eta_stress(sigma - X)%Ir05());
//compute Phi and the derivatives
Phi(0) = Mises_stress(sigma-X) - Hp - sigmaY;
Lambdap = eta_stress(sigma-X);
Lambdaa = eta_stress(sigma-X);
kappa_j[0] = L*Lambdap;
K(0,0) = dPhidp + sum(dPhida%Lambdaa);
B(0, 0) = -1.*sum(dPhidsigma%kappa_j[0]) + K(0,0);
Y_crit(0) = sigmaY;
Fischer_Burmeister_m(Phi, Y_crit, B, Ds_j, ds_j, error);
s_j(0) += ds_j(0);
EP = EP + ds_j(0)*Lambdap;
a = a + ds_j(0)*Lambdaa;
X = kX*(a%Ir05());
//the stress is now computed using the relationship sigma = L(E-Ep)
Eel = Etot + DEtot - alpha*(T + DT - T_init) - EP;
sigma = el_pred(L, Eel, ndi);
}
//Computation of the increments of variables
vec Dsigma = sigma - sigma_start;
vec DEP = EP - EP_start;
double Dp = Ds_j[0];
vec Da = a - a_start;
if (solver_type == 0) {
//Computation of the tangent modulus
mat Bhat = zeros(1, 1);
Bhat(0, 0) = sum(dPhidsigma%kappa_j[0]) - K(0,0);
vec op = zeros(1);
mat delta = eye(1,1);
for (int i=0; i<1; i++) {
if(Ds_j[i] > iota)
op(i) = 1.;
}
mat Bbar = zeros(1,1);
for (int i = 0; i < 1; i++) {
for (int j = 0; j < 1; j++) {
Bbar(i, j) = op(i)*op(j)*Bhat(i, j) + delta(i,j)*(1-op(i)*op(j));
}
}
mat invBbar = zeros(1, 1);
mat invBhat = zeros(1, 1);
invBbar = inv(Bbar);
for (int i = 0; i < 1; i++) {
for (int j = 0; j < 1; j++) {
invBhat(i, j) = op(i)*op(j)*invBbar(i, j);
}
}
std::vector<vec> P_epsilon(1);
P_epsilon[0] = invBhat(0, 0)*(L*dPhidsigma);
std::vector<double> P_theta(1);
P_theta[0] = dPhidtheta - sum(dPhidsigma%(L*alpha));
Lt = L - (kappa_j[0]*P_epsilon[0].t());
}
else if(solver_type == 1) {
sigma_in = -L*EP;
}
double A_p = -Hp;
vec A_a = -X;
double Dgamma_loc = 0.5*sum((sigma_start+sigma)%DEP) + 0.5*(A_p_start + A_p)*Dp + 0.5*sum((A_a_start + A_a)%Da);
//Computation of the mechanical and thermal work quantities
Wm += 0.5*sum((sigma_start+sigma)%DEtot);
Wm_r += 0.5*sum((sigma_start+sigma)%(DEtot-DEP)) - 0.5*sum((A_a_start + A_a)%Da);
Wm_ir += -0.5*(A_p_start + A_p)*Dp;
Wm_d += Dgamma_loc;
///@brief statev evolving variables
//statev
statev(0) = T_init;
statev(1) = p;
statev(2) = EP(0);
statev(3) = EP(1);
statev(4) = EP(2);
statev(5) = EP(3);
statev(6) = EP(4);
statev(7) = EP(5);
statev(8) = a(0);
statev(9) = a(1);
statev(10) = a(2);
statev(11) = a(3);
statev(12) = a(4);
statev(13) = a(5);
}
int tf_csim_casdec_x()
{
CSIM::fir_x fir_x1, fir_x2, fir_x3;
CSIM::counter cnt1, cnt2, cnt3;
CSIM::wgn_x wgn_x1;
CSIM::casdec_x casdec_x1;
int N;
cvec c0;
cvec x_x;
bvec ce, ce1, ce2, ce3;
cvec ya_x1, ya_x2, ya_x3;
cmat ya, yb, yc;
//----------------------------------------
// test of csim
//----------------------------------------
cout << "CSIM::casdec_x Test" << endl;
try {
// mav<4>
c0="(0.25,0.0) (0.25,0.0) (0.25, 0.0) (0.25,0.0)";
cout << "c0=" << c0 << endl;
// discrete cascade settings
fir_x1.set_taps(c0); fir_x1.reset();
fir_x2.set_taps(c0); fir_x2.reset();
fir_x3.set_taps(c0); fir_x3.reset();
cnt1.set_N(2); cnt1.reset();
cnt2.set_N(2); cnt2.reset();
cnt3.set_N(2); cnt3.reset();
cout << "discrete cascade settings " << endl;
// device cascade settings
casdec_x1.set_size(3);
casdec_x1.set_taps(c0);
casdec_x1.reset();
cout << "device cascade settings " << endl;
N=256;
// clock and signal
ce.set_length(N); ce.ones(); // clock
cout << "ce=" << ce << endl;
// discrete cascade proc
while (1) {
x_x = wgn_x1.generate(ce); // source - random mean = (0.0+j0.0) sigma =1.0
cout << "x_x=" << x_x << endl;
ya_x1 = fir_x1.process(ce,x_x); ce1 = cnt1.generate(ce);
ya_x2 = fir_x2.process(ce1,ya_x1); ce2 = cnt2.generate(ce1);
ya_x3 = fir_x3.process(ce2,ya_x2); ce3 = cnt3.generate(ce2);
ya.set_size(N,2);
ya.set_col(0,ya_x3);
ya.set_col(1,to_cvec(to_vec(ce3), zeros(ce3.length())));
// cout << "discrete cascade proc " << endl;
// device cascade proc
yb = casdec_x1.process(ce, x_x);
// cout << "device cascade proc " << endl;
yc.set_size(N,4);
yc.set_col(0, ya.get_col(0));
yc.set_col(1, yb.get_col(0));
yc.set_col(2, ya.get_col(1));
yc.set_col(3, yb.get_col(1));
cout << "yc=" << endl << yc << endl;
}
}
catch (sci_exception except) {
cout<< "\n sci exception:" << endl <<except.get_msg() <<":" << except.get_info() << endl;
}
return 0;
}
/** \brief Create a dense matrix or a matrix with specified sparsity with all entries zero */
static MatType zeros(casadi_int nrow=1, casadi_int ncol=1) {
return zeros(Sparsity::dense(nrow, ncol));
}
/**
* Called before an iteration is started.
*/
void before_iteration(int iteration, graphchi_context &gcontext) {
liklihood_vec = zeros(gcontext.execthreads);
err_vec = zeros(gcontext.execthreads);
}
static MatType zeros(const std::pair<casadi_int, casadi_int>& rc) {
return zeros(rc.first, rc.second);
}
/**
* Vertex update function
*/
void update(graphchi_vertex<VertexDataType, EdgeDataType> &vertex, graphchi_context &gcontext) {
if (vertex.num_edges() > 0) {
vertex_data & vdata = latent_factors_inmem[vertex.id()];
vec R_cache = zeros(vertex.num_edges());
// for each latent feature
for (int x = 0; x < D; x++) {
double numerator = 0.0;
double denominator = 0.0;
bool compute_rmse = (vertex.num_outedges() > 0 && x == 0);
// for each user/item have connection with this node
for (int j = 0; j < vertex.num_edges(); j++) {
float observation = vertex.edge(j)->get_data(); // rating value of this connection
vertex_data & nbr_latent = latent_factors_inmem[vertex.edge(j)->vertex_id()];// latent feature vector of this node
double prediction;
double rmse = 0;
// if x = 0, calculate rmse and initialize R_cache
if (x == 0) {
rmse = bsvd_predict(vdata, nbr_latent, observation, prediction);
R_cache[j] = observation - prediction;
}
//** B term
double B = exp(alpha * (prediction - maxval));
//** C term
double C = exp(alpha * (minval - prediction));
// compute numerator
numerator += 2 * nbr_latent.pvec[x] * (R_cache[j] + vdata.pvec[x] * nbr_latent.pvec[x])
- lambda * alpha * nbr_latent.pvec[x] * (B - C)
+ lambda * alpha * alpha * nbr_latent.pvec[x] * nbr_latent.pvec[x] * vdata.pvec[x] * (B + C);
//compute denominator
denominator += nbr_latent.pvec[x] * nbr_latent.pvec[x] * (2 + lambda * alpha * alpha * (B + C));
//record rmse
if (compute_rmse)
rmse_vec[omp_get_thread_num()] += rmse;
}
// update if denominator != 0
if (denominator != 0.0) {
double z = numerator / denominator;
vec old = vdata.pvec;
for (int j = 0; j < vertex.num_edges(); j++) {
vertex_data & nbr_latent = latent_factors_inmem[vertex.edge(j)->vertex_id()];
//update using equation (7) in ICDM paper
//R_ij -= (z - w_it )*h_jt
R_cache[j] -= ((z - old[x]) * nbr_latent.pvec[x]);
}
//update using equation (8) in ICDM paper
//w_it = z;
vdata.pvec[x] = z;
}
}
}
}
void NitfWriter::done(PointTableRef table)
{
LasWriter::done(table);
try
{
::nitf::Record record(NITF_VER_21);
::nitf::FileHeader header = record.getHeader();
header.getFileHeader().set("NITF");
header.getComplianceLevel().set(m_cLevel);
header.getSystemType().set(m_sType);
header.getOriginStationID().set(m_oStationId);
header.getFileTitle().set(m_fileTitle);
header.getClassification().set(m_fileClass);
header.getMessageCopyNum().set("00000");
header.getMessageNumCopies().set("00000");
header.getEncrypted().set("0");
header.getBackgroundColor().setRawData(const_cast<char*>("000"), 3);
header.getOriginatorName().set(m_origName);
header.getOriginatorPhone().set(m_origPhone);
header.getSecurityGroup().getClassificationSystem().set(m_securityClassificationSystem);
header.getSecurityGroup().getControlAndHandling().set(m_securityControlAndHandling);
header.getSecurityGroup().getClassificationText().set(m_sic);
::nitf::DESegment des = record.newDataExtensionSegment();
des.getSubheader().getFilePartType().set("DE");
des.getSubheader().getTypeID().set("LIDARA DES");
des.getSubheader().getVersion().set("01");
des.getSubheader().getSecurityClass().set(m_securityClass);
::nitf::FileSecurity security = record.getHeader().getSecurityGroup();
des.getSubheader().setSecurityGroup(security.clone());
::nitf::TRE usrHdr("LIDARA DES", "raw_data");
usrHdr.setField("raw_data", "not");
::nitf::Field fld = usrHdr.getField("raw_data");
fld.setType(::nitf::Field::BINARY);
flush();
m_oss.flush();
std::streambuf *buf = m_oss.rdbuf();
long size = buf->pubseekoff(0, m_oss.end);
buf->pubseekoff(0, m_oss.beg);
std::vector<char> bytes(size);
buf->sgetn(bytes.data(), size);
des.getSubheader().setSubheaderFields(usrHdr);
::nitf::ImageSegment image = record.newImageSegment();
::nitf::ImageSubheader subheader = image.getSubheader();
BOX3D bounds = reprojectBoxToDD(table.spatialRef(), m_bounds);
//NITF decimal degree values for corner coordinates only has a
// precision of 3 after the decimal. This may cause an invalid
// polygon due to rounding errors with a small tile. Therefore
// instead of rounding min values will use the floor value and
// max values will use the ceiling values.
bounds.minx = (floor(bounds.minx * 1000)) / 1000.0;
bounds.miny = (floor(bounds.miny * 1000)) / 1000.0;
bounds.maxx = (ceil(bounds.maxx * 1000)) / 1000.0;
bounds.maxy = (ceil(bounds.maxy * 1000)) / 1000.0;
double corners[4][2];
corners[0][0] = bounds.maxy;
corners[0][1] = bounds.minx;
corners[1][0] = bounds.maxy;
corners[1][1] = bounds.maxx;
corners[2][0] = bounds.miny;
corners[2][1] = bounds.maxx;
corners[3][0] = bounds.miny;
corners[3][1] = bounds.minx;
subheader.setCornersFromLatLons(NRT_CORNERS_DECIMAL, corners);
subheader.getImageSecurityClass().set(m_imgSecurityClass);
subheader.setSecurityGroup(security.clone());
if (m_imgDate.size())
subheader.getImageDateAndTime().set(m_imgDate);
::nitf::BandInfo info;
::nitf::LookupTable lt(0,0);
info.init("G", /* The band representation, Nth band */
" ", /* The band subcategory */
"N", /* The band filter condition */
" ", /* The band standard image filter code */
0, /* The number of look-up tables */
0, /* The number of entries/LUT */
lt); /* The look-up tables */
std::vector< ::nitf::BandInfo> bands;
bands.push_back(info);
subheader.setPixelInformation(
"INT", /* Pixel value type */
8, /* Number of bits/pixel */
8, /* Actual number of bits/pixel */
"R", /* Pixel justification */
"NODISPLY", /* Image representation */
"VIS", /* Image category */
1, /* Number of bands */
bands);
subheader.setBlocking(
8, /*!< The number of rows */
8, /*!< The number of columns */
8, /*!< The number of rows/block */
8, /*!< The number of columns/block */
"P"); /*!< Image mode */
//Image Header fields to set
subheader.getImageId().set("None");
subheader.getImageTitle().set(m_imgIdentifier2);
// 64 char string
std::string zeros(64, '0');
std::unique_ptr< ::nitf::BandSource> band(new ::nitf::MemorySource(
const_cast<char*>(zeros.c_str()),
zeros.size() /* memory size */,
0 /* starting offset */,
1 /* bytes per pixel */,
0 /*skip*/));
::nitf::ImageSource iSource;
iSource.addBand(*band);
//AIMIDB
if (!m_aimidb.empty())
{
boost::optional<const Options&> options = m_aimidb.getOptions();
if (options)
{
::nitf::TRE tre("AIMIDB");
std::vector<Option> opts = options->getOptions();
for (auto i = opts.begin(); i != opts.end(); ++i)
{
tre.setField(i->getName(), i->getValue<std::string>());
}
subheader.getExtendedSection().appendTRE(tre);
}
}
//ACFTB
if (!m_acftb.empty())
{
boost::optional<const Options&> options = m_acftb.getOptions();
if (options)
{
::nitf::TRE tre("ACFTB");
std::vector<Option> opts = options->getOptions();
for (auto i = opts.begin(); i != opts.end(); ++i)
{
tre.setField(i->getName(), i->getValue<std::string>());
}
subheader.getExtendedSection().appendTRE(tre);
}
}
::nitf::Writer writer;
::nitf::IOHandle output_io(m_filename.c_str(), NITF_ACCESS_WRITEONLY,
NITF_CREATE);
writer.prepare(output_io, record);
::nitf::SegmentWriter sWriter = writer.newDEWriter(0);
::nitf::SegmentMemorySource sSource(bytes.data(), size, 0, 0, false);
sWriter.attachSource(sSource);
::nitf::ImageWriter iWriter = writer.newImageWriter(0);
iWriter.attachSource(iSource);
writer.write();
output_io.close();
}
catch (except::Throwable & t)
{
std::ostringstream oss;
// std::cout << t.getTrace();
throw pdal_error(t.getMessage());
}
}
USING_NAMESPACE_ACADO
int main( ){
// Define variables, functions and constants:
// ----------------------------------------------------------
DifferentialState dT1;
DifferentialState dT2;
DifferentialState dT3;
DifferentialState dT4;
DifferentialState T1;
DifferentialState T2;
DifferentialState T3;
DifferentialState T4;
DifferentialState W1;
DifferentialState W2;
DifferentialState W3;
DifferentialState W4;
DifferentialState q1;
DifferentialState q2;
DifferentialState q3;
DifferentialState q4;
DifferentialState Omega1;
DifferentialState Omega2;
DifferentialState Omega3;
DifferentialState V1;
DifferentialState V2;
DifferentialState V3;
DifferentialState P1; // x
DifferentialState P2; // y
DifferentialState P3; // z
DifferentialState IP1;
DifferentialState IP2;
DifferentialState IP3;
Control U1;
Control U2;
Control U3;
Control U4;
DifferentialEquation f1, f2;
const double rho = 1.23;
const double A = 0.1;
const double Cl = 0.25;
const double Cd = 0.3*Cl;
const double m = 10;
const double g = 9.81;
const double L = 0.5;
const double Jp = 1e-2;
const double xi = 1e-2;
const double J1 = 0.25;
const double J2 = 0.25;
const double J3 = 1;
const double gain = 1e-4;
const double alpha = 0.0;
// Define the quadcopter ODE model in fully nonlinear form:
// ----------------------------------------------------------
f1 << U1*gain;
f1 << U2*gain;
f1 << U3*gain;
f1 << U4*gain;
f1 << dT1;
f1 << dT2;
f1 << dT3;
f1 << dT4;
f1 << (T1 - W1*xi)/Jp;
f1 << (T2 - W2*xi)/Jp;
f1 << (T3 - W3*xi)/Jp;
f1 << (T4 - W4*xi)/Jp;
f1 << - (Omega1*q2)/2 - (Omega2*q3)/2 - (Omega3*q4)/2 - (alpha*q1*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f1 << (Omega1*q1)/2 - (Omega3*q3)/2 + (Omega2*q4)/2 - (alpha*q2*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f1 << (Omega2*q1)/2 + (Omega3*q2)/2 - (Omega1*q4)/2 - (alpha*q3*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f1 << (Omega3*q1)/2 - (Omega2*q2)/2 + (Omega1*q3)/2 - (alpha*q4*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f1 << (J3*Omega2*Omega3 - J2*Omega2*Omega3 + (A*Cl*L*rho*(W2*W2 - W4*W4))/2)/J1;
f1 << -(J3*Omega1*Omega3 - J1*Omega1*Omega3 + (A*Cl*L*rho*(W1*W1 - W3*W3))/2)/J2;
f1 << (J2*Omega1*Omega2 - J1*Omega1*Omega2 + (A*Cd*rho*(W1*W1 - W2*W2 + W3*W3 - W4*W4))/2)/J3;
f1 << (A*Cl*rho*(2*q1*q3 + 2*q2*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
f1 << -(A*Cl*rho*(2*q1*q2 - 2*q3*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
f1 << (A*Cl*rho*(W1*W1 + W2*W2 + W3*W3 + W4*W4)*(q1*q1 - q2*q2 - q3*q3 + q4*q4))/(2*m) - g;
f1 << V1;
f1 << V2;
f1 << V3;
f1 << P1;
f1 << P2;
f1 << P3;
// Define the quadcopter ODE model in 3-stage format:
// ----------------------------------------------------------
// LINEAR INPUT SYSTEM (STAGE 1):
Matrix M1, A1, B1;
M1 = eye(12);
A1 = zeros(12,12);
B1 = zeros(12,4);
A1(4,0) = 1.0;
A1(5,1) = 1.0;
A1(6,2) = 1.0;
A1(7,3) = 1.0;
A1(8,4) = 1.0/Jp; A1(8,8) = -xi/Jp;
A1(9,5) = 1.0/Jp; A1(9,9) = -xi/Jp;
A1(10,6) = 1.0/Jp; A1(10,10) = -xi/Jp;
A1(11,7) = 1.0/Jp; A1(11,11) = -xi/Jp;
B1(0,0) = gain;
B1(1,1) = gain;
B1(2,2) = gain;
B1(3,3) = gain;
// NONLINEAR SYSTEM (STAGE 2):
f2 << - (Omega1*q2)/2 - (Omega2*q3)/2 - (Omega3*q4)/2 - (alpha*q1*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f2 << (Omega1*q1)/2 - (Omega3*q3)/2 + (Omega2*q4)/2 - (alpha*q2*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f2 << (Omega2*q1)/2 + (Omega3*q2)/2 - (Omega1*q4)/2 - (alpha*q3*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f2 << (Omega3*q1)/2 - (Omega2*q2)/2 + (Omega1*q3)/2 - (alpha*q4*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f2 << (J3*Omega2*Omega3 - J2*Omega2*Omega3 + (A*Cl*L*rho*(W2*W2 - W4*W4))/2)/J1;
f2 << -(J3*Omega1*Omega3 - J1*Omega1*Omega3 + (A*Cl*L*rho*(W1*W1 - W3*W3))/2)/J2;
f2 << (J2*Omega1*Omega2 - J1*Omega1*Omega2 + (A*Cd*rho*(W1*W1 - W2*W2 + W3*W3 - W4*W4))/2)/J3;
f2 << (A*Cl*rho*(2*q1*q3 + 2*q2*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
f2 << -(A*Cl*rho*(2*q1*q2 - 2*q3*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
f2 << (A*Cl*rho*(W1*W1 + W2*W2 + W3*W3 + W4*W4)*(q1*q1 - q2*q2 - q3*q3 + q4*q4))/(2*m) - g;
// LINEAR OUTPUT SYSTEM (STAGE 3):
Matrix M3, A3;
M3 = eye(6);
A3 = zeros(6,6);
A3(3,0) = 1.0;
A3(4,1) = 1.0;
A3(5,2) = 1.0;
OutputFcn f3;
f3 << V1;
f3 << V2;
f3 << V3;
f3 << 0.0;
f3 << 0.0;
f3 << 0.0;
// ----------------------------------------------------------
// ----------------------------------------------------------
SIMexport sim1( 10, 1.0 );
sim1.setModel( f1 );
sim1.set( INTEGRATOR_TYPE, INT_IRK_GL4 );
sim1.set( NUM_INTEGRATOR_STEPS, 50 );
sim1.setTimingSteps( 10000 );
acadoPrintf( "-----------------------------------------------------------\n Using a QuadCopter ODE model in fully nonlinear form:\n-----------------------------------------------------------\n" );
sim1.exportAndRun( "quadcopter_export", "init_quadcopter.txt", "controls_quadcopter.txt" );
// ----------------------------------------------------------
// ----------------------------------------------------------
SIMexport sim2( 10, 1.0 );
sim2.setLinearInput( M1, A1, B1 );
sim2.setModel( f2 );
sim2.setLinearOutput( M3, A3, f3 );
sim2.set( INTEGRATOR_TYPE, INT_IRK_GL4 );
sim2.set( NUM_INTEGRATOR_STEPS, 50 );
sim2.setTimingSteps( 10000 );
acadoPrintf( "-----------------------------------------------------------\n Using a QuadCopter ODE model in 3-stage format:\n-----------------------------------------------------------\n" );
sim2.exportAndRun( "quadcopter_export", "init_quadcopter.txt", "controls_quadcopter.txt" );
return 0;
}
void reset_rmse(int exec_threads){
logstream(LOG_DEBUG)<<"Detected number of threads: " << exec_threads << std::endl;
num_threads = exec_threads;
rmse_vec = zeros(exec_threads);
}
bool
CppDetEstCircle4 (vector < double >x, vector < double >y,
double cellsize, unsigned int maxR, unsigned int sigmacoef,
double epsion, unsigned int maxIter, unsigned int minnumfit,
vector < double >&detectedcircle,
vector < double >&mapfinalHTcircle,
vector < double >&iterationdata, vector < double >&ArcInfo,
vector < double >&finalx, vector < double >&finaly,
vector < double >&DebugInfo)
{
// based from 0.
// detectedcircle: [yc, xc, r], in unit of meter.
// mapfinalHTcircle: [rowc, colc, r], in unit of pixel.
if (x.size () != y.size ()) {
#ifdef MATLABPRINT
mexPrintf
("CppDetEstCircle ==> input x and y do not have the same length!\n");
#endif
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
ArcInfo.clear ();
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return false;
}
mxArray *mx_x, *mx_y, *mx_refmatrix;
double *tempGetPr;
vector < unsigned int >img, dim_img (2), dim_refmatrix (2);
vector < double >refmatrix;
// call CppGridPts to grid the points cloud into cellsize*cellsize cells
bool callflag;
callflag =
CppGridPts (x, y, cellsize, maxR, minnumfit, img, dim_img, refmatrix,
dim_refmatrix);
if (!callflag) {
#ifdef MATLABPRINT
mexPrintf ("CppDetEstCircle ==> calling CppGridPts failed!\n");
#endif
// maybe inputs are invalid!
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
ArcInfo.clear ();
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return false;
}
if (img.size () == 0 || refmatrix.size () == 0) {
// less than minnumfit points in this section
#ifdef DEBUG
mexPrintf
("CppDetEstCircle ==> calling CppGridPts ==> less than minnumfit points in this section!\n");
#endif
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = 0.0;
iterationdata[1] = mxGetNaN ();
iterationdata[2] = mxGetNaN ();
iterationdata[3] = static_cast < double >(x.size ());
ArcInfo.clear ();
ArcInfo.resize (3, 0.0);
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return true;
}
// convert x, y to row and col by MATLAB function: map2pix
// xpix, ypix: based from 0.
vector < double >xpix (x.size ()), ypix (y.size ());
mxArray *prhs[3], *plhs[2];
mx_x = mxCreateDoubleMatrix (x.size (), 1, mxREAL);
mx_y = mxCreateDoubleMatrix (y.size (), 1, mxREAL);
mx_refmatrix =
mxCreateDoubleMatrix (dim_refmatrix[0], dim_refmatrix[1], mxREAL);
tempGetPr = mxGetPr (mx_x);
for (unsigned int i = 0; i < x.size (); i++) {
tempGetPr[i] = x[i];
}
tempGetPr = mxGetPr (mx_y);
for (unsigned int i = 0; i < y.size (); i++) {
tempGetPr[i] = y[i];
}
tempGetPr = mxGetPr (mx_refmatrix);
for (unsigned int i = 0; i < refmatrix.size (); i++) {
tempGetPr[i] = refmatrix[i];
}
prhs[1] = mx_x;
prhs[2] = mx_y;
prhs[0] = mx_refmatrix;
mexCallMATLAB (2, plhs, 3, prhs, "map2pix");
tempGetPr = mxGetPr (plhs[1]);
for (unsigned int i = 0; i < x.size (); i++) {
xpix[i] = tempGetPr[i] - 1;
}
tempGetPr = mxGetPr (plhs[0]);
for (unsigned int i = 0; i < y.size (); i++) {
ypix[i] = tempGetPr[i] - 1;
}
mxDestroyArray (mx_x);
mxDestroyArray (mx_y);
vector < double >estcircle;
CppCircleFitting (xpix, ypix, estcircle);
if (estcircle.size () == 0) {
#ifdef DEBUG
mexPrintf ("CppDetEstCircle ==> CppCircleFitting return empty!\n");
#endif
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = 0.0;
iterationdata[1] = mxGetNaN ();
iterationdata[2] = mxGetNaN ();
iterationdata[3] = static_cast < double >(x.size ());
ArcInfo.clear ();
ArcInfo.resize (3, 0.0);
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return true;
}
if (estcircle[2] < 0) {
#ifdef DEBUG
mexPrintf ("CppDetEstCircle ==> CppCircleFitting return negative!\n");
#endif
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = 0.0;
iterationdata[1] = mxGetNaN ();
iterationdata[2] = mxGetNaN ();
iterationdata[3] = static_cast < double >(x.size ());
ArcInfo.clear ();
ArcInfo.resize (3, 0.0);
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return true;
}
vector < double >precircle (3, mxGetInf ());
vector < bool > ROIimage (img.size ());
vector < double >ROIxpix, ROIypix;
vector < unsigned int >inputImage (img.size ());
vector < unsigned int >edgeImage (img.size ());
vector < unsigned int >HTrow, HTcol, HTr, HTpeak;
vector < double >zeros (img.size (), 0.0);
vector < double >finalHTcircle, allRsigma;
unsigned int iterativecount = 0;
bool fitflag = true;
double centerchange, radiuschange, finalRsigma;
transform (img.begin (), img.end (), zeros.begin (), ROIimage.begin (),
std::not_equal_to < double >());
centerchange =
sqrt ((estcircle[0] - precircle[0]) * (estcircle[0] - precircle[0]) +
(estcircle[1] - precircle[1]) * (estcircle[1] - precircle[1]));
radiuschange = fabs (estcircle[2] - precircle[2]);
while (centerchange > epsion / cellsize || radiuschange > epsion / cellsize) {
// In this while loop, estcircle and precircle are both in unit of pixel.
// initially detect circle by Hough Transform.
inputImage.clear ();
inputImage.resize (img.size (), 0);
transform (img.begin (), img.end (), ROIimage.begin (),
inputImage.begin (), std::multiplies < unsigned int >());
//int debugcount=0;
//for (unsigned int i=0; i<img.size(); i++)
//{
// if ( (img[i]!=0) && ROIimage[i] )
// {
// inputImage[i]=img[i];
// debugcount++;
// }
//}
HTrow.clear ();
HTcol.clear ();
HTr.clear ();
HTpeak.clear ();
if (!
(CppIterCHT
(inputImage, dim_img, maxR, edgeImage, dim_img, HTrow, HTcol, HTr,
HTpeak)
)) {
#ifdef MATLABPRINT
mexPrintf ("CppDetEstCircle ==> calling CppIterCHT failed!\n");
#endif
return false;
}
if (HTrow.size () == 0) {
// no circle detected by CHT
fitflag = false;
break;
}
// select one circle from the above derived by CHT.
finalHTcircle.clear ();
allRsigma.clear ();
finalRsigma =
CppSelectHTCircle (edgeImage, dim_img, HTrow, HTcol, HTr, finalHTcircle,
allRsigma);
if (finalRsigma < 0) {
#ifdef MATLABPRINT
mexPrintf ("CppDetEstCircle ==> calling CppSelectHTCircle failed!\n");
#endif
return false;
}
// mark ROI
ROIimage.clear ();
if (!
(CppMarkROI
(edgeImage, dim_img, finalHTcircle, xpix, ypix, sigmacoef,
finalRsigma, ROIimage, ROIxpix, ROIypix)
)) {
#ifdef MATLABPRINT
mexPrintf ("CppDetEstCircle ==> calling CppMarkROI failed!\n");
#endif
return false;
}
if (ROIxpix.size () < minnumfit) {
#ifdef DEBUG
mexPrintf
("CppDetEstCircle ==> less than minnumfit points used to fit a circle!\n");
#endif
fitflag = false;
break;
}
precircle.clear ();
precircle.resize (estcircle.size (), 0.0);
copy (estcircle.begin (), estcircle.end (), precircle.begin ());
// fit circles using points in ROI
CppCircleFitting (ROIxpix, ROIypix, estcircle);
if (estcircle.size () == 0) {
#ifdef DEBUG
mexPrintf ("CppDetEstCircle ==> CppCircleFitting return empty!\n");
#endif
fitflag = false;
break;
}
if (estcircle[2] < 0) {
#ifdef DEBUG
mexPrintf ("CppDetEstCircle ==> CppCircleFitting return negative!\n");
#endif
fitflag = false;
break;
}
// calculate change compared with previous circle
centerchange =
sqrt ((estcircle[0] - precircle[0]) * (estcircle[0] - precircle[0]) +
(estcircle[1] - precircle[1]) * (estcircle[1] - precircle[1]));
radiuschange = fabs (estcircle[2] - precircle[2]);
iterativecount += 1;
if (iterativecount == maxIter) {
fitflag = true;
break;
}
}
// Now, we begin to check if estcircle(detectedcircle) is derived in while loop and if the derived is valid.
// We check it from 3 aspects.
// 1st check: whether circle is derived, see if fitflag is true
if (!fitflag) {
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = static_cast < double >(iterativecount);
iterationdata[1] = centerchange * cellsize;
iterationdata[2] = radiuschange * cellsize;
iterationdata[3] = static_cast < double >(ROIxpix.size ());
ArcInfo.clear ();
ArcInfo.resize (3, 0.0);
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return true;
}
//2nd check:
//see if the detectedcircle(or estcircle) locates within point
//cloud of trunk, if true, this horizontal point cloud section is
//unqualified, discard the detectedcircle. If in the 3*3 window
//centered at detectedcircle point density in every cell is greater
//than 1, we think the circle center locating within trunk.
double ptsdensity;
ptsdensity =
CppNeighborPtsDensity (img, dim_img, 3, estcircle[0], estcircle[1]);
if (ptsdensity < 0) {
#ifdef MATLABPRINT
mexPrintf ("CppDetEstCircle ==> calling CppNeighborPtsDensity failed!\n");
#endif
return false;
}
if (mxIsInf (ptsdensity)) {
// If ptsdensity is Inf, the derived center locates outside the rasterized image.
detectedcircle.clear ();
mapfinalHTcircle.clear ();
detectedcircle.resize (3, 0.0);
mapfinalHTcircle.resize (3, 0.0);
prhs[0] = mx_refmatrix;
prhs[1] = mxCreateDoubleMatrix (2, 1, mxREAL);
prhs[2] = mxCreateDoubleMatrix (2, 1, mxREAL);
tempGetPr = mxGetPr (prhs[1]);
tempGetPr[0] = estcircle[0] + 1;
tempGetPr[1] = finalHTcircle[0] + 1;
tempGetPr = mxGetPr (prhs[2]);
tempGetPr[0] = estcircle[1] + 1;
tempGetPr[1] = finalHTcircle[1] + 1;
mexCallMATLAB (2, plhs, 3, prhs, "pix2map");
tempGetPr = mxGetPr (plhs[0]); //xc
detectedcircle[1] = tempGetPr[0];
mapfinalHTcircle[1] = tempGetPr[1]; // xc
tempGetPr = mxGetPr (plhs[1]); //yc
detectedcircle[0] = tempGetPr[0];
mapfinalHTcircle[0] = tempGetPr[1]; // yc
detectedcircle[2] = estcircle[2] * cellsize;
mapfinalHTcircle[2] = finalHTcircle[2] * cellsize; // r
mxDestroyArray (prhs[1]);
mxDestroyArray (prhs[2]);
mxDestroyArray (plhs[0]);
mxDestroyArray (plhs[1]);
/*detectedcircle.assign(estcircle.begin(), estcircle.end());
mapfinalHTcircle.assign(finalHTcircle.begin(), finalHTcircle.end()); */
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = static_cast < double >(iterativecount);
iterationdata[1] = mxGetNaN ();
iterationdata[2] = mxGetNaN ();
iterationdata[3] = static_cast < double >(ROIxpix.size ());
ArcInfo.clear ();
ArcInfo.resize (3, 0.0);
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return true;
} else if (ptsdensity > 1) {
// If ptsdensity is greater than 1, the derived center is on the trunk, not valid!
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = static_cast < double >(iterativecount);
iterationdata[1] = centerchange * cellsize;
iterationdata[2] = radiuschange * cellsize;
iterationdata[3] = static_cast < double >(ROIxpix.size ());
ArcInfo.clear ();
ArcInfo.resize (3, 0.0);
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return true;
}
// 3rd check: Iteration (while loop) ends possibly without getting right circle but due to reaching maximum iteration count or failed HT.
// So here the estcircle derived from while loop is checked whether satisfying the right circle condition.
if (iterativecount == maxIter) {
if (centerchange > epsion / cellsize || radiuschange > epsion / cellsize) {
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = static_cast < double >(maxIter);
iterationdata[1] = centerchange * cellsize;
iterationdata[2] = radiuschange * cellsize;
iterationdata[3] = static_cast < double >(ROIxpix.size ());
ArcInfo.clear ();
ArcInfo.resize (3, 0.0);
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return true;
}
}
//After all checked, we can output the valid derived circle.
detectedcircle.clear ();
mapfinalHTcircle.clear ();
detectedcircle.resize (3, 0.0);
mapfinalHTcircle.resize (3, 0.0);
prhs[0] = mx_refmatrix;
prhs[1] = mxCreateDoubleMatrix (2, 1, mxREAL);
prhs[2] = mxCreateDoubleMatrix (2, 1, mxREAL);
tempGetPr = mxGetPr (prhs[1]);
tempGetPr[0] = estcircle[0] + 1;
tempGetPr[1] = finalHTcircle[0] + 1;
tempGetPr = mxGetPr (prhs[2]);
tempGetPr[0] = estcircle[1] + 1;
tempGetPr[1] = finalHTcircle[1] + 1;
mexCallMATLAB (2, plhs, 3, prhs, "pix2map");
tempGetPr = mxGetPr (plhs[0]); //xc
detectedcircle[1] = tempGetPr[0];
mapfinalHTcircle[1] = tempGetPr[1]; // xc
tempGetPr = mxGetPr (plhs[1]); //yc
detectedcircle[0] = tempGetPr[0];
mapfinalHTcircle[0] = tempGetPr[1]; // yc
detectedcircle[2] = estcircle[2] * cellsize;
mapfinalHTcircle[2] = finalHTcircle[2] * cellsize; // r
mxDestroyArray (prhs[1]);
mxDestroyArray (prhs[2]);
mxDestroyArray (plhs[0]);
mxDestroyArray (plhs[1]);
// convert ROIxpix, ROIypix to finalx, finaly
finalx.clear ();
finalx.resize (ROIxpix.size ());
finaly.clear ();
finaly.resize (ROIypix.size ());
prhs[0] = mx_refmatrix;
prhs[1] = mxCreateDoubleMatrix (ROIypix.size (), 1, mxREAL);
prhs[2] = mxCreateDoubleMatrix (ROIxpix.size (), 1, mxREAL);
tempGetPr = mxGetPr (prhs[1]);
for (unsigned int i = 0; i < ROIypix.size (); i++) {
tempGetPr[i] = ROIypix[i] + 1;
}
tempGetPr = mxGetPr (prhs[2]);
for (unsigned int i = 0; i < ROIxpix.size (); i++) {
tempGetPr[i] = ROIxpix[i] + 1;
}
mexCallMATLAB (2, plhs, 3, prhs, "pix2map");
tempGetPr = mxGetPr (plhs[0]); //finalx
for (unsigned int i = 0; i < ROIxpix.size (); i++) {
finalx[i] = tempGetPr[i];
}
tempGetPr = mxGetPr (plhs[1]); //finaly
for (unsigned int i = 0; i < ROIypix.size (); i++) {
finaly[i] = tempGetPr[i];
}
mxDestroyArray (prhs[1]);
mxDestroyArray (prhs[2]);
mxDestroyArray (plhs[0]);
mxDestroyArray (plhs[1]);
mxDestroyArray (mx_refmatrix);
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = static_cast < double >(iterativecount);
iterationdata[1] = centerchange * cellsize;
iterationdata[2] = radiuschange * cellsize;
iterationdata[3] = static_cast < double >(ROIxpix.size ());
ArcInfo.clear ();
if (!CppArcInfo (ROIxpix, ROIypix, estcircle, ArcInfo)) {
#ifdef MATLABPRINT
mexPrintf ("CppDetEstCircle ==> calling CppArcInfo failed!\n");
#endif
return false;
}
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
// Because when rasterizing point clouds
// in the function CppGridPts the center of the first pixel,
// i.e. the pixel at [1,1] locates at (minx, maxy),
// we have to transform the angle in i-j(raster) coordinate system
// to x-y(point clouds) coordinate system.
// In x-y coordinate system, y axis points to north,
// so angle is presented in counterclockwise.
// In contrast, in i-j coordinate system,
// i axis (i.e. y axis in raster) points to south,
// so angle is presented in clockwise.
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
ArcInfo[0] = 2 * PI - ArcInfo[0];
ArcInfo[1] = 2 * PI - ArcInfo[1];
DebugInfo.clear ();
DebugInfo.resize (1);
DebugInfo[0] = finalRsigma * cellsize;
#ifdef DEBUG
mexPrintf ("CppDetEstCircle ==> finally points used to fit a circle: %d!\n",
ROIxpix.size ());
#endif
return true;
}
//------------------------------------------------------------------------------
void HamiltonMatrix::setHamiltonian(mat h) {
H = h;
Ht = zeros(h.n_rows ,h.n_cols);
}