//------------------------------------------------------------------------------
void Triangulation::stereo_triangulation(const double xL[2], const double xR[2],
const cv::Mat& R, const cv::Mat& T, const double fc_left[2], const double cc_left[2],
const double kc_left[5], const double fc_right[2], const double cc_right[2],
const double kc_right[5], double XW[3], const bool undistortPoint)
{
// Normalize the image projection according to the intrinsic parameters of the left
// and right cameras.
double xt_[2], xtt_[2];
normalize_pixel(xL, fc_left, cc_left, kc_left, xt_, undistortPoint);
normalize_pixel(xR, fc_right, cc_right, kc_right, xtt_, undistortPoint);
// Extend the normalized projections in homogeneous coordinates
cv::Mat xt(3, 1, CV_64FC1);
cv::Mat xtt(3, 1, CV_64FC1);
xt.at<double>(0,0) = xt_[0]; xt.at<double>(1,0) = xt_[1]; xt.at<double>(2,0) = 1.0;
xtt.at<double>(0,0) = xtt_[0]; xtt.at<double>(1,0) = xtt_[1]; xtt.at<double>(2,0) = 1.0;
// Triangulation of the rays in 3D space:
cv::Mat u = R * xt;
double n_xt2 = xt.dot(xt);
double n_xtt2 = xtt.dot(xtt);
double dot_uxtt = u.dot(xtt);
double DD = n_xt2 * n_xtt2 - dot_uxtt*dot_uxtt;
double dot_uT = u.dot(T);
double dot_xttT = xtt.dot(T);
double dot_xttu = u.dot(xtt);
double NN1 = dot_xttu*dot_xttT - n_xtt2 * dot_uT;
double NN2 = n_xt2*dot_xttT - dot_uT*dot_xttu;
double Zt = NN1/DD;
double Ztt = NN2/DD;
cv::Mat X1 = xt * Zt;
// Tranpose of rotation matrix
cv::Mat Rt(3, 3, CV_64FC1);
cv::transpose(R, Rt);
cv::Mat X2 = Rt * (xtt*Ztt - T);
cv::Mat Xworld = 0.5 * (X1 + X2);
XW[0] = Xworld.at<double>(0,0);
XW[1] = Xworld.at<double>(1,0);
XW[2] = Xworld.at<double>(2,0);
}
/* this function takes in the UArray_T of the pixels, and outputs the
Y, Pb, Pr representation
*/
CVC *rgb_pixels_to_CVC(UArray_T block, int denominator)
{
int num_pixels = UArray_length(block);
float avg_Pb = 0.0;
float avg_Pr = 0.0;
Pnm_rgb cur_pix;
Pnm_rgb_float cur_pix_float;
CVC *YPbPr = malloc(sizeof(struct CVC));
assert(YPbPr);
YPbPr->Y = malloc(sizeof(Lum_vals) * num_pixels);
assert(YPbPr->Y);
YPbPr->num_vals = num_pixels;
for (int i = 0; i < num_pixels; i++) {
cur_pix = (Pnm_rgb)UArray_at(block, i);
cur_pix_float = normalize_pixel(cur_pix, denominator);
YPbPr->Y[i] = get_Y(cur_pix_float);
avg_Pb += get_Pb(cur_pix_float);
avg_Pr += get_Pr(cur_pix_float);
free(cur_pix_float);
}
YPbPr->avg_Pb = avg_Pb / (float)num_pixels;
YPbPr->avg_Pr = avg_Pr / (float)num_pixels;
return YPbPr;
}
