void TunerBody::launch(void)
{
ros::NodeHandle n;
ros::Subscriber image_sub = n.subscribe("/image_raw", 1, image_raw_callBack);
ros::Publisher tunedResult_pub = n.advertise <road_wizard::TunedResult> ("tuned_result", ADVERTISE_QUEUE_SIZE, ADVERTISE_LATCH);
/* valiables to check status change */
cv::Point prev_clicked = cv::Point(-1, -1);
int prev_hw = 0;
int prev_sw = 0;
int prev_vw = 0;
while (ros::ok())
{
ros::spinOnce();
if (src_img.empty())
continue;
base = cv::Mat::zeros(src_img.rows, src_img.cols * 2, CV_8UC3);
mask = cv::Mat::zeros(src_img.rows, src_img.cols, CV_8UC1);
cv::Mat result = src_img.clone();
/* get clicked coordinates on the image */
cv::Point targetPoint = Clicked_point;
/* get current slider position */
int hw = H_slider_val;
int sw = S_slider_val;
int vw = V_slider_val;
cv::setMouseCallback(windowName, onMouse);
/* convert input image to HSV color image */
cv::Mat hsv_img;
cv::cvtColor(src_img, hsv_img, cv::COLOR_BGR2HSV);
if (targetPoint != prev_clicked &&
targetPoint != cv::Point(-1, -1))
{
std::cerr << "Selected_set updated" << std::endl;
int hue = (int)hsv_img.at<cv::Vec3b>(targetPoint.y, targetPoint.x)[0];
int sat = (int)hsv_img.at<cv::Vec3b>(targetPoint.y, targetPoint.x)[1];
int val = (int)hsv_img.at<cv::Vec3b>(targetPoint.y, targetPoint.x)[2];
/* save HSV values into correspond variables */
Selected_set->hue.center = hue;
Selected_set->sat.center = sat;
Selected_set->val.center = val;
Selected_set->isUpdated = true;
}
Selected_set->hue.range = hw;
Selected_set->sat.range = sw;
Selected_set->val.range = vw;
if (prev_hw != hw || prev_sw != sw || prev_vw != vw) {
Selected_set->isUpdated = true;
}
colorTrack(hsv_img,
Selected_set->hue.center,
Selected_set->sat.center,
Selected_set->val.center,
Selected_set->hue.range,
Selected_set->sat.range,
Selected_set->val.range,
&mask);
std::vector< std::vector<cv::Point> > contours;
std::vector<cv::Vec4i> hierarchy;
cv::Mat mask_clone = mask.clone();
cv::findContours(mask_clone,
contours,
hierarchy,
CV_RETR_TREE,
CV_CHAIN_APPROX_SIMPLE);
int maxIndex = index_max(contours);
std::vector<cv::Point> hull;
if (maxIndex != -1)
{
convexHull(contours[maxIndex], hull); /*draw round detected area by line*/
drawContours(result, std::vector< std::vector<cv::Point> >(1, hull), 0, CV_RGB(220, 30, 20), 3);
}
/* display result */
cv::Mat roi_result = base(cv::Rect(0, 0, result.cols, result.rows));
result.copyTo(roi_result);
cv::Scalar mask_color;
switch (Signal_color) {
case GREEN:
mask_color = CV_RGB(0, 255, 0);
break;
case YELLOW:
mask_color = CV_RGB(255, 255, 0);
break;
case RED:
mask_color = CV_RGB(255, 0, 0);
break;
}
cv::Mat colorBack(mask.rows, mask.cols, CV_8UC3, mask_color);
cv::Mat mask_colored;
colorBack.copyTo(mask_colored, mask);
cv::Mat roi_mask = base(cv::Rect(result.cols, 0, mask_colored.cols, mask_colored.rows));
mask_colored.copyTo(roi_mask);
cv::imshow(windowName, base);
cv::waitKey(10);
// if ( (cv::waitKey(10)) == '\x1b') { /* if 'ESC' key is typed, finish the program */
// break;
// }
/* save current status */
prev_clicked = targetPoint;
prev_hw = hw;
prev_sw = sw;
prev_vw = vw;
/* publish tuned result */
road_wizard::TunedResult res;
res.Red.Hue.center = Red_set.hue.center;
res.Red.Hue.range = Red_set.hue.range;
res.Red.Sat.center = Red_set.sat.center;
res.Red.Sat.range = Red_set.sat.range;
res.Red.Val.center = Red_set.val.center;
res.Red.Val.range = Red_set.val.range;
res.Yellow.Hue.center = Yellow_set.hue.center;
res.Yellow.Hue.range = Yellow_set.hue.range;
res.Yellow.Sat.center = Yellow_set.sat.center;
res.Yellow.Sat.range = Yellow_set.sat.range;
res.Yellow.Val.center = Yellow_set.val.center;
res.Yellow.Val.range = Yellow_set.val.range;
res.Green.Hue.center = Green_set.hue.center;
res.Green.Hue.range = Green_set.hue.range;
res.Green.Sat.center = Green_set.sat.center;
res.Green.Sat.range = Green_set.sat.range;
res.Green.Val.center = Green_set.val.center;
res.Green.Val.range = Green_set.val.range;
tunedResult_pub.publish(res);
}
cv::destroyAllWindows();
} /* void TunerBody::launch() */
/*
* Compute the following entities:
* - responsibilities r(k|ij)
* - mu_ij_k
* - Lambda_ij_k
* - log determinante Lamda_ij_k
*
* And then use these to compute likelihood and gradients
*/
void Likelihood_Protein::compute_f_df(int hessian_pseudocount)
{
//initialize parameters for second gaussian
mu_ij_k.zeros(400, this->nr_components);
lambda_ij_k.zeros(400, 400, this->nr_components);
lambda_ij_k_inv.zeros(400, 400, this->nr_components);
log_det_lambda_ij_k.zeros(this->nr_components);
responsibilities.zeros(this->nr_components);
//initialize likelihood vector
log_likelihood.zeros(this->number_of_pairs);
//intialize gradients to zero
grad_weight.zeros(this->nr_components, 2);
grad_mu.zeros(400,this-> nr_components);
grad_precMat.zeros(400, this->nr_components);
for(int pair = 0; pair < this->number_of_pairs; pair++){
int i = this->i_indices(pair);
int j = this->j_indices(pair);
int lin_index = i*(L - (i+1)/2.0 - 1) + j - 1; //i*L - i*(i+1)/2 + j-(i+1);
int contact = this->protein_contacts(pair);
//for computation of likelihood
arma::vec log_density(this->nr_components, arma::fill::zeros);
arma::vec vqij = mq_ij.col(lin_index);
double N_ij = mN_ij(i,j);
arma::vec w_ij = w_ij3d.tube(i,j);
//diagonal matrix Qij = diag(q'ji)
//q'ijab = q(x_i=a, x_j=b) - (lambda_w * wijab / N_ij) --> has been precomputed
arma::mat Qij = arma::diagmat(vqij);
//outer product
arma::mat qij_prod = vqij * vqij.t();
//determine negative Hessian = Hij
arma::mat diff = Qij - qij_prod;
arma::mat H_ij = N_ij * diff + regularizer_w; //eq 37
// remove after debugging ---------------------------------------------------------------------------------
//debugging artefacts in learning
//add counts to Hessian to see if we have problems with non-informative data
H_ij.diag() += hessian_pseudocount;
//H_ij = arma::diagmat(H_ij);
// remove after debugging ---------------------------------------------------------------------------------
//precompute product H_ij * wij
arma::vec Hij_wij_prod = H_ij * w_ij;
for(int k = 0; k < this->nr_components; k++){
//gaussian parameters of coupling prior
double weight_k = this->parameters.get_weight(k, contact);
arma::vec mu_k = this->parameters.get_mean(k);
arma::mat lambda_k = this->parameters.get_precMat(k);
//---------------- simplify computation in case that lambda_k is diagonal matrix
// A, A_inv, lambda_k, Qij are diagonal matrices
arma::vec A = N_ij * vqij + lambda_k.diag(); //represents diagonal matrix
arma::vec A_inv = 1.0 / A; //represents diagonal matrix
double log_det_A = arma::sum(arma::log(A));
arma::vec Ainv_qij_product = A_inv % vqij; ////column vector
double triple_product = arma::sum(vqij % Ainv_qij_product);
//---------------- matrix computations in case lambda_k is NOT diagonal matrix
//A is a diagonal matrix, as Qij and lambda_k are diagonal matrices
//arma::mat A = N_ij * Qij + lambda_k; //diagonal
//arma::mat A_inv = arma::diagmat(1.0 / A.diag()); //diagonal
//double log_det_A = arma::sum(arma::log(A.diag()));
//arma::vec Ainv_qij_product = arma::vec(A_inv * vqij); //400x1 dim matrix
//double triple_product = arma::as_scalar(vqij.t() * Ainv_qij_product);
//compute lambda_ij_k_mat
arma::mat lambda_ij_k_mat = H_ij - regularizer_w + lambda_k;
lambda_ij_k.slice(k) = lambda_ij_k_mat;
//debugging: we assume diagonal Hessian ================================================================
// arma::mat lambda_ij_k_mat_inv(400,400,arma::fill::zeros);
// lambda_ij_k_mat_inv.diag() = 1.0 / lambda_ij_k_mat.diag();
//debugging=======================================================================================
//compute inverse of lambda_ij_k_mat
//---------------- simplify computation in case that lambda_k is diagonal matrix
arma::mat lambda_ij_k_mat_inv = arma::diagmat(A_inv) + (Ainv_qij_product * Ainv_qij_product.t()) / (1.0/N_ij - triple_product);
//---------------- matrix computations in case lambda_k is NOT diagonal matrix
//arma::mat lambda_ij_k_mat_inv = A_inv + (Ainv_qij_product * Ainv_qij_product.t()) / (1.0/N_ij - triple_product);
lambda_ij_k_inv.slice(k) = lambda_ij_k_mat_inv;
//compute mu_ij_k from precomputed entities
arma::vec mu_ij_k_vec = lambda_ij_k_mat_inv * ( Hij_wij_prod + lambda_k * mu_k);
mu_ij_k.col(k) = mu_ij_k_vec;
//debugging: we assume diagonal Hessian ================================================================
// log_det_lambda_ij_k(k) = arma::sum(arma::log(lambda_ij_k_mat.diag()));
//debugging=======================================================================================
//save log determinant of lambda_ij_k, see page 16 Saikats theory
log_det_lambda_ij_k(k) = log(1 - N_ij * triple_product) + log_det_A;
//ratio of two gaussians in log space
// N(0 | mu_k, lambda_k)
//------------------------------
// N(0 | mu_ij_k, lambda_ij,k)
double gaussian_ratio_logdensity = log_density_gaussian_ratio( mu_k,
mu_ij_k_vec,
lambda_k,
lambda_ij_k_mat,
this->parameters.get_log_det_inv_covMat(k),
log_det_lambda_ij_k(k) );
if(this->debug > 0 and gaussian_ratio_logdensity>1000){
std::cout << protein_id << " " << i << " " << j << " " << pair << " " << contact << " " << k << std::endl;
std::cout << "Gaussian log density > 1: " << gaussian_ratio_logdensity << std::endl;
std::cout << "A_inv.max() : " << A_inv.max() << std::endl;
arma::uword max_ind = index_max(A_inv);
std::cout << "A(max_ind) : " << A(max_ind) << std::endl;
std::cout << "vqij(max_ind): " << vqij(max_ind) << std::endl;
std::cout << "N_ij: " << N_ij << std::endl;
std::cout << "mu_ij_k_vec(max_ind) : " << mu_ij_k_vec(max_ind) << std::endl;
std::cout << "mu_k(max_ind) : " << mu_k(max_ind) << std::endl;
std::cout << "lambda_ij_k_mat(max_ind, max_ind) : " << lambda_ij_k_mat(max_ind, max_ind) << std::endl;
std::cout << "lambda_ij_k_mat_inv(max_ind, max_ind) : " << lambda_ij_k_mat_inv(max_ind, max_ind) << std::endl;
std::cout << "lambda_k(max_ind, max_ind) : " << lambda_k(max_ind, max_ind) << std::endl;
std::cout << "parameters.get_log_det_inv_covMat(k) : " << this->parameters.get_log_det_inv_covMat(k) << std::endl;
std::cout << "log_det_lambda_ij_k(k) : " << log_det_lambda_ij_k(k) << std::endl;
}
log_density(k) = log(weight_k) + gaussian_ratio_logdensity;
}//end loop over components k
//Johannes suggestion how to precisely compute the responsibilities
double a_max = arma::max(log_density);
this->responsibilities = arma::exp(log_density - a_max);//r_nk = exp( a_nk - a_max)
double sum_resp = arma::sum(this->responsibilities); //sum += r_nk
this->responsibilities /= sum_resp; //normalization of responsibilities, NOT in log space => r_nk /= sum;
//save neg likelihood of current pair
double f = log(sum_resp) + a_max;
if(! std::isnormal(f)) {
std::cout << "ERROR: likelihood cannot be computed for protein " << protein_id << ", i " << i << ", j " << j << " ("<< contact <<"): " << f << std::endl;
std::cout << "Nij: " << N_ij << ", sum_resp: " << sum_resp << ", a_max: " << a_max << std::endl;
for(int k = 0; k < this->nr_components; k++){
std::cout << "component: " << k << ", sum_precMat(k)diag: "<< arma::sum(this->parameters.get_precMat(k).diag()) << ", responsibilty:" << this->responsibilities(k) << ", log_density: " << log_density(k) << ", log_det_lambda_ij_k: " << log_det_lambda_ij_k(k) << std::endl;
}
continue;
} else log_likelihood(pair) = f;
// Compute ALL gradients for ALL components
for(int k = 0; k < this->nr_components; k++){
//weights (either for contact or non_contact)
grad_weight(k, contact) += gradient_weight_comp(k, contact);
//mu
grad_mu.col(k) += gradient_mu_comp(k);
//precMat - diagonal
arma::mat grad_precMat_protein = gradient_precisionMatrix_comp(k);
grad_precMat.col(k) += grad_precMat_protein.diag();
}//end components
}//end loop over ij pairs
}
