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