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;
}
