示例#1
0
hMatrix Inverse_Kinematics(hMatrix Initial_T,hMatrix Goal_T,double *Initial_t, double *DH_alpha, double *DH_a, double *DH_d, int joint){

	for(int i=0; i<joint; i++){
		Initial_theta[i] = *Initial_t;
		Initial_t++;
	}

	hMatrix Initial_Theta(7,1);
	hMatrix J(6,7), Pinv_J(7,6);
	hMatrix n_a(3,1),s_a(3,1),a_a(3,1),n_t(3,1),s_t(3,1),a_t(3,1),p_del(3,1);
	double x,y,z,rx,ry,rz;
	double error_position[3]= {Goal_T.element(0,3)-Initial_T.element(0,3),Goal_T.element(1,3)-Initial_T.element(1,3),Goal_T.element(2,3)-Initial_T.element(2,3)};
	hMatrix P(3,1),R(3,1),Rotation(3,3),dx_temp1(3,1),dx_temp2(3,1),dX(6,1),del_Theta(7,1),Temp(7,1);


	Initial_Theta.SET(7,1,Initial_theta);

			Initial_T = T_hMatrix(&Initial_theta[0], &DH_alpha[0], &DH_a[0], &DH_d[0], joint);
			J = Jacobian_hMatrix(&Initial_theta[0], &DH_alpha[0], &DH_a[0], &DH_d[0]);
			Pinv_J = Pseudo_Inverse(J);

			for(int i = 0; i<3; i++){
				n_a.SetElement(i,0,Initial_T.element(i,0));
				s_a.SetElement(i,0,Initial_T.element(i,1));
				a_a.SetElement(i,0,Initial_T.element(i,2));
				n_t.SetElement(i,0,Goal_T.element(i,0));
				s_t.SetElement(i,0,Goal_T.element(i,1));
				a_t.SetElement(i,0,Goal_T.element(i,2));
				p_del.SetElement(i,0,Goal_T.element(i,3)-Initial_T.element(i,3));
			}
			
			x = dot(n_a, p_del); 
			y = dot(s_a, p_del); 
			z = dot(a_a, p_del); ;
			rx = (dot(a_a,s_t)-dot(a_t,s_a))/2;
			ry = (dot(n_a,a_t)-dot(n_t,a_a))/2;
			rz = (dot(s_a,n_t)-dot(s_t,n_a))/2;

			double dx_P[3] = {x,y,z},dx_R[3] = {rx,ry,rz};

			P.SET(3,1,&dx_P[0]);
			R.SET(3,1,&dx_R[0]);

			Rotation = T_Rotation(Initial_T);
			dx_temp1 = Rotation*P;
			dx_temp2 = Rotation*R;

			for(int i =0; i<3; i++){
				dX.SetElement(i,0,dx_temp1.element(i,0));
				dX.SetElement(i+3,0,dx_temp2.element(i,0));
			}
			
			del_Theta = Pinv_J*dX;

			for(int i=0; i<joint; i++)
				Temp.SetElement(i,0,Initial_Theta.element(i,0) + del_Theta.element(i,0));
			Initial_Theta = Temp;
	
	return Initial_Theta;
}
示例#2
0
/*
* Фильтрация облачности
*/
int TFiltr::filtr_processing( struct TFiltrParams &p,
							  TBlk0_AVHRR &in_blk0,  short *in_data,
							  TBlk0_AVHRR &in2_blk0, short *in2_data,
							  TBlk0_AVHRR &in3_blk0, short *in3_data,
							  TBlk0_AVHRR &in4_blk0, short *in4_data,
							  TBlk0_AVHRR &in5_blk0, short *in5_data,
							  TBlk0_AVHRR &out_blk0, short *out_data,
							  int *filtr_stat ) {
	// Перевод параметров в статические члены класса
	TFiltr::p = p;
	TFiltr::in_blk0 = in_blk0;
	TFiltr::in2_blk0 = in2_blk0;
	TFiltr::in3_blk0 = in3_blk0;
	TFiltr::in4_blk0 = in4_blk0;
	TFiltr::in5_blk0 = in5_blk0;
	TFiltr::out_blk0 = out_blk0;
	TFiltr::in_data = in_data;
	TFiltr::in2_data = in2_data;
	TFiltr::in3_data = in3_data;
	TFiltr::in4_data = in4_data;
	TFiltr::in5_data = in5_data;
	TFiltr::out_data = out_data;
	TFiltr::cols = in_blk0.totalPixNum;
	TFiltr::scans = in_blk0.totalFrameNum;

	static const int step = 128;
	int length = scans * cols;

	// навигационная подсистема
	double julian_date;
	TStraightReferencer* r = navigationSystemInit( (TBlk0&)in_blk0, &julian_date );
	TAutoPtr<TStraightReferencer> a_t(r);

	for( int i = 0; i < length; i++ )
		out_data[i] = 0;

	for (int scan = 0; scan < scans; scan++) {
		int col1 = 0;
		int col2 = col1 + step;
		double ang1 = angle( scan, col1, *r, julian_date );
		double ang2 = angle( scan, col2, *r, julian_date );
		for (int j = 0; j < cols; j++) {
			// Определяем синус угла восхождения на Солнце
			double ang;
			if( j == col1 ) {
				ang = ang1;
			} else if ( j == col2 ) {
				ang = ang2;
			} else if ( j > col2 ) {
				col1 = col2;
				col2 = col1 + step;
				ang1 = ang2;
				ang2 = angle( scan, col2, *r, julian_date );
				ang =  ang1+(j-col1)*(ang2-ang1)/double(step);
			} else {
				ang = ang1+(j-col1)*(ang2-ang1)/double(step);
			}
			// синус угла восхождения на Солнце определен

			int ind = scan*cols + j;
			out_data[ind] = in_data[ind];
			if( in_data[ind] < 0 ) {
				out_data[ind] = in_data[ind];
				if( filtr_stat && in_data[ind] == multichLostValue ) {
					filtr_stat[MASK_MULTICH]++;
				}
			} else {
				//  маска, показывающая каким фильтрам удовлетворил
				//  текущий пиксель
				unsigned int mask_filtr = 0;

				//  Фильтрация по альбедо
				if (p.albedo_flag) {
					if( albedoTest( scan, j, ang ) ) {
						out_data[ind] = lostValue;
						mask_filtr = MASK_ALBEDO_FILTR;
					}
				}

				//  Фильтрация по температуре
				if (p.temp_flag) {
					if( tempTest( scan, j, ang ) ) {
						out_data[ind] = lostValue;
						mask_filtr |= MASK_TEMP_FILTR;
					}
				}

				//  Фильтрация по разности третьего - четвертого каналов
				if (p.day_delta34_flag || p.night_delta34_flag ) {
					if( delta34Test( scan, j, ang ) ) {
						out_data[ind] = lostValue;
						mask_filtr |= MASK_34_DELTA_FILTR;
					}
				}

				//  Фильтрация по разности третьего - пятого каналов
				if (p.day_delta35_flag || p.night_delta35_flag ) {
					if( delta35Test( scan, j, ang ) ) {
						out_data[ind] = lostValue;
						mask_filtr |= MASK_35_DELTA_FILTR;
					}
				}

				//  Фильтрация по разности эго - пятого каналов
				if (p.day_delta45_flag || p.night_delta45_flag ) {
					if( delta45Test( scan, j, ang ) ) {
						out_data[ind] = lostValue;
						mask_filtr |= MASK_45_DELTA_FILTR;
					}
				}

				//  Фильтрация по критерию пространственной неоднородности для температуры
				if (p.temp_uniformity_flag ) {
					if( scan > 0 && scan < scans && j > 0 && j < cols ) {
						if( tempUniformityTest( scan, j, ang ) ) {
							out_data[ind] = lostValue;
							mask_filtr |= MASK_TEMP_SMOOTH_FILTR;
						}
					}
				}

				if (p.albedo_uniformity_flag ) {
					if( scan > 0 && scan < scans && j > 0 && j < cols ) {
						if( albedoUniformityTest( scan, j, ang ) ) {
							out_data[ind] = lostValue;
							mask_filtr |= MASK_ALBEDO_SMOOTH_FILTR;
						}
					}
				}

				if( p.stat_flag && out_data[ind] == lostValue) {
					out_data[ind] = -mask_filtr-256;
				}

				if(filtr_stat )
					filtr_stat[mask_filtr]++;   // Для статистики
			}
		}
	}   // Конец большого цикла по всем пикселям

	if( p.cloud_border_flag ) {
		borderProcessing( p.cloud_border_win_size, p.max_filtered_percent, &(filtr_stat[MASK_BORDER_FILTR]) );
	}
	out_blk0.processLevel |= 4;
	return 0;
}