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