int test_rotate90() { #ifdef _MSC_VER cv::Mat matSrc = cv::imread("E:/GitCode/OpenCV_Test/test_images/1.jpg", 1); #else cv::Mat matSrc = cv::imread("test_images/1.jpg", 1); #endif if (!matSrc.data) { std::cout << "read image fail" << std::endl; return -1; } int width = matSrc.cols; int height = matSrc.rows; fbc::Mat_<uchar, 3> mat1(height, width, matSrc.data); fbc::Mat_<uchar, 3> matTranspose(width, height); fbc::transpose(mat1, matTranspose); // clockwise rotation 90 fbc::Mat_<uchar, 3> matRotate90(width, height); fbc::flip(matTranspose, matRotate90, 1); cv::Mat tmp2(width, height, CV_8UC3, matRotate90.data); #ifdef _MSC_VER cv::imwrite("E:/GitCode/OpenCV_Test/test_images/rotate_90.jpg", tmp2); #else cv::imwrite("test_images/rotate_90.jpg", tmp2); #endif // clockwise rotation 180 fbc::Mat_<uchar, 3> matRotate180(height, width); fbc::flip(mat1, matRotate180, -1); cv::Mat tmp3(height, width, CV_8UC3, matRotate180.data); #ifdef _MSC_VER cv::imwrite("E:/GitCode/OpenCV_Test/test_images/rotate_180.jpg", tmp3); #else cv::imwrite("test_images/rotate_180.jpg", tmp3); #endif // clockwise rotation 270 fbc::Mat_<uchar, 3> matRotate270(width, height); fbc::flip(matTranspose, matRotate270, 0); cv::Mat tmp4(matTranspose.rows, matTranspose.cols, CV_8UC3, matRotate270.data); #ifdef _MSC_VER cv::imwrite("E:/GitCode/OpenCV_Test/test_images/rotate_270.jpg", tmp4); #else cv::imwrite("test_images/rotate_270.jpg", tmp4); #endif return 0; }
void kfUpdate(float *xl, float *omega) { static float phi = 0; static float theta = 0; static float psi = 0; float dphi, dtheta, dP; float sin_theta; float cos_phi = cos(phi); float sin_phi = sin(phi); float cos_theta = cos(theta); float tan_theta = tan(theta); float a_values[] = { -wy*cos_phi*tan_theta + wz*sin_phi*tan_theta, (-wy*sin_phi + wz*cos_phi) / (cos_theta*cos_theta), wy*sin_phi - wz*cos_phi, 0 }; float c_values[6]; float h_values[3]; matAssignValues(A, a_values); dphi = wx - wy*sin_phi*tan_theta - wz*cos_phi*tan_theta; dtheta = -wy*cos_phi - wz*sin_phi; phi = phi + dphi * TIME_STEP; theta = theta + dtheta * TIME_STEP; // computing dP = APA' + Q matTranspose(temp2x2, A); // A' matDotProduct(temp2x2, P, temp2x2); // PA' matDotProduct(temp2x2, A, temp2x2); // APA' matAdd(dP, temp2x2, Q); // APA' + Q // computing P = P + dt*dP matScale(temp2x2, dP, TIME_STEP); // dt*dP matAdd(P, P, temp2x2); // P + dt*dP cos_phi = cos(phi); sin_phi = sin(phi); cos_theta = cos(theta); sin_theta = sin(theta); c_values = {0, cos_theta, -cos_theta*cos_phi, sin_theta*sin_phi, cos_theta*sin_phi, sin_theta*cos_phi } matAssignValues(C, c_values); // L = PC'(R + CPC')^-1 matTranspose(temp2x3, C); // C' matDotProduct(temp2x3, P, temp2x3); // PC' matDotProduct(temp3x3, C, temp2x3); // CPC' matAdd(temp3x3, R, temp3x3); // R + CPC' matInverse(temp3x3, temp3x3); // (R + CPC')^-1 matDotProduct(L, temp2x3, temp3x3); // PC'(R + CPC')^-1 // P = (I - LC)P matDotProduct(temp2x2, L, C); // LC matSub(temp2x2, I, temp2x2); // I - LC matDotProduct(P, temp2x2, P); // (I - LC)P h_values = {sin_theta, -cos_theta*sin_phi, -cos_theta*cos_phi}; matAssignValues(H, h_values); /* ph = ph + dot(L[0], self.ab - h) th = th + dot(L[1], self.ab - h) */ // change the values so that they stay between -PI and +PI phi = ((phi + PI) % (2*PI)) - PI; theta = ((theta + PI) % (2*PI)) - PI; psidot = wy * sin(ph) / cos(th) + * cos(ph) / cos(th); psi += psidot * TIME_STEP; }