int main()
{
if(!lcm1.good()) return 1;
VideoCapture cap(1);
cap >> frame;
start_time = utime_now();
float x=0,y=0,z=0;
int estPixel;
//Temporary local variables
Rect trackWindow;
int hsize = 16;
float hranges[] = {0,180};
const float* phranges = hranges;
Mat hsv, hue, mask, hist, histimg = Mat::zeros(200, 320, CV_8UC3), backproj;
bool paused = false;
int _vmin,_vmax;
namedWindow( "CamShift Demo", 1 );
setMouseCallback( "CamShift Demo", onMouse, 0 );
while(!frame.empty())
{
RotatedRect trackBox;
if( !paused )
{
//resize(frame,frame,Size(FRAME_W,FRAME_H));
frame_number = cap.get(CV_CAP_PROP_POS_FRAMES);
}
frame.copyTo(image);
if( !paused )
{
cvtColor(image, hsv, COLOR_BGR2HSV);
if( trackObject )
{
_vmin = CAM_VMIN, _vmax = CAM_VMAX;
inRange(hsv, Scalar(0, CAM_SMIN, MIN(_vmin,_vmax)),
Scalar(180, 256, MAX(_vmin, _vmax)), mask);
int ch[] = {0, 0};
hue.create(hsv.size(), hsv.depth());
mixChannels(&hsv, 1, &hue, 1, ch, 1);
if( trackObject < 0 )
{
destroyAllWindows();
Mat roi(hue, selection), maskroi(mask, selection);
calcHist(&roi, 1, 0, maskroi, hist, 1, &hsize, &phranges);
normalize(hist, hist, 0, 255, CV_MINMAX);
trackWindow = selection;
trackObject = 1;
histimg = Scalar::all(0);
int binW = histimg.cols / hsize;
Mat buf(1, hsize, CV_8UC3);
for( int i = 0; i < hsize; i++ )
buf.at<Vec3b>(i) = Vec3b(saturate_cast<uchar>(i*180./hsize), 255, 255);
cvtColor(buf, buf, CV_HSV2BGR);
for( int i = 0; i < hsize; i++ )
{
int val = saturate_cast<int>(hist.at<float>(i)*histimg.rows/255);
rectangle( histimg, Point(i*binW,histimg.rows),Point((i+1)*binW,histimg.rows - val),Scalar(buf.at<Vec3b>(i)), -1, 8 );
}
}
calcBackProject(&hue, 1, 0, hist, backproj, &phranges);
backproj &= mask;
trackBox = CamShift(backproj, trackWindow,
TermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ));
if( trackWindow.area() <= 1 )
{
int cols = backproj.cols, rows = backproj.rows, r = (MIN(cols, rows) + 5)/6;
trackWindow = Rect(trackWindow.x - r, trackWindow.y - r,trackWindow.x + r, trackWindow.y + r) &Rect(0, 0, cols, rows);
if (trackWindow.area() == 0)
trackWindow = Rect(360,240,100,100);
}
rectangle(image,trackBox.boundingRect(),Scalar(0,0,255),3);
}
if( backprojMode )
cvtColor( backproj, image, COLOR_GRAY2BGR );
else if( trackObject < 0 )
paused = false;
if( selectObject && selection.width > 0 && selection.height > 0 )
{
Mat roi(image, selection);
bitwise_not(roi, roi);
}
imshow( "CamShift Demo", image );
waitKey(10);
char c = (char)waitKey(30);
if( c == 27 )
exit(0);
switch(c)
{
case 'b':
backprojMode = !backprojMode;
break;
case 'c':
trackObject = 0;
histimg = Scalar::all(0);
break;
case 'h':
showHist = !showHist;
if( !showHist )
destroyWindow( "Histogram" );
else
namedWindow( "Histogram", 1 );
break;
case 'p':
paused = !paused;
break;
default:
;
}
if(!paused)
{
estPixel = trackBox.boundingRect().width;
if(estPixel!=1)
{
z = hgtEst(estPixel,0.0025,3.31,177.8);
x = persProj(trackBox.boundingRect().x+((trackBox.boundingRect().width)/2.0), z, 1325.55);
y = persProj(trackBox.boundingRect().y+((trackBox.boundingRect().height)/2.0),z, 1325.55);
}
else
{
x=0;y=0;z=0;
}
cap >> frame;
end_time = utime_now();
time_diff = end_time - start_time;
// cout<<time_diff<<endl;
start_time = end_time;
vision_msg.timestamp = utime_now();
vision_msg.velocity[0] = (vision_msg.position[0] - x)/time_diff*1000;
vision_msg.velocity[1] = (vision_msg.position[1] - y)/time_diff*1000;
vision_msg.velocity[2] = (vision_msg.position[2] - z)/time_diff*1000;
vision_msg.position[0] = x;
vision_msg.position[1] = y;
vision_msg.position[2] = z;
lcm1.publish("vision state", &vision_msg);
if( frame.empty() )
break;
}
}
int main() {
if(!lcm1.good())
return 1;
lcm1.subscribe("vicon_state", &Handler::handleMessage, &handlerObject);
initialize_Matrices();
// initial conditions
mat tmp0;
tmp0 = {0, 0, 0, 1.2, 0, 0, 1.3, 0, 0, 0, 0, 0};
tmp0 = trans(tmp0);
mat z0(12*HORIZON, 1);
mat beq(9*HORIZON,1);
x0 = {1, 0, 0, 1, 0, 0, 0, 0, 0};
x0 = trans(x0);
//for testing purpose
float noise_mu = 0.0001;
mat noise = noise_mu * x0;
mat beqFirstHorizon = A_d_all*x0 + noise;
for(int i = 0; i < 9; i++) {
beq(i,0) = beqFirstHorizon(i, 0);
}
for(int i = 0; i < 12; i ++) {
z0(i,0) = tmp0(i,0);
}
// set the values of entrices of beq for other horizons
for(int i = 9; i < HORIZON*9; i ++) {
beq(i, 0) = noise(i%9,0);
}
for (int i = 12; i < HORIZON*12; i++) {
z0(i,0) = tmp0(i%12,0);
}
mat v0 = 0.05 * randi<mat>(9*HORIZON, 1, distr_param(0, 20));
mat z = z0;
mat v = v0;
vec r(21);
r.ones();
vec r_d(12*HORIZON);
vec r_p(9*HORIZON);
float w_r_p = 1e-4;
float w_d_p = 1e-4;
int loop_num = 0;
float fval;
float x, y, z_h;
mat HESSIAN, GRADIENT;
float u1, u2, u3, h;
float tmp_denum_grad_x_1, tmp_denum_grad_x_2, tmp_denum_grad_y_1, tmp_denum_grad_y_2;
float d_phi_dx, d_phi_dy, d_phi_dz;
float d_phi_du1, d_phi_du2, d_phi_du3;
float tmp_denum_x_1, tmp_denum_x_2, tmp_denum_y_1, tmp_denum_y_2;
float dd_phi_dxdx, dd_phi_dxdz, dd_phi_dydy, dd_phi_dydz, dd_phi_dzdz;
float dd_phi_du1du1, dd_phi_du2du2, dd_phi_du3du3;
// infeasible start newton method params
vec d_v, d_z, beta;
mat inv_PHI, Y;
//linear search parameters
float beta_line_search; //parameter
float d; //step size
vec line_search_tmp;
clock_t start;
double duration;
start = clock();
thread first(updateStateThread);
while(1) {
start = clock();
beqFirstHorizon = A_d_all*x0 + noise;
for(int i = 0; i < 9; i++) {
beq(i,0) = beqFirstHorizon(i, 0);
}
while(norm(r) > 1e-4) {
duration = ( clock() - start ) / (double) CLOCKS_PER_SEC;
if(duration > 0.2) {
continue;
}
fval = dot(trans(z), (H * z));
HESSIAN.zeros(12*HORIZON, 12*HORIZON);
GRADIENT.zeros(12*HORIZON, 1);
for (int i = 0; i < HORIZON; i++) {
x = z.at(3 + i * 12);
y = z.at(6 + i * 12);
z_h = z.at(9 + i * 12);
u1 = z.at(0 + 12 * i);
u2 = z.at(1 + 12 * i);
u3 = z.at(2 + 12 * i);
h = -z_h + better_height;
tmp_denum_grad_x_1 = h*tan_half_phi - x;
tmp_denum_grad_x_2 = h*tan_half_phi + x;
tmp_denum_grad_y_1 = h*tan_half_psi - y;
tmp_denum_grad_y_2 = h*tan_half_psi + y;
d_phi_dx = 1/tmp_denum_grad_x_1 - 1/tmp_denum_grad_x_2;
d_phi_dy = 1/tmp_denum_grad_y_1 - 1/tmp_denum_grad_y_2;
d_phi_dz = 1/(z_max - z_h) - 1/(z_h - z_min) + tan_half_phi/tmp_denum_grad_x_1 + tan_half_phi/tmp_denum_grad_x_2
+ tan_half_psi/tmp_denum_grad_y_1 + tan_half_psi/tmp_denum_grad_y_2;
d_phi_du1 = 1/(u1_max - u1) - 1/(u1 - u1_min);
d_phi_du2 = 1/(u2_max - u2) - 1/(u2 - u2_min);
d_phi_du3 = 1/(u3_max - u3) - 1/(u3 - u3_min);
GRADIENT.at(i*12) = mu_u*d_phi_du1;
GRADIENT.at(i*12 + 1) = mu_u*d_phi_du2;
GRADIENT.at(i*12 + 2) = mu_u*d_phi_du3;
GRADIENT.at(i*12 + 3) = mu_state*d_phi_dx;
GRADIENT.at(i*12 + 6) = mu_state*d_phi_dy;
GRADIENT.at(i*12 + 9) = mu_state*d_phi_dz;
tmp_denum_x_1 = (h*tan_half_phi - x)*(h*tan_half_phi - x);
tmp_denum_x_2 = (h*tan_half_phi + x)*(h*tan_half_phi + x);
tmp_denum_y_1 = (h*tan_half_psi - y)*(h*tan_half_psi - y);
tmp_denum_y_2 = (h*tan_half_psi + y)*(h*tan_half_psi + y);
dd_phi_dxdx = 1/tmp_denum_x_1 + 1/tmp_denum_x_2;
dd_phi_dxdz = tan_half_phi/tmp_denum_x_1 - tan_half_phi/tmp_denum_x_2;
dd_phi_dydy = 1/tmp_denum_y_1 + 1/tmp_denum_y_2;
dd_phi_dydz = tan_half_psi/tmp_denum_y_1 - tan_half_psi/tmp_denum_y_2;
dd_phi_dzdz = 1/(z_max - z_h)/(z_max - z_h) + 1/(z_h - z_min)/(z_h - z_min) + tan_half_phi*tan_half_phi/tmp_denum_x_1
+ tan_half_phi*tan_half_phi/tmp_denum_x_2 + tan_half_psi*tan_half_psi/tmp_denum_y_1 + tan_half_psi*tan_half_psi/tmp_denum_y_2;
dd_phi_du1du1 = 1/(u1_max - u1)/(u1_max - u1) + 1/(u1 - u1_min)/(u1 - u1_min);
dd_phi_du2du2 = 1/(u2_max - u2)/(u2_max - u2) + 1/(u2 - u2_min)/(u2 - u2_min);
dd_phi_du3du3 = 1/(u3_max - u3)/(u3_max - u3) + 1/(u3 - u3_min)/(u3 - u3_min);
HESSIAN.at(12*i, 12*i) = mu_u*dd_phi_du1du1;
HESSIAN.at(12*i + 1, 12*i + 1) = mu_u*dd_phi_du2du2;
HESSIAN.at(12*i + 2, 12*i + 2) = mu_u*dd_phi_du3du3;
HESSIAN.at(12*i + 3, 12*i + 3) = mu_state*dd_phi_dxdx;
HESSIAN.at(12*i + 3, 12*i + 9) = mu_state*dd_phi_dxdz;
HESSIAN.at(12*i + 6, 12*i + 6) = mu_state*dd_phi_dydy;
HESSIAN.at(12*i + 6, 12*i + 9) = mu_state*dd_phi_dydz;
HESSIAN.at(12*i + 9, 12*i + 9) = mu_state*dd_phi_dzdz;
HESSIAN.at(12*i + 9, 12*i + 3) = mu_state*dd_phi_dxdz;
HESSIAN.at(12*i + 9, 12*i + 6) = mu_state*dd_phi_dydz;
}
r_d = 2*H*z + GRADIENT + trans(C)*v;
r_p = C*z - beq;
r = r_d;
r.insert_rows(12*HORIZON, r_p);
// start = clock();
inv_PHI = inv(2*H + HESSIAN);
// duration = ( clock() - start ) / (double) CLOCKS_PER_SEC;
// cout<<"duration: " <<duration <<endl;
// return 0;
// inv_PHI.print("inv_PHI");
// C.print("C");
Y = C*inv_PHI*C.t();
// Y.print("Y");
beta = -r_p + C*inv_PHI*r_d;
d_v = -inv_sympd(Y)*beta;
d_z = inv_PHI*(-r_d - C.t()*d_v);
// line search
beta_line_search = 0.5;
d = 1;
line_search_tmp = 2*H*(z + d*d_z) + GRADIENT + C.t()*(v + d*d_v);
line_search_tmp.insert_rows(line_search_tmp.n_rows, (C*(z + d*d_z)-beq));
while(norm(line_search_tmp) > (1 - 0.4*d)*norm(r) ) {
d = beta_line_search*d;
}
// update z and v
z = z + d*d_z;
v = v + d*d_v;
// r.print("r");
// cout<<__LINE__<<endl;
}
accel_msg.timestamp = utime_now();
accel_msg.accel[0] = z(0);
accel_msg.accel[1] = z(1);
accel_msg.accel[2] = z(2);
lcm1.publish("acceleration", &accel_msg);
cout<<"duration: " <<duration <<endl;
r.ones();
usleep(5e4);
}
first.join();
cout<<"control:u1 u2 u3 " <<z(0,0)<<", " <<z(1,0) <<", " <<z(2,0)<<endl;
}
