void RateMeyerHaeseler::readRateFile(char *rate_file) { cout << "Reading site-specific rate file " << rate_file << " ..." << endl; try { ifstream in; in.exceptions(ios::failbit | ios::badbit); in.open(rate_file); char line[256]; int site, i; double rate; int nsites = phylo_tree->aln->getNSite(); resize(phylo_tree->aln->getNPattern(), -1.0); int saturated_sites = 0, saturated_ptn = 0; in.getline(line, sizeof(line)); //if (strncmp(line, "Site", 4) != 0) throw "Wrong header line"; for (i = 0; i < nsites; i++) { in.getline(line, sizeof(line)); stringstream ss(line); string tmp; ss >> tmp; site = convert_int(tmp.c_str()); if (site <= 0 || site > nsites) throw "Wrong site number (must be between 1 and #sites)"; site--; ss >> tmp; rate = convert_double(tmp.c_str()); if (rate < 0.0) throw "Negative rate not allowed"; if (rate <= 0.0) rate = MIN_SITE_RATE; int ptn = phylo_tree->aln->getPatternID(site); if (rate >= MAX_SITE_RATE) { rate = MAX_SITE_RATE; saturated_sites += phylo_tree->aln->at(ptn).frequency; saturated_ptn ++; } at(ptn) = rate; } in.clear(); // set the failbit again in.exceptions(ios::failbit | ios::badbit); in.close(); for (i = 0; i < size(); i++) if (at(i) < 0.0) throw "Some site has no rate information"; if (saturated_sites) { stringstream str; str << saturated_sites << " sites (" << saturated_ptn << " patterns) show too high rates (>=" << MAX_SITE_RATE << ")"; outWarning(str.str()); } } catch (const char *str) { outError(str); } catch (string str) { outError(str); } catch(ios::failure) { outError(ERR_READ_INPUT); } }
void Hokuyo::read(SampleBuffer *sb, cvg_long *timestampMs) { int ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST); if (ret < 0) outError("Cannot request data"); ret = urg_receiveData(&urg, sb->getDataPtr(), sb->getMaxLength()); if (ret < 0) outError("Cannot receive data"); sb->setCurrentLength(ret); if (timestampMs != NULL) (*timestampMs) = urg_recentTimestamp(&urg); }
/* * Reading models from resource directory */ bool LinemodInterface::readModels(const std::string &resourcePath) { DIR *dp; struct dirent *dirp; struct stat fileStat; if((dp = opendir(resourcePath.c_str())) == NULL) { outError("path (" << resourcePath << ") does not exist!"); return false; } std::vector<std::string> modelNames; std::vector<std::vector<std::string> > modelFiles; while((dirp = readdir(dp)) != NULL) { if(dirp->d_type != DT_DIR || dirp->d_name[0] == '.') { continue; } std::string name = dirp->d_name; std::string path = resourcePath + "/" + name + "/linemod.yml.gz"; if(!stat(path.c_str(), &fileStat) && S_ISREG(fileStat.st_mode)) { outDebug("read model: " << path); readModel(path); } } closedir(dp); return true; }
MongoDBBridge::MongoDBBridge(const boost::property_tree::ptree &pt) : CamInterface(pt) { readConfig(pt); outInfo("initialize"); storage = rs::Storage(host, db); actualFrame = 0; storage.getScenes(frames); if(continual) { actualFrame = frames.size(); } else { if(frames.empty()) { outError("No frames in database found!"); throw_exception_message("no frames found.") } outInfo("found " << frames.size() << " frames in database."); lastTimestamp = 0x7FFFFFFFFFFFFFFF; }
double RateGammaInvar::optimizeParameters(double gradient_epsilon) { int ndim = getNDim(); // return if nothing to be optimized if (ndim == 0) return phylo_tree->computeLikelihood(); if (verbose_mode >= VB_MED) cout << "Optimizing " << name << " model parameters by " << optimize_alg << " algorithm..." << endl; if (optimize_alg.find("EM_RR") != string::npos) { return randomRestartOptimization(gradient_epsilon); } else if (optimize_alg.find("Brent") != string::npos || phylo_tree->aln->frac_const_sites == 0.0 || isFixPInvar() || isFixGammaShape()) { double lh = phylo_tree->computeLikelihood(); cur_optimize = 0; double gamma_lh = RateGamma::optimizeParameters(gradient_epsilon); ASSERT(gamma_lh >= lh-0.1); cur_optimize = 1; double invar_lh = -DBL_MAX; invar_lh = RateInvar::optimizeParameters(gradient_epsilon); ASSERT(invar_lh >= gamma_lh-0.1); cur_optimize = 0; return invar_lh; } else if (optimize_alg.find("EM") != string::npos) { return optimizeWithEM(gradient_epsilon); } else if (optimize_alg.find("BFGS") != string::npos) { //if (freq_type == FREQ_ESTIMATE) scaleStateFreq(false); double *variables = new double[ndim+1]; double *upper_bound = new double[ndim+1]; double *lower_bound = new double[ndim+1]; bool *bound_check = new bool[ndim+1]; double score; // by BFGS algorithm setVariables(variables); setBounds(lower_bound, upper_bound, bound_check); score = -minimizeMultiDimen(variables, ndim, lower_bound, upper_bound, bound_check, max(gradient_epsilon, TOL_GAMMA_SHAPE)); getVariables(variables); phylo_tree->clearAllPartialLH(); score = phylo_tree->computeLikelihood(); delete [] bound_check; delete [] lower_bound; delete [] upper_bound; delete [] variables; return score; } else { string errMsg = "Unknown optimization algorithm: " + optimize_alg; outError(errMsg.c_str()); return 0.0; } }
void Log::outErrorST(const char * str, ...) { va_list ap; va_start(ap, str); char nnew_str[MAX_QUERY_LEN]; vsnprintf(nnew_str, MAX_QUERY_LEN, str, ap); va_end(ap); ACE_Stack_Trace st; outError("%s [Stacktrace: %s]", nnew_str, st.c_str()); }
void oLog::LargeErrorMessage(const char* source, ...) { std::vector<char*> lines; char* pointer; va_list ap; va_start(ap, source); pointer = const_cast<char*>(source); lines.push_back(pointer); size_t i, j, k; pointer = va_arg(ap, char*); while (pointer != NULL) { lines.push_back(pointer); pointer = va_arg(ap, char*); } va_end(ap); outError("*********************************************************************"); outError("* MAJOR ERROR/WARNING *"); outError("* =================== *"); for (std::vector<char*>::iterator itr = lines.begin(); itr != lines.end(); ++itr) { std::stringstream sstext; i = strlen(*itr); j = (i <= 65) ? 65 - i : 0; sstext << "* " << *itr; for (k = 0; k < j; ++k) { sstext << " "; } sstext << " *"; outError(sstext.str().c_str()); } outError("*********************************************************************"); }
RateFree::RateFree(int ncat, double start_alpha, string params, bool sorted_rates, string opt_alg, PhyloTree *tree) : RateGamma(ncat, start_alpha, false, tree) { fix_params = 0; prop = NULL; this->sorted_rates = sorted_rates; optimizing_params = 0; this->optimize_alg = opt_alg; setNCategory(ncat); if (params.empty()) return; DoubleVector params_vec; try { convert_double_vec(params.c_str(), params_vec); int i; double sum, sum_prop; if (params_vec.size() == ncategory) { // only inputing prop for (i = 0, sum_prop = 0.0; i < ncategory; i++) { prop[i] = params_vec[i]; rates[i] = 1.0; sum_prop += prop[i]; } fix_params = 1; } else { if (params_vec.size() != ncategory*2) outError("Number of parameters for FreeRate model must be twice number of categories"); for (i = 0, sum = 0.0, sum_prop = 0.0; i < ncategory; i++) { prop[i] = params_vec[i*2]; rates[i] = params_vec[i*2+1]; sum += prop[i]*rates[i]; sum_prop += prop[i]; } for (i = 0; i < ncategory; i++) rates[i] /= sum; fix_params = 2; } if (fabs(sum_prop-1.0) > 1e-5) outError("Sum of category proportions not equal to 1"); } catch (string &str) { outError(str); } }
double PartitionModel::computeFunction(double shape) { PhyloSuperTree *tree = (PhyloSuperTree*)site_rate->getTree(); double res = 0.0; linked_alpha = shape; for (PhyloSuperTree::iterator it = tree->begin(); it != tree->end(); it++) if ((*it)->getRate()->isGammaRate()) { res += (*it)->getRate()->computeFunction(shape); } if (res == 0.0) outError("No partition has Gamma rate heterogeneity!"); return res; }
void NGSAlignment::readFritzFile(const char *filename) { cout << "Reading Fritz file " << filename << " ..." << endl; try { ifstream in; in.exceptions(ios::failbit | ios::badbit); in.open(filename); in.clear(); int i, total_size; string tmp; in >> tmp; ncategory = convert_int(tmp.c_str()); if (ncategory < 1) throw "Wrong number of positions"; in >> tmp; num_states = convert_int(tmp.c_str()); total_size = ncategory*num_states*num_states; if (num_states < 1) throw "Wrong number of states"; pair_freq = new double[total_size]; for (i=0; i < total_size; i++) { in >> tmp; double count = convert_double(tmp.c_str()); if (count < 0) throw "Wrong count"; pair_freq[i] = count; } // set the failbit again in.exceptions(ios::failbit | ios::badbit); in.close(); } catch (const char *str) { outError(str); } catch (string str) { outError(str); } catch (ios::failure) { outError(ERR_READ_INPUT); } cout << ncategory << " matrices of size " << num_states << endl; }
void oLog::Init(int fileLogLevel, LogType logType) { SetFileLoggingLevel( fileLogLevel ); const char* logNormalFilename = NULL, *logErrorFilename = NULL; switch ( logType ) { case LOGON_LOG: { logNormalFilename = "logon-normal.log"; logErrorFilename = "logon-error.log"; break; } case WORLD_LOG: { logNormalFilename = "world-normal.log"; logErrorFilename = "world-error.log"; break; } } errno_t err = fopen_s( &m_normalFile, logNormalFilename, "a" ); if ( err ) { fprintf( stderr, "%s: Error opening '%s': %s\n", __FUNCTION__, logNormalFilename, error2str(err) ); } else { tm aTm; localtime_s( &aTm, &UNIXTIME ); outBasic( "[%-4d-%02d-%02d %02d:%02d:%02d] " , aTm.tm_year + 1900, aTm.tm_mon + 1, aTm.tm_mday, aTm.tm_hour, aTm.tm_min, aTm.tm_sec ); } err = fopen_s( &m_errorFile, logErrorFilename, "a" ); if ( err ) { fprintf( stderr, "%s: Error opening '%s': %s\n", __FUNCTION__, logErrorFilename, error2str(err) ); } else { tm aTm; localtime_s( &aTm, &UNIXTIME ); outError( "[%-4d-%02d-%02d %02d:%02d:%02d] " , aTm.tm_year + 1900, aTm.tm_mon + 1, aTm.tm_mday, aTm.tm_hour, aTm.tm_min, aTm.tm_sec ); } }
void oLog::Init(int32 fileLogLevel, LogType logType) { SetFileLoggingLevel(fileLogLevel); const char* logNormalFilename = NULL, *logErrorFilename = NULL; switch(logType) { case LOGON_LOG: { logNormalFilename = "logon-normal.log"; logErrorFilename = "logon-error.log"; break; } case WORLD_LOG: { logNormalFilename = "world-normal.log"; logErrorFilename = "world-error.log"; break; } } m_normalFile = fopen(logNormalFilename, "a"); if(m_normalFile == NULL) fprintf(stderr, "%s: Error opening '%s': %s\n", __FUNCTION__, logNormalFilename, strerror(errno)); else { tm* aTm = localtime(&UNIXTIME); outBasic("[%-4d-%02d-%02d %02d:%02d:%02d] ", aTm->tm_year + 1900, aTm->tm_mon + 1, aTm->tm_mday, aTm->tm_hour, aTm->tm_min, aTm->tm_sec); } m_errorFile = fopen(logErrorFilename, "a"); if(m_errorFile == NULL) fprintf(stderr, "%s: Error opening '%s': %s\n", __FUNCTION__, logErrorFilename, strerror(errno)); else { tm* aTm = localtime(&UNIXTIME); outError("[%-4d-%02d-%02d %02d:%02d:%02d] ", aTm->tm_year + 1900, aTm->tm_mon + 1, aTm->tm_mday, aTm->tm_hour, aTm->tm_min, aTm->tm_sec); } }
void ROSKinectBridge::lookupTransform(uima::CAS &tcas, const ros::Time ×tamp) { if(!lookUpViewpoint) { return; } tf::StampedTransform transform; try { outInfo("TIME Before lookup:" << ros::Time::now()); listener->waitForTransform(tfTo, tfFrom, timestamp, ros::Duration(10)); listener->lookupTransform(tfTo, tfFrom, timestamp, transform); rs::Scene scene = rs::SceneCas(tcas).getScene(); rs::StampedTransform vp(rs::conversion::to(tcas, transform)); scene.viewPoint.set(vp); outInfo("added viewpoint to scene"); } catch(tf::TransformException &ex) { outError(ex.what()); } }
void ROSKinectBridge::cb_(const sensor_msgs::Image::ConstPtr rgb_img_msg, const sensor_msgs::Image::ConstPtr depth_img_msg, const sensor_msgs::CameraInfo::ConstPtr camera_info_msg) { // static int frame = 0; // outWarn("got image: " << frame++); cv::Mat color, depth; // bool isHDColor; sensor_msgs::CameraInfo cameraInfo, cameraInfoHD; cv_bridge::CvImageConstPtr orig_rgb_img; orig_rgb_img = cv_bridge::toCvShare(rgb_img_msg, sensor_msgs::image_encodings::BGR8); cameraInfo = sensor_msgs::CameraInfo(*camera_info_msg); if(filterBlurredImages && detector.detectBlur(orig_rgb_img->image)) { lock.lock(); _newData = false; lock.unlock(); outWarn("Skipping blurred image!"); return; } cv_bridge::CvImageConstPtr orig_depth_img; orig_depth_img = cv_bridge::toCvShare(depth_img_msg, depth_img_msg->encoding); if(depth_img_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) { depth = orig_depth_img->image.clone(); } else if(depth_img_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) { orig_depth_img->image.convertTo(depth, CV_16U, 0.001); } else { outError("Unknown depth image type!"); return; } color = orig_rgb_img->image.clone(); if(color.cols == 1280 || color.cols == 1920) // HD or Kinect 2 { // isHDColor = true; if(color.cols == 1280) { color = color(cv::Rect(0, depthOffset, 1280, 960)); cameraInfo.K[5] -= depthOffset; cameraInfo.height = 960; } cameraInfoHD = cameraInfo; cameraInfo.height /= 2.0; cameraInfo.width /= 2.0; cameraInfo.roi.height /= 2.0; cameraInfo.roi.width /= 2.0; cameraInfo.roi.x_offset /= 2.0; cameraInfo.roi.y_offset /= 2.0; cameraInfo.K[0] /= 2.0; cameraInfo.K[2] /= 2.0; cameraInfo.K[4] /= 2.0; cameraInfo.K[5] /= 2.0; cameraInfo.P[0] /= 2.0; cameraInfo.P[2] /= 2.0; cameraInfo.P[5] /= 2.0; cameraInfo.P[6] /= 2.0; } else if(color.cols == 640) { // isHDColor = false; cameraInfoHD = cameraInfo; cameraInfoHD.height *= 2.0; cameraInfoHD.width *= 2.0; cameraInfoHD.roi.height *= 2.0; cameraInfoHD.roi.width *= 2.0; cameraInfoHD.roi.x_offset *= 2.0; cameraInfoHD.roi.y_offset *= 2.0; cameraInfoHD.K[0] *= 2.0; cameraInfoHD.K[2] *= 2.0; cameraInfoHD.K[4] *= 2.0; cameraInfoHD.K[5] *= 2.0; cameraInfoHD.P[0] *= 2.0; cameraInfoHD.P[2] *= 2.0; cameraInfoHD.P[5] *= 2.0; cameraInfoHD.P[6] *= 2.0; } else if(color.cols == 512)//512*424 { // isHDColor = false; cameraInfoHD = cameraInfo; cameraInfoHD.height *= 3.75; cameraInfoHD.width *= 3.75; cameraInfoHD.roi.height *= 3.75; cameraInfoHD.roi.width *= 3.75; cameraInfoHD.roi.x_offset *= 3.75; cameraInfoHD.roi.y_offset *= 3.75; cameraInfoHD.K[0] *= 3.75; cameraInfoHD.K[2] *= 3.75; cameraInfoHD.K[4] *= 3.75; cameraInfoHD.K[5] *= 3.75; cameraInfoHD.P[0] *= 3.75; cameraInfoHD.P[2] *= 3.75; cameraInfoHD.P[5] *= 3.75; cameraInfoHD.P[6] *= 3.75; } else { outError("Unknown color image size!"); return; } lock.lock(); this->color = color; this->depth = depth; this->cameraInfo = cameraInfo; this->cameraInfoHD = cameraInfoHD; _newData = true; // outWarn("new data"); lock.unlock(); }
double RateFree::optimizeWithEM() { size_t ptn, c; size_t nptn = phylo_tree->aln->getNPattern(); size_t nmix = ncategory; const double MIN_PROP = 1e-4; // double *lk_ptn = aligned_alloc<double>(nptn); double *new_prop = aligned_alloc<double>(nmix); PhyloTree *tree = new PhyloTree; // attach memory to save space // tree->central_partial_lh = phylo_tree->central_partial_lh; // tree->central_scale_num = phylo_tree->central_scale_num; // tree->central_partial_pars = phylo_tree->central_partial_pars; tree->copyPhyloTree(phylo_tree); tree->optimize_by_newton = phylo_tree->optimize_by_newton; tree->setParams(phylo_tree->params); tree->setLikelihoodKernel(phylo_tree->sse); tree->setNumThreads(phylo_tree->num_threads); // initialize model ModelFactory *model_fac = new ModelFactory(); model_fac->joint_optimize = phylo_tree->params->optimize_model_rate_joint; // model_fac->unobserved_ptns = phylo_tree->getModelFactory()->unobserved_ptns; RateHeterogeneity *site_rate = new RateHeterogeneity; tree->setRate(site_rate); site_rate->setTree(tree); model_fac->site_rate = site_rate; tree->model_factory = model_fac; tree->setParams(phylo_tree->params); double old_score = 0.0; // EM algorithm loop described in Wang, Li, Susko, and Roger (2008) for (int step = 0; step < ncategory; step++) { // first compute _pattern_lh_cat double score; score = phylo_tree->computePatternLhCat(WSL_RATECAT); if (score > 0.0) { phylo_tree->printTree(cout, WT_BR_LEN+WT_NEWLINE); writeInfo(cout); } ASSERT(score < 0); if (step > 0) { if (score <= old_score-0.1) { phylo_tree->printTree(cout, WT_BR_LEN+WT_NEWLINE); writeInfo(cout); cout << "Partition " << phylo_tree->aln->name << endl; cout << "score: " << score << " old_score: " << old_score << endl; } ASSERT(score > old_score-0.1); } old_score = score; memset(new_prop, 0, nmix*sizeof(double)); // E-step // decoupled weights (prop) from _pattern_lh_cat to obtain L_ci and compute pattern likelihood L_i for (ptn = 0; ptn < nptn; ptn++) { double *this_lk_cat = phylo_tree->_pattern_lh_cat + ptn*nmix; double lk_ptn = phylo_tree->ptn_invar[ptn]; for (c = 0; c < nmix; c++) { lk_ptn += this_lk_cat[c]; } ASSERT(lk_ptn != 0.0); lk_ptn = phylo_tree->ptn_freq[ptn] / lk_ptn; // transform _pattern_lh_cat into posterior probabilities of each category for (c = 0; c < nmix; c++) { this_lk_cat[c] *= lk_ptn; new_prop[c] += this_lk_cat[c]; } } // M-step, update weights according to (*) int maxpropid = 0; double new_pinvar = 0.0; for (c = 0; c < nmix; c++) { new_prop[c] = new_prop[c] / phylo_tree->getAlnNSite(); if (new_prop[c] > new_prop[maxpropid]) maxpropid = c; } // regularize prop bool zero_prop = false; for (c = 0; c < nmix; c++) { if (new_prop[c] < MIN_PROP) { new_prop[maxpropid] -= (MIN_PROP - new_prop[c]); new_prop[c] = MIN_PROP; zero_prop = true; } } // break if some probabilities too small if (zero_prop) break; bool converged = true; double sum_prop = 0.0; for (c = 0; c < nmix; c++) { // new_prop[c] = new_prop[c] / phylo_tree->getAlnNSite(); // check for convergence sum_prop += new_prop[c]; converged = converged && (fabs(prop[c]-new_prop[c]) < 1e-4); prop[c] = new_prop[c]; new_pinvar += new_prop[c]; } new_pinvar = 1.0 - new_pinvar; if (new_pinvar > 1e-4 && getPInvar() != 0.0) { converged = converged && (fabs(getPInvar()-new_pinvar) < 1e-4); if (isFixPInvar()) outError("Fixed given p-invar is not supported"); setPInvar(new_pinvar); // setOptimizePInvar(false); phylo_tree->computePtnInvar(); } ASSERT(fabs(sum_prop+new_pinvar-1.0) < MIN_PROP); // now optimize rates one by one double sum = 0.0; for (c = 0; c < nmix; c++) { tree->copyPhyloTree(phylo_tree); ModelMarkov *subst_model; if (phylo_tree->getModel()->isMixture() && phylo_tree->getModelFactory()->fused_mix_rate) subst_model = (ModelMarkov*)phylo_tree->getModel()->getMixtureClass(c); else subst_model = (ModelMarkov*)phylo_tree->getModel(); tree->setModel(subst_model); subst_model->setTree(tree); model_fac->model = subst_model; if (subst_model->isMixture() || subst_model->isSiteSpecificModel() || !subst_model->isReversible()) tree->setLikelihoodKernel(phylo_tree->sse); // initialize likelihood tree->initializeAllPartialLh(); // copy posterior probability into ptn_freq tree->computePtnFreq(); double *this_lk_cat = phylo_tree->_pattern_lh_cat+c; for (ptn = 0; ptn < nptn; ptn++) tree->ptn_freq[ptn] = this_lk_cat[ptn*nmix]; double scaling = rates[c]; tree->scaleLength(scaling); tree->optimizeTreeLengthScaling(MIN_PROP, scaling, 1.0/prop[c], 0.001); converged = converged && (fabs(rates[c] - scaling) < 1e-4); rates[c] = scaling; sum += prop[c] * rates[c]; // reset subst model tree->setModel(NULL); subst_model->setTree(phylo_tree); } phylo_tree->clearAllPartialLH(); if (converged) break; } // sort the rates in increasing order if (sorted_rates) quicksort(rates, 0, ncategory-1, prop); // deattach memory // tree->central_partial_lh = NULL; // tree->central_scale_num = NULL; // tree->central_partial_pars = NULL; delete tree; aligned_free(new_prop); return phylo_tree->computeLikelihood(); }
void Hokuyo::open(const char *deviceString, cvg_int bps) { close(); if (urg_connect(&urg, deviceString, bps) < 0) outError("Unable to connect to device"); started = true; }