void AccessThreadTwo(LockingSharedObject<int> & obj){
  SharedObject<int>::Accessor acc(obj);
  ASSERT_EQ(1,acc.access());
}
inline
void
spop_mean::apply_noalias_fast
  (
        SpMat<typename T1::elem_type>& out,
  const SpProxy<T1>&                   p,
  const uword                          dim
  )
  {
  arma_extra_debug_sigprint();
  
  typedef typename T1::elem_type eT;
  typedef typename T1::pod_type   T;
  
  const uword p_n_rows = p.get_n_rows();
  const uword p_n_cols = p.get_n_cols();
  
  if( (p_n_rows == 0) || (p_n_cols == 0) || (p.get_n_nonzero() == 0) )
    {
    if(dim == 0)  { out.zeros((p_n_rows > 0) ? 1 : 0, p_n_cols); }
    if(dim == 1)  { out.zeros(p_n_rows, (p_n_cols > 0) ? 1 : 0); }
    
    return;
    }
  
  if(dim == 0) // find the mean in each column
    {
    Row<eT> acc(p_n_cols, fill::zeros);
    
    if(SpProxy<T1>::must_use_iterator)
      {
      typename SpProxy<T1>::const_iterator_type it     = p.begin();
      typename SpProxy<T1>::const_iterator_type it_end = p.end();
      
      while(it != it_end)  { acc[it.col()] += (*it);  ++it; }
      
      acc /= T(p_n_rows);
      }
    else
      {
      for(uword col = 0; col < p_n_cols; ++col)
        {
        acc[col] = arrayops::accumulate
          (
          &p.get_values()[p.get_col_ptrs()[col]],
          p.get_col_ptrs()[col + 1] - p.get_col_ptrs()[col]
          ) / T(p_n_rows);
        }
      }
    
    out = acc;
    }
  else
  if(dim == 1)  // find the mean in each row
    {
    Col<eT> acc(p_n_rows, fill::zeros);
    
    typename SpProxy<T1>::const_iterator_type it     = p.begin();
    typename SpProxy<T1>::const_iterator_type it_end = p.end();
    
    while(it != it_end)  { acc[it.row()] += (*it);  ++it; }
    
    acc /= T(p_n_cols);
    
    out = acc;
    }
  
  if(out.is_finite() == false)
    {
    spop_mean::apply_noalias_slow(out, p, dim);
    }
  }
Esempio n. 3
0
void
Image::copyUnProcessedChannelsForPremult(const bool premult,
                                         const bool originalPremult,
                                         const std::bitset<4> processChannels,
                                         const RectI& roi,
                                         const ImagePtr& originalImage)
{
    ReadAccess acc( originalImage.get() );
    int dstRowElements = dstNComps * _bounds.width();
    PIX* dst_pixels = (PIX*)pixelAt(roi.x1, roi.y1);

    assert(dst_pixels);
    const bool doR = !processChannels[0] && (dstNComps >= 2);
    const bool doG = !processChannels[1] && (dstNComps >= 2);
    const bool doB = !processChannels[2] && (dstNComps >= 3);
    const bool doA = !processChannels[3] && (dstNComps == 1 || dstNComps == 4);
    assert(srcNComps == 4 || !originalPremult); // only RGBA can be premult
    assert(dstNComps == 4 || !premult); // only RGBA can be premult
    Q_UNUSED(premult);
    Q_UNUSED(originalPremult);

    for ( int y = roi.y1; y < roi.y2; ++y, dst_pixels += (dstRowElements - (roi.x2 - roi.x1) * dstNComps) ) {
        for (int x = roi.x1; x < roi.x2; ++x, dst_pixels += dstNComps) {
            const PIX* src_pixels = originalImage ? (const PIX*)acc.pixelAt(x, y) : 0;
            PIX srcA = src_pixels ? maxValue : 0; /* be opaque for anything that doesn't contain alpha */
            if ( ( (srcNComps == 1) || (srcNComps == 4) ) && src_pixels ) {
#             ifdef DEBUG
                assert(src_pixels[srcNComps - 1] == src_pixels[srcNComps - 1]); // check for NaN
#             endif
                srcA = src_pixels[srcNComps - 1];
            }
#         ifdef NATRON_COPY_CHANNELS_UNPREMULT
            PIX dstAorig = maxValue;
#         endif
            if ( (dstNComps == 1) || (dstNComps == 4) ) {
#             ifdef DEBUG
                assert(dst_pixels[dstNComps - 1] == dst_pixels[dstNComps - 1]); // check for NaN
#             endif
#             ifdef NATRON_COPY_CHANNELS_UNPREMULT
                dstAorig = dst_pixels[dstNComps - 1];
#             endif
            }
            if (doR) {
#             ifdef DEBUG
                assert(!src_pixels || src_pixels[0] == src_pixels[0]); // check for NaN
                assert(dst_pixels[0] == dst_pixels[0]); // check for NaN
#             endif
                DOCHANNEL(0);
#             ifdef DEBUG
                assert(dst_pixels[0] == dst_pixels[0]); // check for NaN
#             endif
            }
            if (doG) {
#             ifdef DEBUG
                assert(!src_pixels || src_pixels[1] == src_pixels[1]); // check for NaN
                assert(dst_pixels[1] == dst_pixels[1]); // check for NaN
#             endif
                DOCHANNEL(1);
#             ifdef DEBUG
                assert(dst_pixels[1] == dst_pixels[1]); // check for NaN
#             endif
            }
            if (doB) {
#             ifdef DEBUG
                assert(!src_pixels || src_pixels[2] == src_pixels[2]); // check for NaN
                assert(dst_pixels[2] == dst_pixels[2]); // check for NaN
#             endif
                DOCHANNEL(2);
#             ifdef DEBUG
                assert(dst_pixels[2] == dst_pixels[2]); // check for NaN
#             endif
            }
            if (doA) {
#             ifdef NATRON_COPY_CHANNELS_UNPREMULT
                if (premult) {
                    if (dstAorig != 0) {
                        // unpremult, then premult
                        if ( (dstNComps >= 2) && !doR ) {
                            dst_pixels[0] = (dst_pixels[0] / (float)dstAorig) * srcA;
#                         ifdef DEBUG
                            assert(dst_pixels[0] == dst_pixels[0]); // check for NaN
#                         endif
                        }
                        if ( (dstNComps >= 2) && !doG ) {
                            dst_pixels[1] = (dst_pixels[1] / (float)dstAorig) * srcA;
#                         ifdef DEBUG
                            assert(dst_pixels[1] == dst_pixels[1]); // check for NaN
#                         endif
                        }
                        if ( (dstNComps >= 2) && !doB ) {
                            dst_pixels[2] = (dst_pixels[2] / (float)dstAorig) * srcA;
#                         ifdef DEBUG
                            assert(dst_pixels[2] == dst_pixels[2]); // check for NaN
#                         endif
                        }
                    }
                }
#              endif // NATRON_COPY_CHANNELS_UNPREMULT
                // coverity[dead_error_line]
                dst_pixels[dstNComps - 1] = srcA;
#              ifdef DEBUG
                assert(dst_pixels[dstNComps - 1] == dst_pixels[dstNComps - 1]); // check for NaN
#              endif
            }
        }
    }
} // Image::copyUnProcessedChannelsForPremult
/*
 * EKF Attitude Estimator main function.
 *
 * Estimates the attitude recursively once started.
 *
 * @param argc number of commandline arguments (plus command name)
 * @param argv strings containing the arguments
 */
int attitude_estimator_ekf_thread_main(int argc, char *argv[])
{

	float dt = 0.005f;
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
	float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f};					/**< Measurement vector */
	float x_aposteriori_k[12];		/**< states */
	float P_aposteriori_k[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
				     0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
				     0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,
				     0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,
				     0,   0,   0,   0,  100.f,  0,   0,   0,   0,   0,   0,   0,
				     0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,
				     0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,
				     0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,
				     0,   0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,
				     0,   0,   0,   0,   0,   0,   0,   0,  0.0f, 100.0f,   0,   0,
				     0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   100.0f,   0,
				     0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   0,   100.0f,
				    }; /**< init: diagonal matrix with big values */

	float x_aposteriori[12];
	float P_aposteriori[144];

	/* output euler angles */
	float euler[3] = {0.0f, 0.0f, 0.0f};

	float Rot_matrix[9] = {1.f,  0,  0,
			      0,  1.f,  0,
			      0,  0,  1.f
			     };		/**< init: identity matrix */

	float debugOutput[4] = { 0.0f };

	/* Initialize filter */
	AttitudeEKF_initialize();

	struct sensor_combined_s raw;
	memset(&raw, 0, sizeof(raw));
	struct vehicle_gps_position_s gps;
	memset(&gps, 0, sizeof(gps));
	gps.eph = 100000;
	gps.epv = 100000;
	struct vehicle_global_position_s global_pos;
	memset(&global_pos, 0, sizeof(global_pos));
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	struct vehicle_control_mode_s control_mode;
	memset(&control_mode, 0, sizeof(control_mode));

	uint64_t last_data = 0;
	uint64_t last_measurement = 0;
	uint64_t last_vel_t = 0;

	/* current velocity */
	math::Vector<3> vel;
	vel.zero();
	/* previous velocity */
	math::Vector<3> vel_prev;
	vel_prev.zero();
	/* actual acceleration (by GPS velocity) in body frame */
	math::Vector<3> acc;
	acc.zero();
	/* rotation matrix */
	math::Matrix<3, 3> R;
	R.identity();

	/* subscribe to raw data */
	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
	/* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
	orb_set_interval(sub_raw, 3);

	/* subscribe to GPS */
	int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position));

	/* subscribe to GPS */
	int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position));

	/* subscribe to param changes */
	int sub_params = orb_subscribe(ORB_ID(parameter_update));

	/* subscribe to control mode */
	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));

	/* subscribe to vision estimate */
	int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));

	/* advertise attitude */
	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);

	int loopcounter = 0;

	thread_running = true;

	/* keep track of sensor updates */
	uint64_t sensor_last_timestamp[3] = {0, 0, 0};

	struct attitude_estimator_ekf_params ekf_params;
	memset(&ekf_params, 0, sizeof(ekf_params));

	struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 };

	/* initialize parameter handles */
	parameters_init(&ekf_param_handles);

	bool initialized = false;

	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };

	/* magnetic declination, in radians */
	float mag_decl = 0.0f;

	/* rotation matrix for magnetic declination */
	math::Matrix<3, 3> R_decl;
	R_decl.identity();

	struct vision_position_estimate_s vision {};

	/* register the perf counter */
	perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");

	/* Main loop*/
	while (!thread_should_exit) {

		px4_pollfd_struct_t fds[2];
		fds[0].fd = sub_raw;
		fds[0].events = POLLIN;
		fds[1].fd = sub_params;
		fds[1].events = POLLIN;
		int ret = px4_poll(fds, 2, 1000);

		if (ret < 0) {
			/* XXX this is seriously bad - should be an emergency */
		} else if (ret == 0) {
			/* check if we're in HIL - not getting sensor data is fine then */
			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);

			if (!control_mode.flag_system_hil_enabled) {
				warnx("WARNING: Not getting sensor data - sensor app running?");
			}

		} else {

			/* only update parameters if they changed */
			if (fds[1].revents & POLLIN) {
				/* read from param to clear updated flag */
				struct parameter_update_s update;
				orb_copy(ORB_ID(parameter_update), sub_params, &update);

				/* update parameters */
				parameters_update(&ekf_param_handles, &ekf_params);
			}

			/* only run filter if sensor values changed */
			if (fds[0].revents & POLLIN) {

				/* get latest measurements */
				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);

				bool gps_updated;
				orb_check(sub_gps, &gps_updated);
				if (gps_updated) {
					orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);

					if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
						mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));

						/* update mag declination rotation matrix */
						R_decl.from_euler(0.0f, 0.0f, mag_decl);
					}
				}

				bool global_pos_updated;
				orb_check(sub_global_pos, &global_pos_updated);
				if (global_pos_updated) {
					orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
				}

				if (!initialized) {
					// XXX disabling init for now
					initialized = true;

					// gyro_offsets[0] += raw.gyro_rad_s[0];
					// gyro_offsets[1] += raw.gyro_rad_s[1];
					// gyro_offsets[2] += raw.gyro_rad_s[2];
					// offset_count++;

					// if (hrt_absolute_time() - start_time > 3000000LL) {
					// 	initialized = true;
					// 	gyro_offsets[0] /= offset_count;
					// 	gyro_offsets[1] /= offset_count;
					// 	gyro_offsets[2] /= offset_count;
					// }

				} else {

					perf_begin(ekf_loop_perf);

					/* Calculate data time difference in seconds */
					dt = (raw.timestamp - last_measurement) / 1000000.0f;
					last_measurement = raw.timestamp;
					uint8_t update_vect[3] = {0, 0, 0};

					/* Fill in gyro measurements */
					if (sensor_last_timestamp[0] != raw.timestamp) {
						update_vect[0] = 1;
						// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
						sensor_last_timestamp[0] = raw.timestamp;
					}

					z_k[0] =  raw.gyro_rad_s[0] - gyro_offsets[0];
					z_k[1] =  raw.gyro_rad_s[1] - gyro_offsets[1];
					z_k[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];

					/* update accelerometer measurements */
					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
						update_vect[1] = 1;
						// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
					}

					hrt_abstime vel_t = 0;
					bool vel_valid = false;
					if (gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
						vel_valid = true;
						if (global_pos_updated) {
							vel_t = global_pos.timestamp;
							vel(0) = global_pos.vel_n;
							vel(1) = global_pos.vel_e;
							vel(2) = global_pos.vel_d;
						}
					}

					if (vel_valid) {
						/* velocity is valid */
						if (vel_t != 0) {
							/* velocity updated */
							if (last_vel_t != 0 && vel_t != last_vel_t) {
								float vel_dt = (vel_t - last_vel_t) / 1000000.0f;
								/* calculate acceleration in body frame */
								acc = R.transposed() * ((vel - vel_prev) / vel_dt);
							}
							last_vel_t = vel_t;
							vel_prev = vel;
						}

					} else {
						/* velocity is valid, reset acceleration */
						acc.zero();
						vel_prev.zero();
						last_vel_t = 0;
					}

					z_k[3] = raw.accelerometer_m_s2[0] - acc(0);
					z_k[4] = raw.accelerometer_m_s2[1] - acc(1);
					z_k[5] = raw.accelerometer_m_s2[2] - acc(2);

					/* update magnetometer measurements */
					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp &&
						/* check that the mag vector is > 0 */
						fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] +
							raw.magnetometer_ga[1] * raw.magnetometer_ga[1] +
							raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) {
						update_vect[2] = 1;
						// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
					}

					bool vision_updated = false;
					orb_check(vision_sub, &vision_updated);

					if (vision_updated) {
						orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision);
					}

					if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) {

						math::Quaternion q(vision.q);
						math::Matrix<3, 3> Rvis = q.to_dcm();

						math::Vector<3> v(1.0f, 0.0f, 0.4f);

						math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw
											    // Rrw * Vw = vn. This way we have consistency
						z_k[6] = vn(0);
						z_k[7] = vn(1);
						z_k[8] = vn(2);
					} else {
						z_k[6] = raw.magnetometer_ga[0];
						z_k[7] = raw.magnetometer_ga[1];
						z_k[8] = raw.magnetometer_ga[2];
					}

					static bool const_initialized = false;

					/* initialize with good values once we have a reasonable dt estimate */
					if (!const_initialized && dt < 0.05f && dt > 0.001f) {
						dt = 0.005f;
						parameters_update(&ekf_param_handles, &ekf_params);

						/* update mag declination rotation matrix */
						if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
							mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));

						}

						/* update mag declination rotation matrix */
						R_decl.from_euler(0.0f, 0.0f, mag_decl);

						x_aposteriori_k[0] = z_k[0];
						x_aposteriori_k[1] = z_k[1];
						x_aposteriori_k[2] = z_k[2];
						x_aposteriori_k[3] = 0.0f;
						x_aposteriori_k[4] = 0.0f;
						x_aposteriori_k[5] = 0.0f;
						x_aposteriori_k[6] = z_k[3];
						x_aposteriori_k[7] = z_k[4];
						x_aposteriori_k[8] = z_k[5];
						x_aposteriori_k[9] = z_k[6];
						x_aposteriori_k[10] = z_k[7];
						x_aposteriori_k[11] = z_k[8];

						const_initialized = true;
					}

					/* do not execute the filter if not initialized */
					if (!const_initialized) {
						continue;
					}

					/* Call the estimator */
					AttitudeEKF(false, // approx_prediction
							(unsigned char)ekf_params.use_moment_inertia,
							update_vect,
							dt,
							z_k,
							ekf_params.q[0], // q_rotSpeed,
							ekf_params.q[1], // q_rotAcc
							ekf_params.q[2], // q_acc
							ekf_params.q[3], // q_mag
							ekf_params.r[0], // r_gyro
							ekf_params.r[1], // r_accel
							ekf_params.r[2], // r_mag
							ekf_params.moment_inertia_J,
							x_aposteriori,
							P_aposteriori,
							Rot_matrix,
							euler,
							debugOutput);

					/* swap values for next iteration, check for fatal inputs */
					if (PX4_ISFINITE(euler[0]) && PX4_ISFINITE(euler[1]) && PX4_ISFINITE(euler[2])) {
						memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
						memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));

					} else {
						/* due to inputs or numerical failure the output is invalid, skip it */
						continue;
					}

					if (last_data > 0 && raw.timestamp - last_data > 30000) {
						warnx("sensor data missed! (%llu)\n", static_cast<unsigned long long>(raw.timestamp - last_data));
					}

					last_data = raw.timestamp;

					/* send out */
					att.timestamp = raw.timestamp;

					att.roll = euler[0];
					att.pitch = euler[1];
					att.yaw = euler[2] + mag_decl;

					att.rollspeed = x_aposteriori[0];
					att.pitchspeed = x_aposteriori[1];
					att.yawspeed = x_aposteriori[2];
					att.rollacc = x_aposteriori[3];
					att.pitchacc = x_aposteriori[4];
					att.yawacc = x_aposteriori[5];

					att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
					att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
					att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);

					/* copy offsets */
					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));

					/* magnetic declination */

					math::Matrix<3, 3> R_body = (&Rot_matrix[0]);
					R = R_decl * R_body;
					math::Quaternion q;
					q.from_dcm(R);
					/* copy rotation matrix */
					memcpy(&att.R[0], &R.data[0][0], sizeof(att.R));
					memcpy(&att.q[0],&q.data[0],sizeof(att.q));
					att.R_valid = true;

					if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1])
						&& PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) {
						// Broadcast
						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);

					} else {
						warnx("ERR: NaN estimate!");
					}

					perf_end(ekf_loop_perf);
				}
			}
		}

		loopcounter++;
	}

	thread_running = false;

	return 0;
}
Esempio n. 5
0
 std::ostream& join_output(std::ostream& o, const V& values, Acc acc, std::string sep="") {
   o << acc(values[0]);
   for(auto i = 1u; i < values.size(); i++)
     o << sep << acc(values[i]);
   return o;
 }
Esempio n. 6
0
int main(int argc, char *argv[]) {
    long   k,m;		/* dimension and codebook size			*/
    float  *vec;	/* current vector 				*/
    float  *cb;		/* vector codebook				*/
    float  *cent;	/* centroids for each codebook entry		*/
    long   *n;		/* number of vectors in this interval		*/
    long   J;		/* number of vectors in training set		*/
    long   i,j;
    long   ind;	     	/* index of current vector			*/
    float  se;		/* squared error for this iteration		*/
    float  Dn,Dn_1;	/* current and previous iterations distortion	*/
    float  delta;	/* improvement in distortion 			*/
    FILE   *ftrain;	/* file containing training set			*/
    FILE   *fvq;	/* file containing vector quantiser		*/

    /* Interpret command line arguments */

    if (argc != 5)	{
	printf("usage: vqtrain TrainFile K M VQFile\n");
	exit(0);
    }

    /* Open training file */

    ftrain = fopen(argv[1],"rb");
    if (ftrain == NULL) {
	printf("Error opening training database file: %s\n",argv[1]);
	exit(1);
    }

    /* determine k and m, and allocate arrays */

    k = atol(argv[2]);
    m = atol(argv[3]);
    printf("dimension K=%ld  number of entries M=%ld\n", k,m);
    vec = (float*)malloc(sizeof(float)*k);
    cb = (float*)malloc(sizeof(float)*k*m);
    cent = (float*)malloc(sizeof(float)*k*m);
    n = (long*)malloc(sizeof(long)*m);
    if (cb == NULL || cb == NULL || cent == NULL || vec == NULL) {
	printf("Error in malloc.\n");
	exit(1);
    }

    /* determine size of training set */

    J = 0;
    while(fread(vec, sizeof(float), k, ftrain) == k)
    J++;
    printf("J=%ld entries in training set\n", J);

    /* set up initial codebook state from samples of training set */

    rewind(ftrain);
    fread(cb, sizeof(float), k*m, ftrain);

    /* main loop */

    Dn = 1E32;
    j = 1;
    do {
	Dn_1 = Dn;

	/* zero centroids */

	for(i=0; i<m; i++) {
	    zero(&cent[i*k], k);
	    n[i] = 0;
	}

	/* quantise training set */

	se = 0.0;
	rewind(ftrain);
	for(i=0; i<J; i++) {
	    fread(vec, sizeof(float), k, ftrain);
	    ind = quantise(cb, vec, k, m, &se);
	    n[ind]++;
	    acc(&cent[ind*k], vec, k);
	}
	Dn = se/J;
	delta = (Dn_1-Dn)/Dn;

	printf("\r  Iteration %ld, Dn = %f, Delta = %e\n", j, Dn, delta);
	j++;

	/* determine new codebook from centriods */

	if (delta > DELTAQ)
	    for(i=0; i<m; i++) {
		if (n[i] != 0) {
		    norm(&cent[i*k], k, n[i]);
		    memcpy(&cb[i*k], &cent[i*k], k*sizeof(float));
		}
	    }

    } while (delta > DELTAQ);

    /* save codebook to disk */

    fvq = fopen(argv[4],"wt");
    if (fvq == NULL) {
	printf("Error opening VQ file: %s\n",argv[4]);
	exit(1);
    }

    for(j=0; j<m; j++) {
	for(i=0; i<k; i++)
	    fprintf(fvq,"%f  ",cb[j*k+i]);
	fprintf(fvq,"\n");
    }
    fclose(fvq);

    return 0;
}
int main()
{
	// Read input image
	cv::Mat image= cv::imread("road.jpg",0);
	if (!image.data)
		return 0; 
	// image is resize for book printing
	cv::resize(image, image, cv::Size(), 0.6, 0.6);

    // Display the image
	cv::namedWindow("Original Image");
	cv::imshow("Original Image",image);

	// Compute Sobel
	EdgeDetector ed;
	ed.computeSobel(image);

    // Display the Sobel orientation
	cv::namedWindow("Sobel (orientation)");
	cv::imshow("Sobel (orientation)",ed.getSobelOrientationImage());
	cv::imwrite("ori.bmp",ed.getSobelOrientationImage());

    // Display the Sobel low threshold
	cv::namedWindow("Sobel (low threshold)");
	cv::imshow("Sobel (low threshold)",ed.getBinaryMap(125));

    // Display the Sobel high threshold
	cv::namedWindow("Sobel (high threshold)");
	cv::imshow("Sobel (high threshold)",ed.getBinaryMap(350));

	// Apply Canny algorithm
	cv::Mat contours;
	cv::Canny(image,contours,125,350);

    // Display the image of contours
	cv::namedWindow("Canny Contours");
	cv::imshow("Canny Contours",255-contours);

	// Create a test image
	cv::Mat test(200,200,CV_8U,cv::Scalar(0));
	cv::line(test,cv::Point(100,0),cv::Point(200,200),cv::Scalar(255));
	cv::line(test,cv::Point(0,50),cv::Point(200,200),cv::Scalar(255));
	cv::line(test,cv::Point(0,200),cv::Point(200,0),cv::Scalar(255));
	cv::line(test,cv::Point(200,0),cv::Point(0,200),cv::Scalar(255));
	cv::line(test,cv::Point(100,0),cv::Point(100,200),cv::Scalar(255));
	cv::line(test,cv::Point(0,100),cv::Point(200,100),cv::Scalar(255));

    // Display the test image
	cv::namedWindow("Test Image");
	cv::imshow("Test Image",test);
	cv::imwrite("test.bmp",test);

	// Hough tranform for line detection
	std::vector<cv::Vec2f> lines;
	cv::HoughLines(contours,lines,1,PI/180,50);

	// Draw the lines
	cv::Mat result(contours.rows,contours.cols,CV_8U,cv::Scalar(255));
	image.copyTo(result);

	std::cout << "Lines detected: " << lines.size() << std::endl;

	std::vector<cv::Vec2f>::const_iterator it= lines.begin();
	while (it!=lines.end()) {

		float rho= (*it)[0];   // first element is distance rho
		float theta= (*it)[1]; // second element is angle theta
		
		if (theta < PI/4. || theta > 3.*PI/4.) { // ~vertical line
		
			// point of intersection of the line with first row
			cv::Point pt1(rho/cos(theta),0);        
			// point of intersection of the line with last row
			cv::Point pt2((rho-result.rows*sin(theta))/cos(theta),result.rows);
			// draw a white line
			cv::line( result, pt1, pt2, cv::Scalar(255), 1); 

		} else { // ~horizontal line

			// point of intersection of the line with first column
			cv::Point pt1(0,rho/sin(theta));        
			// point of intersection of the line with last column
			cv::Point pt2(result.cols,(rho-result.cols*cos(theta))/sin(theta));
			// draw a white line
			cv::line( result, pt1, pt2, cv::Scalar(255), 1); 
		}

		std::cout << "line: (" << rho << "," << theta << ")\n"; 

		++it;
	}

    // Display the detected line image
	cv::namedWindow("Lines with Hough");
	cv::imshow("Lines with Hough",result);

	// Create LineFinder instance
	LineFinder ld;

	// Set probabilistic Hough parameters
	ld.setLineLengthAndGap(100,20);
	ld.setMinVote(60);

	// Detect lines
	std::vector<cv::Vec4i> li= ld.findLines(contours);
	ld.drawDetectedLines(image);
	cv::namedWindow("Lines with HoughP");
	cv::imshow("Lines with HoughP",image);

	std::vector<cv::Vec4i>::const_iterator it2= li.begin();
	while (it2!=li.end()) {

		std::cout << "(" << (*it2)[0] << ","<< (*it2)[1]<< ")-(" 
			     << (*it2)[2]<< "," << (*it2)[3] << ")" <<std::endl;

		++it2;
	}

	// Display one line
	image= cv::imread("road.jpg",0);
	// image is resize for book printing
	cv::resize(image, image, cv::Size(), 0.6, 0.6);

	int n = 0;
	cv::line(image, cv::Point(li[n][0],li[n][1]),cv::Point(li[n][2],li[n][3]),cv::Scalar(255),5);
	cv::namedWindow("One line of the Image");
	cv::imshow("One line of the Image",image);

	// Extract the contour pixels of the first detected line
	cv::Mat oneline(image.size(),CV_8U,cv::Scalar(0));
	cv::line(oneline, cv::Point(li[n][0],li[n][1]),cv::Point(li[n][2],li[n][3]),cv::Scalar(255),3);
	cv::bitwise_and(contours,oneline,oneline);
	cv::namedWindow("One line");
	cv::imshow("One line",255-oneline);

	std::vector<cv::Point> points;

	// Iterate over the pixels to obtain all point positions
	for( int y = 0; y < oneline.rows; y++ ) {
    
		uchar* rowPtr = oneline.ptr<uchar>(y);
    
		for( int x = 0; x < oneline.cols; x++ ) {

		    // if on a contour
			if (rowPtr[x]) {

				points.push_back(cv::Point(x,y));
			}
		}
    }
	
	// find the best fitting line
	cv::Vec4f line;
	cv::fitLine(points,line,CV_DIST_L2,0,0.01,0.01);

	std::cout << "line: (" << line[0] << "," << line[1] << ")(" << line[2] << "," << line[3] << ")\n"; 

	int x0= line[2]; // a point on the line
	int y0= line[3];
	int x1= x0+100*line[0]; // add a vector of length 100
	int y1= y0+100*line[1];
	image= cv::imread("road.jpg",0);
	// image is resize for book printing
	cv::resize(image, image, cv::Size(), 0.6, 0.6);

	// draw the line
	cv::line(image,cv::Point(x0,y0),cv::Point(x1,y1),0,3);
	cv::namedWindow("Fitted line");
	cv::imshow("Fitted line",image);

	// eliminate inconsistent lines
	ld.removeLinesOfInconsistentOrientations(ed.getOrientation(),0.4,0.1);

   // Display the detected line image
	image= cv::imread("road.jpg",0);
	// image is resize for book printing
	cv::resize(image, image, cv::Size(), 0.6, 0.6);

	ld.drawDetectedLines(image);
	cv::namedWindow("Detected Lines (2)");
	cv::imshow("Detected Lines (2)",image);

	// Create a Hough accumulator
	cv::Mat acc(200,180,CV_8U,cv::Scalar(0));

	// Choose a point
	int x=50, y=30;

	// loop over all angles
	for (int i=0; i<180; i++) {

		double theta= i*PI/180.;

		// find corresponding rho value 
		double rho= x*std::cos(theta)+y*std::sin(theta);
		int j= static_cast<int>(rho+100.5);

		std::cout << i << "," << j << std::endl;

		// increment accumulator
		acc.at<uchar>(j,i)++;
	}

	// draw the axes
	cv::line(acc, cv::Point(0,0), cv::Point(0,acc.rows-1), 255);	
	cv::line(acc, cv::Point(acc.cols-1,acc.rows-1), cv::Point(0,acc.rows-1), 255);	
	
	cv::imwrite("hough1.bmp",255-(acc*100));

	// Choose a second point
	x=30, y=10;

	// loop over all angles
	for (int i=0; i<180; i++) {

		double theta= i*PI/180.;
		double rho= x*cos(theta)+y*sin(theta);
		int j= static_cast<int>(rho+100.5);

		acc.at<uchar>(j,i)++;
	}

	cv::namedWindow("Hough Accumulator");
	cv::imshow("Hough Accumulator",acc*100);
	cv::imwrite("hough2.bmp",255-(acc*100));

	// Detect circles
	image= cv::imread("chariot.jpg",0);
	// image is resize for book printing
	cv::resize(image, image, cv::Size(), 0.6, 0.6);

	cv::GaussianBlur(image, image, cv::Size(5, 5), 1.5);
	std::vector<cv::Vec3f> circles;
	cv::HoughCircles(image, circles, CV_HOUGH_GRADIENT, 
		2,   // accumulator resolution (size of the image / 2) 
		20,  // minimum distance between two circles
		200, // Canny high threshold 
		60, // minimum number of votes 
		15, 50); // min and max radius

	std::cout << "Circles: " << circles.size() << std::endl;
	
	// Draw the circles
	image= cv::imread("chariot.jpg",0);
	// image is resize for book printing
	cv::resize(image, image, cv::Size(), 0.6, 0.6);

	std::vector<cv::Vec3f>::const_iterator itc = circles.begin();
	
	while (itc!=circles.end()) {
		
	  cv::circle(image, 
		  cv::Point((*itc)[0], (*itc)[1]), // circle centre
		  (*itc)[2], // circle radius
		  cv::Scalar(255), // color 
		  2); // thickness
		
	  ++itc;	
	}

	cv::namedWindow("Detected Circles");
	cv::imshow("Detected Circles",image);

	cv::waitKey();
	return 0;
}
Esempio n. 8
0
  bool DMCcuda::runWithNonlocal() 
  { 
    resetRun();
    Mover->MaxAge = 1;
    IndexType block = 0;
    IndexType nAcceptTot = 0;
    IndexType nRejectTot = 0;
    int nat = W.getTotalNum();
    int nw  = W.getActiveWalkers();
    
    vector<RealType>  LocalEnergy(nw), LocalEnergyOld(nw), 
      oldScale(nw), newScale(nw);
    vector<PosType>   delpos(nw);
    vector<PosType>   dr(nw);
    vector<PosType>   newpos(nw);
    vector<ValueType> ratios(nw), rplus(nw), rminus(nw), R2prop(nw), R2acc(nw);
    vector<PosType>  oldG(nw), newG(nw);
    vector<ValueType> oldL(nw), newL(nw);
    vector<Walker_t*> accepted(nw);
    Matrix<ValueType> lapl(nw, nat);
    Matrix<GradType>  grad(nw, nat);
    vector<vector<NonLocalData> > Txy(nw);
    for (int iw=0; iw<nw; iw++)
      W[iw]->Weight = 1.0;
    do {
      IndexType step = 0;
      nAccept = nReject = 0;
      Estimators->startBlock(nSteps);
      
      do {
        step++;
	CurrentStep++;
	nw = W.getActiveWalkers();
	LocalEnergy.resize(nw);    	oldScale.resize(nw);
	newScale.resize(nw);    	delpos.resize(nw);
	dr.resize(nw);                  newpos.resize(nw);
	ratios.resize(nw);	        rplus.resize(nw);
	rminus.resize(nw);              oldG.resize(nw);
	newG.resize(nw);                oldL.resize(nw);
	newL.resize(nw);                accepted.resize(nw);
	lapl.resize(nw, nat);           grad.resize(nw, nat);
	R2prop.resize(nw,0.0);          R2acc.resize(nw,0.0);

	W.updateLists_GPU();

	Txy.resize(nw);
	for (int iw=0; iw<nw; iw++) {
	  Txy[iw].clear();
	  Txy[iw].push_back(NonLocalData(-1, 1.0, PosType()));
	  W[iw]->Age++;
	}
	
        for(int iat=0; iat<nat; iat++) {
	  Psi.getGradient (W, iat, oldG);
	  
          //create a 3N-Dimensional Gaussian with variance=1
          makeGaussRandomWithEngine(delpos,Random);
          for(int iw=0; iw<nw; iw++) {
	    delpos[iw] *= m_sqrttau;
	    oldScale[iw] = getDriftScale(m_tauovermass,oldG[iw]);
	    dr[iw] = delpos[iw] + (oldScale[iw]*oldG[iw]);
            newpos[iw]=W[iw]->R[iat] + dr[iw];
	    ratios[iw] = 1.0;
	    R2prop[iw] += dot(delpos[iw], delpos[iw]);
	  }
	  W.proposeMove_GPU(newpos, iat);
	  
          Psi.ratio(W,iat,ratios,newG, newL);
	  	  
          accepted.clear();
	  vector<bool> acc(nw, false);
          for(int iw=0; iw<nw; ++iw) {
	    PosType drOld = 
	      newpos[iw] - (W[iw]->R[iat] + oldScale[iw]*oldG[iw]);
	    RealType logGf = -m_oneover2tau * dot(drOld, drOld);
	    newScale[iw]   = getDriftScale(m_tauovermass,newG[iw]);
	    PosType drNew  = 
	      (newpos[iw] + newScale[iw]*newG[iw]) - W[iw]->R[iat];
	    RealType logGb =  -m_oneover2tau * dot(drNew, drNew);
	    RealType x = logGb - logGf;
	    RealType prob = ratios[iw]*ratios[iw]*std::exp(x);
	    
            if(Random() < prob && ratios[iw] > 0.0) {
              accepted.push_back(W[iw]);
	      nAccept++;
	      W[iw]->R[iat] = newpos[iw];
	      W[iw]->Age = 0;
	      acc[iw] = true;
	      R2acc[iw] += dot(delpos[iw], delpos[iw]);
	    }
	    else 
	      nReject++;
	  }
	  W.acceptMove_GPU(acc);
	  if (accepted.size())
	    Psi.update(accepted,iat);
	}
	for (int iw=0; iw < nw; iw++) 
	  if (W[iw]->Age)
	    cerr << "Encountered stuck walker with iw=" << iw << endl;

	//	Psi.recompute(W, false);
	Psi.gradLapl(W, grad, lapl);
	H.evaluate (W, LocalEnergy, Txy);
	if (CurrentStep == 1)
	  LocalEnergyOld = LocalEnergy;
	
	// Now, attempt nonlocal move
	accepted.clear();
	vector<int> iatList;
	vector<PosType> accPos;
	for (int iw=0; iw<nw; iw++) {
	  int ibar = NLop.selectMove(Random(), Txy[iw]);
	  // cerr << "Txy[iw].size() = " << Txy[iw].size() << endl;
	  if (ibar) {
	    accepted.push_back(W[iw]);
	    int iat = Txy[iw][ibar].PID;
	    iatList.push_back(iat);
	    accPos.push_back(W[iw]->R[iat] + Txy[iw][ibar].Delta);
	  }
	}
	if (accepted.size()) {
	  //   W.proposeMove_GPU(newpos, iatList);
	  Psi.ratio(accepted,iatList, accPos, ratios, newG, newL);
	  Psi.update(accepted,iatList);
	  for (int i=0; i<accepted.size(); i++)
	    accepted[i]->R[iatList[i]] = accPos[i];
	  W.copyWalkersToGPU();
	}

	// Now branch
	for (int iw=0; iw<nw; iw++) {
	  W[iw]->Weight *= branchEngine->branchWeight(LocalEnergy[iw], LocalEnergyOld[iw]);
	  W[iw]->getPropertyBase()[R2ACCEPTED] = R2acc[iw];
	  W[iw]->getPropertyBase()[R2PROPOSED] = R2prop[iw];
	}
	Mover->setMultiplicity(W.begin(), W.end());
	branchEngine->branch(CurrentStep,W);
	nw = W.getActiveWalkers();
	LocalEnergyOld.resize(nw);
	for (int iw=0; iw<nw; iw++)
	  LocalEnergyOld[iw] = W[iw]->getPropertyBase()[LOCALENERGY];
      } while(step<nSteps);
      Psi.recompute(W, true);


      double accept_ratio = (double)nAccept/(double)(nAccept+nReject);
      Estimators->stopBlock(accept_ratio);

      nAcceptTot += nAccept;
      nRejectTot += nReject;
      ++block;
      
      recordBlock(block);
    } while(block<nBlocks);
    //finalize a qmc section
    return finalize(block);
  }
Esempio n. 9
0
  bool DMCcuda::run() 
  { 
    bool NLmove = NonLocalMove == "yes";
    bool scaleweight = ScaleWeight == "yes";
    if (NLmove) 
      app_log() << "  Using Casula nonlocal moves in DMCcuda.\n";
    if (scaleweight)
      app_log() << "  Scaling weight per Umrigar/Nightengale.\n";
      
    resetRun();
    Mover->MaxAge = 1;
    IndexType block = 0;
    IndexType nAcceptTot = 0;
    IndexType nRejectTot = 0;
    int nat = W.getTotalNum();
    int nw  = W.getActiveWalkers();
    
    vector<RealType>  LocalEnergy(nw), LocalEnergyOld(nw), 
      oldScale(nw), newScale(nw);
    vector<PosType>   delpos(nw);
    vector<PosType>   dr(nw);
    vector<PosType>   newpos(nw);
    vector<ValueType> ratios(nw), rplus(nw), rminus(nw), R2prop(nw), R2acc(nw);
    vector<PosType>  oldG(nw), newG(nw);
    vector<ValueType> oldL(nw), newL(nw);
    vector<Walker_t*> accepted(nw);
    Matrix<ValueType> lapl(nw, nat);
    Matrix<GradType>  grad(nw, nat);
    vector<ValueType> V2(nw), V2bar(nw);
    vector<vector<NonLocalData> > Txy(nw);

    for (int iw=0; iw<nw; iw++)
      W[iw]->Weight = 1.0;
    do {
      IndexType step = 0;
      nAccept = nReject = 0;
      Estimators->startBlock(nSteps);
      
      do {
        step++;
	CurrentStep++;
	nw = W.getActiveWalkers();
	ResizeTimer.start();
	LocalEnergy.resize(nw);       oldScale.resize(nw);
	newScale.resize(nw);          delpos.resize(nw);
	dr.resize(nw);                newpos.resize(nw);
	ratios.resize(nw);            rplus.resize(nw);
	rminus.resize(nw);            oldG.resize(nw);
	newG.resize(nw);              oldL.resize(nw);
	newL.resize(nw);              accepted.resize(nw);
	lapl.resize(nw, nat);         grad.resize(nw, nat);
	R2prop.resize(nw,0.0);        R2acc.resize(nw,0.0);
	V2.resize(nw,0.0);            V2bar.resize(nw,0.0);

	W.updateLists_GPU();
	ResizeTimer.stop();

	if (NLmove) {
	  Txy.resize(nw);
	  for (int iw=0; iw<nw; iw++) {
	    Txy[iw].clear();
	    Txy[iw].push_back(NonLocalData(-1, 1.0, PosType()));
	  }
	}

	for (int iw=0; iw<nw; iw++)
	  W[iw]->Age++;
	
	DriftDiffuseTimer.start();
        for(int iat=0; iat<nat; iat++) {
	  Psi.getGradient (W, iat, oldG);

          //create a 3N-Dimensional Gaussian with variance=1
          makeGaussRandomWithEngine(delpos,Random);
          for(int iw=0; iw<nw; iw++) {
	    delpos[iw] *= m_sqrttau;
	    oldScale[iw] = getDriftScale(m_tauovermass,oldG[iw]);
	    dr[iw] = delpos[iw] + (oldScale[iw]*oldG[iw]);
            newpos[iw]=W[iw]->R[iat] + dr[iw];
	    ratios[iw] = 1.0;
	    R2prop[iw] += dot(delpos[iw], delpos[iw]);
	  }
	  W.proposeMove_GPU(newpos, iat);
	  
          Psi.ratio(W,iat,ratios,newG, newL);
	  	  
          accepted.clear();
	  vector<bool> acc(nw, false);
          for(int iw=0; iw<nw; ++iw) {
	    PosType drOld = 
	      newpos[iw] - (W[iw]->R[iat] + oldScale[iw]*oldG[iw]);
	    RealType logGf = -m_oneover2tau * dot(drOld, drOld);
	    newScale[iw]   = getDriftScale(m_tauovermass,newG[iw]);
	    PosType drNew  = 
	      (newpos[iw] + newScale[iw]*newG[iw]) - W[iw]->R[iat];
	    RealType logGb =  -m_oneover2tau * dot(drNew, drNew);
	    RealType x = logGb - logGf;
	    RealType prob = ratios[iw]*ratios[iw]*std::exp(x);
	    
            if(Random() < prob && ratios[iw] > 0.0) {
              accepted.push_back(W[iw]);
	      nAccept++;
	      W[iw]->R[iat] = newpos[iw];
	      W[iw]->Age = 0;
	      acc[iw] = true;
	      R2acc[iw] += dot(delpos[iw], delpos[iw]);
	      V2[iw]    += m_tauovermass * m_tauovermass * dot(newG[iw],newG[iw]);
	      V2bar[iw] +=  newScale[iw] *  newScale[iw] * dot(newG[iw],newG[iw]);
	    }
	    else {
	      nReject++;
	      V2[iw]    += m_tauovermass * m_tauovermass * dot(oldG[iw],oldG[iw]);
	      V2bar[iw] +=  oldScale[iw] *  oldScale[iw] * dot(oldG[iw],oldG[iw]);
	    }	      
	  }
	  W.acceptMove_GPU(acc);
	  if (accepted.size())
	    Psi.update(accepted,iat);
	}
	DriftDiffuseTimer.stop();
	//	Psi.recompute(W, false);
	Psi.gradLapl(W, grad, lapl);
	HTimer.start();
	if (NLmove)	  H.evaluate (W, LocalEnergy, Txy);
	else    	  H.evaluate (W, LocalEnergy);
	HTimer.stop();
// 	for (int iw=0; iw<nw; iw++) {
// 	  branchEngine->clampEnergy(LocalEnergy[iw]);
// 	  W[iw]->getPropertyBase()[LOCALENERGY] = LocalEnergy[iw];
// 	}
	if (CurrentStep == 1)
	  LocalEnergyOld = LocalEnergy;
	
	if (NLmove) {
	  // Now, attempt nonlocal move
	  accepted.clear();
	  vector<int> iatList;
	  vector<PosType> accPos;
	  for (int iw=0; iw<nw; iw++) {
	    /// HACK HACK HACK
// 	    if (LocalEnergy[iw] < -2300.0) {
// 	      cerr << "Walker " << iw << " has energy " 
// 		   << LocalEnergy[iw] << endl;;
// 	      double maxWeight = 0.0;
// 	      int elMax = -1;
// 	      PosType posMax;
// 	      for (int j=1; j<Txy[iw].size(); j++)
// 		if (std::fabs(Txy[iw][j].Weight) > std::fabs(maxWeight)) {
// 		  maxWeight = Txy[iw][j].Weight;
// 		  elMax = Txy[iw][j].PID;
// 		  posMax = W[iw]->R[elMax] + Txy[iw][j].Delta;
// 		}
// 	      cerr << "Maximum weight is " << maxWeight << " for electron " 
// 		   << elMax << " at position " << posMax << endl;
// 	      PosType unit = W.Lattice.toUnit(posMax);
// 	      unit[0] -= round(unit[0]);
// 	      unit[1] -= round(unit[1]);
// 	      unit[2] -= round(unit[2]);
// 	      cerr << "Reduced position = " << unit << endl;
// 	    }

	    int ibar = NLop.selectMove(Random(), Txy[iw]);
		    
	    if (ibar) {
	      accepted.push_back(W[iw]);
	      int iat = Txy[iw][ibar].PID;
	      iatList.push_back(iat);
	      accPos.push_back(W[iw]->R[iat] + Txy[iw][ibar].Delta);
	    }
	  }
	  if (accepted.size()) {
	    Psi.ratio(accepted,iatList, accPos, ratios, newG, newL);
	    Psi.update(accepted,iatList);
	    for (int i=0; i<accepted.size(); i++)
	      accepted[i]->R[iatList[i]] = accPos[i];
	    W.NLMove_GPU (accepted, accPos, iatList);
	    // HACK HACK HACK
	    // Recompute the kinetic energy
	    // Psi.gradLapl(W, grad, lapl);
	    // H.evaluate (W, LocalEnergy);
	    //W.copyWalkersToGPU();
	  }
	}

	// Now branch
	BranchTimer.start();
	for (int iw=0; iw<nw; iw++) {
	  RealType v2=0.0, v2bar=0.0;
	  for(int iat=0; iat<nat; iat++) {
	    v2 += dot(W.G[iat],W.G[iat]);
	    RealType newscale = getDriftScale(m_tauovermass,newG[iw]);
	    v2 += m_tauovermass * m_tauovermass * dot(newG[iw],newG[iw]);
	    v2bar +=  newscale * newscale * dot(newG[iw],newG[iw]);
	  }
	  //RealType scNew = std::sqrt(V2bar[iw] / V2[iw]);
	  RealType scNew = std::sqrt(v2bar/v2);
	  RealType scOld = (CurrentStep == 1) ? scNew : W[iw]->getPropertyBase()[DRIFTSCALE];

	  W[iw]->getPropertyBase()[DRIFTSCALE] = scNew;
	  // fprintf (stderr, "iw = %d  scNew = %1.8f  scOld = %1.8f\n", iw, scNew, scOld);
	  RealType tauRatio = R2acc[iw] / R2prop[iw];
	  if (tauRatio < 0.5) 
	    cerr << "  tauRatio = " << tauRatio << endl;
	  RealType taueff = m_tauovermass * tauRatio;
	  if (scaleweight) 
	    W[iw]->Weight *= branchEngine->branchWeightTau
	      (LocalEnergy[iw], LocalEnergyOld[iw], scNew, scOld, taueff);
	  else
	    W[iw]->Weight *= branchEngine->branchWeight
	      (LocalEnergy[iw], LocalEnergyOld[iw]);

	  W[iw]->getPropertyBase()[R2ACCEPTED] = R2acc[iw];
	  W[iw]->getPropertyBase()[R2PROPOSED] = R2prop[iw];
	  
	}
	Mover->setMultiplicity(W.begin(), W.end());
	branchEngine->branch(CurrentStep,W);
	nw = W.getActiveWalkers();
	LocalEnergyOld.resize(nw);
	for (int iw=0; iw<nw; iw++)
	  LocalEnergyOld[iw] = W[iw]->getPropertyBase()[LOCALENERGY];
	BranchTimer.stop();
      } while(step<nSteps);
      Psi.recompute(W, true);


      double accept_ratio = (double)nAccept/(double)(nAccept+nReject);
      Estimators->stopBlock(accept_ratio);

      nAcceptTot += nAccept;
      nRejectTot += nReject;
      ++block;
      
      recordBlock(block);
    } while(block<nBlocks);
    //finalize a qmc section
    return finalize(block);
  }
Esempio n. 10
0
  bool VMCcuda::run() { 
    if (UseDrift == "yes")
      return runWithDrift();

    resetRun();
    IndexType block = 0;
    IndexType nAcceptTot = 0;
    IndexType nRejectTot = 0;
    IndexType updatePeriod= (QMCDriverMode[QMC_UPDATE_MODE]) 
      ? Period4CheckProperties 
      : (nBlocks+1)*nSteps;
    
    int nat = W.getTotalNum();
    int nw  = W.getActiveWalkers();
    
    vector<RealType>  LocalEnergy(nw);
    vector<PosType>   delpos(nw);
    vector<PosType>   newpos(nw);
    vector<ValueType> ratios(nw);
    vector<GradType>  oldG(nw), newG(nw);
    vector<ValueType> oldL(nw), newL(nw);
    vector<Walker_t*> accepted(nw);
    Matrix<ValueType> lapl(nw, nat);
    Matrix<GradType>  grad(nw, nat);
    double Esum;

    // First do warmup steps
    for (int step=0; step<myWarmupSteps; step++) {
      for(int iat=0; iat<nat; ++iat)  {
	//create a 3N-Dimensional Gaussian with variance=1
	makeGaussRandomWithEngine(delpos,Random);
	for(int iw=0; iw<nw; ++iw) {
	  PosType G = W[iw]->G[iat];
	  newpos[iw]=W[iw]->R[iat] + m_sqrttau*delpos[iw];
	  ratios[iw] = 1.0;
	}
	W.proposeMove_GPU(newpos, iat);
	    
	Psi.ratio(W,iat,ratios,newG, newL);
	    
	accepted.clear();

	vector<bool> acc(nw, true);
	if (W.UseBoundBox)
	  checkBounds (newpos, acc);

	for(int iw=0; iw<nw; ++iw) {
	  if(acc[iw] && ratios[iw]*ratios[iw] > Random()) {
	    accepted.push_back(W[iw]);
	    nAccept++;
	    W[iw]->R[iat] = newpos[iw];
	    acc[iw] = true;
	  }
	  else {
	    acc[iw] = false;
	    nReject++;
	  }
	}
	W.acceptMove_GPU(acc);
	if (accepted.size())
	  Psi.update(accepted,iat);
      }
    }

    do {
      IndexType step = 0;
      nAccept = nReject = 0;
      Esum = 0.0;
      Estimators->startBlock(nSteps);
      do
      {
        ++step;++CurrentStep;
	for (int isub=0; isub<nSubSteps; isub++) {
	  for(int iat=0; iat<nat; ++iat)  {
	    //create a 3N-Dimensional Gaussian with variance=1
	    makeGaussRandomWithEngine(delpos,Random);
	    for(int iw=0; iw<nw; ++iw) {
	      PosType G = W[iw]->G[iat];
	      newpos[iw]=W[iw]->R[iat] + m_sqrttau*delpos[iw];
	      ratios[iw] = 1.0;
	    }
	    W.proposeMove_GPU(newpos, iat);
	    
	    Psi.ratio(W,iat,ratios,newG, newL);
	    
	    accepted.clear();

	    vector<bool> acc(nw, true);
	    if (W.UseBoundBox)
	      checkBounds (newpos, acc);

	    for(int iw=0; iw<nw; ++iw) {
	      if(acc[iw] && ratios[iw]*ratios[iw] > Random()) {
		accepted.push_back(W[iw]);
		nAccept++;
		W[iw]->R[iat] = newpos[iw];
		acc[iw] = true;
	      }
	      else {
		acc[iw]=false;
		nReject++;
	      }
	    }
	    W.acceptMove_GPU(acc);
	    if (accepted.size())
	      Psi.update(accepted,iat);
	  }
	}
	Psi.gradLapl(W, grad, lapl);
	H.evaluate (W, LocalEnergy);
	if (myPeriod4WalkerDump && (CurrentStep % myPeriod4WalkerDump)==0) 
	  W.saveEnsemble();
	Estimators->accumulate(W);
      } while(step<nSteps);
      Psi.recompute(W);

      // vector<RealType> logPsi(W.WalkerList.size(), 0.0);
      // Psi.evaluateLog(W, logPsi);
      
      double accept_ratio = (double)nAccept/(double)(nAccept+nReject);
      Estimators->stopBlock(accept_ratio);

      nAcceptTot += nAccept;
      nRejectTot += nReject;
      ++block;
      recordBlock(block);
    } while(block<nBlocks);

    //Mover->stopRun();

    //finalize a qmc section
    return finalize(block);
  }
Esempio n. 11
0
int main(int argc, char** argv)
{
    boost::log::core::get()->set_filter(boost::log::trivial::severity >= boost::log::trivial::info);
    int32_t port = (argc >= 3) ? atoi(argv[2]) : 9092;

    csi::kafka::broker_address broker;
    if (argc >= 2)
    {
        broker = csi::kafka::broker_address(argv[1], port);
    }
    else
    {
        broker = csi::kafka::broker_address(DEFAULT_BROKER_ADDRESS, port);
    }

    boost::asio::io_service io_service;
    std::auto_ptr<boost::asio::io_service::work> work(new boost::asio::io_service::work(io_service));
    boost::thread bt(boost::bind(&boost::asio::io_service::run, &io_service));

 


    //HIGHLEVEL CONSUMER
    csi::kafka::highlevel_consumer consumer(io_service, TOPIC_NAME, 1000, 10000);
    consumer.connect( { broker } );
    //std::vector<int64_t> result = consumer.get_offsets();
    consumer.connect_forever({ broker } );

    consumer.set_offset(csi::kafka::earliest_available_offset);

    boost::thread do_log([&consumer]
    {
        boost::accumulators::accumulator_set<double, boost::accumulators::stats<boost::accumulators::tag::rolling_mean> > acc(boost::accumulators::tag::rolling_window::window_size = 10);
        while (true)
        {
            boost::this_thread::sleep(boost::posix_time::seconds(1));

            std::vector<csi::kafka::highlevel_consumer::metrics>  metrics = consumer.get_metrics();
            uint32_t rx_msg_sec_total = 0;
            uint32_t rx_kb_sec_total = 0;
            for (std::vector<csi::kafka::highlevel_consumer::metrics>::const_iterator i = metrics.begin(); i != metrics.end(); ++i)
            {
                std::cerr << "\t partiton:" << (*i).partition << "\t" << (*i).rx_msg_sec << " msg/s \t" << (*i).rx_kb_sec << "KB/s \troundtrip:" << (*i).rx_roundtrip << " ms" << std::endl;
                rx_msg_sec_total += (*i).rx_msg_sec;
                rx_kb_sec_total += (*i).rx_kb_sec;
            }
            std::cerr << "\t          \t" << rx_msg_sec_total << " msg/s \t" << rx_kb_sec_total << "KB/s" << std::endl;
        }
    });


    consumer.stream_async([](const boost::system::error_code& ec1, csi::kafka::error_codes ec2, std::shared_ptr<csi::kafka::fetch_response::topic_data::partition_data> response)
    {
        if (ec1 || ec2)
        {
            std::cerr << " decode error: ec1:" << ec1 << " ec2" << csi::kafka::to_string(ec2) << std::endl;
            return;
        }
        std::cerr << "+";
    });

    consumer.fetch(boost::bind(handle_fetch, &consumer, _1));


    while (true)
        boost::this_thread::sleep(boost::posix_time::seconds(30));

    consumer.close();

    work.reset();
    io_service.stop();

    return EXIT_SUCCESS;
}
Esempio n. 12
0
  bool VMCcuda::runWithDrift() 
  { 
    resetRun();
    IndexType block = 0;
    IndexType nAcceptTot = 0;
    IndexType nRejectTot = 0;
    int nat = W.getTotalNum();
    int nw  = W.getActiveWalkers();
    
    vector<RealType>  LocalEnergy(nw), oldScale(nw), newScale(nw);
    vector<PosType>   delpos(nw);
    vector<PosType>   dr(nw);
    vector<PosType>   newpos(nw);
    vector<ValueType> ratios(nw), rplus(nw), rminus(nw);
    vector<PosType>  oldG(nw), newG(nw);
    vector<ValueType> oldL(nw), newL(nw);
    vector<Walker_t*> accepted(nw);
    Matrix<ValueType> lapl(nw, nat);
    Matrix<GradType>  grad(nw, nat);

    // First, do warmup steps
    for (int step=0; step<myWarmupSteps; step++) {
      for(int iat=0; iat<nat; iat++) {
	Psi.getGradient (W, iat, oldG);
	    
	//create a 3N-Dimensional Gaussian with variance=1
	makeGaussRandomWithEngine(delpos,Random);
	for(int iw=0; iw<nw; iw++) {
	  oldScale[iw] = getDriftScale(m_tauovermass,oldG[iw]);
	  dr[iw] = (m_sqrttau*delpos[iw]) + (oldScale[iw]*oldG[iw]);
	  newpos[iw]=W[iw]->R[iat] + dr[iw];
	  ratios[iw] = 1.0;
	}
	W.proposeMove_GPU(newpos, iat);
	    
	Psi.ratio(W,iat,ratios,newG, newL);
	    
	accepted.clear();
	vector<bool> acc(nw, true);
	if (W.UseBoundBox)
	  checkBounds (newpos, acc);

	for(int iw=0; iw<nw; ++iw) {
	  PosType drOld = 
	    newpos[iw] - (W[iw]->R[iat] + oldScale[iw]*oldG[iw]);
	  RealType logGf = -m_oneover2tau * dot(drOld, drOld);
	  newScale[iw]   = getDriftScale(m_tauovermass,newG[iw]);
	  PosType drNew  = 
	    (newpos[iw] + newScale[iw]*newG[iw]) - W[iw]->R[iat];

	  RealType logGb =  -m_oneover2tau * dot(drNew, drNew);
	  RealType x = logGb - logGf;
	  RealType prob = ratios[iw]*ratios[iw]*std::exp(x);

	  if(acc[iw] && Random() < prob) {
	    accepted.push_back(W[iw]);
	    nAccept++;
	    W[iw]->R[iat] = newpos[iw];
	    acc[iw] = true;
	  }
	  else {
	    acc[iw] = false;
	    nReject++;
	  }
	}
	W.acceptMove_GPU(acc);
	if (accepted.size())
	  Psi.update(accepted,iat);
      }
    }

    // Now do data collection steps
    do {
      IndexType step = 0;
      nAccept = nReject = 0;
      Estimators->startBlock(nSteps);
      do {
        step++;
	CurrentStep++;
	for (int isub=0; isub<nSubSteps; isub++) {
	  for(int iat=0; iat<nat; iat++) {
	    Psi.getGradient (W, iat, oldG);
	    
	    //create a 3N-Dimensional Gaussian with variance=1
	    makeGaussRandomWithEngine(delpos,Random);
	    for(int iw=0; iw<nw; iw++) {
	      oldScale[iw] = getDriftScale(m_tauovermass,oldG[iw]);
	      dr[iw] = (m_sqrttau*delpos[iw]) + (oldScale[iw]*oldG[iw]);
	      newpos[iw]=W[iw]->R[iat] + dr[iw];
	      ratios[iw] = 1.0;
	    }
	    W.proposeMove_GPU(newpos, iat);
	    
	    Psi.ratio(W,iat,ratios,newG, newL);
	    
	    accepted.clear();

	    vector<bool> acc(nw, true);
	    if (W.UseBoundBox)
	      checkBounds (newpos, acc);

	    for(int iw=0; iw<nw; ++iw) {
	      PosType drOld = 
		newpos[iw] - (W[iw]->R[iat] + oldScale[iw]*oldG[iw]);
	      // if (dot(drOld, drOld) > 25.0)
	      //   cerr << "Large drift encountered!  Old drift = " << drOld << endl;
	      RealType logGf = -m_oneover2tau * dot(drOld, drOld);
	      newScale[iw]   = getDriftScale(m_tauovermass,newG[iw]);
	      PosType drNew  = 
		(newpos[iw] + newScale[iw]*newG[iw]) - W[iw]->R[iat];
	      // if (dot(drNew, drNew) > 25.0)
	      //   cerr << "Large drift encountered!  Drift = " << drNew << endl;
	      RealType logGb =  -m_oneover2tau * dot(drNew, drNew);
	      RealType x = logGb - logGf;
	      RealType prob = ratios[iw]*ratios[iw]*std::exp(x);
	      
	      if(acc[iw] && Random() < prob) {
		accepted.push_back(W[iw]);
		nAccept++;
		W[iw]->R[iat] = newpos[iw];
		acc[iw] = true;
	      }
	      else {
		acc[iw] = false;
		nReject++;
	      }
	    }
	    W.acceptMove_GPU(acc);
	    if (accepted.size())
	      Psi.update(accepted,iat);
	  }
	  // cerr << "Rank = " << myComm->rank() <<
	  //   "  CurrentStep = " << CurrentStep << "  isub = " << isub << endl;
	}
	Psi.gradLapl(W, grad, lapl);
	H.evaluate (W, LocalEnergy);
	if (myPeriod4WalkerDump && (CurrentStep % myPeriod4WalkerDump)==0) 
	  W.saveEnsemble();
	Estimators->accumulate(W);
      } while(step<nSteps);
      Psi.recompute(W);
      
      double accept_ratio = (double)nAccept/(double)(nAccept+nReject);
      Estimators->stopBlock(accept_ratio);

      nAcceptTot += nAccept;
      nRejectTot += nReject;
      ++block;
      recordBlock(block);
    } while(block<nBlocks);
    //finalize a qmc section
    if (!myComm->rank())
      gpu::cuda_memory_manager.report();
    return finalize(block);
  }
Esempio n. 13
0
main()
{
      
/******************************************************************************/
/*  BUILDING DISK POSITION DISTRIBUTION                                       */
/******************************************************************************/

    std::cout << "Building disk\n";         //  printing disk parameters
    std::cout << "  Mdisk = " << Md << "\n";
    std::cout << "  Ndisk = " << Ndisk << "\n";
    std::cout << "  Radial Scale Length h = " << h << "\n";
    std::cout << "  Vertical Scale Length z0 = " << z0 << "\n";
    std::cout << "  Radial Cutoff = " << diskcut << "\n";
    std::cout << "  Vertical Cutoff = " << thickcut << "\n";
    
    std::vector<Vector> disk(Ndisk, Vector(3)); //  array of disk positions
    
    min = 0.0, max = diskcut;               //  setting max and min radii
    topbound = findtopbound(surfacedist, min, max);   //  finding topbound
    
    for (int i = 0; i < Ndisk; i++)         //  for each particle in disk
    {
        r = rejectionsample(surfacedist, min, max, topbound); //  choose
                                            //  random radius
        disk[i][0] = r;                     //  assigning as such
        disk[i][1] = randomreal(0.0, 2.0*PI); //  randomize azimuthal angle
    }                                       
    
    min = -thickcut;                        //  setting min and max
    max = thickcut;                         //  heights
    topbound = findtopbound(diskthick, min, max);   //  finding topbound
    for (int i = 0; i < Ndisk; i++)         //  for each disk particle
    {
        disk[i][2] = rejectionsample(diskthick, min, max, topbound);    //
                                            //  choosing height randomly
        bodies[i].mass = Md / ((double)Ndisk);  //  assigning masses such that
                                            //  total mass is correct
        bodies[i].pos = cyltocart(disk[i]); //  transforming to cartesian coords
        bodies[i].id = i;                   //  assigning particle id
        bodies[i].DM = false;               //  these are not dark
    }
    

/******************************************************************************/
/*  BUILDING HALO POSITION DISTRIBUTION                                       */
/******************************************************************************/
    
    std::cout << "Building Halo\n";         //  printing parameters
    std::cout << "  Mhalo = " << Mh << "\n";
    std::cout << "  Nhalo = " << Nhalo << "\n";
    std::cout << "  Scale Length rc = " << rc << "\n";
    std::cout << "  Scale Length gamma = " << g << "\n";
    std::cout << "  Cutoff Radius = " << halocut << "\n";

    std::vector<Vector> halo(Nhalo, Vector(3)); //  array of halo positions
    
    min = 0.0, max = halocut;               //  max and min for distribution
    topbound = findtopbound(hernquisthalo, min, max);   //  finding topbound
    for (int i = 0; i < Nhalo; i++)         //  for each bulge particle
    {
        r = rejectionsample(hernquisthalo, min, max, topbound);  //  select r from
                                            //  distribution
        bodies[Ndisk + i].pos = randomsphere(r);    //  randomize position
                                            //  on sphere
        bodies[Ndisk + i].mass = Mh / ((double)Nhalo);  //  normalizing mass
        bodies[Ndisk + i].id = Ndisk + i;   //  setting appropriate ID
        bodies[Ndisk + i].DM = true;        //  these are dark
        halo[i] = carttosphere(bodies[Ndisk + i].pos);  //  saving copy in
                                            //  spherical coords for later
    }
    

/******************************************************************************/
/*  BUILDING BULGE POSITION DISTRIBUTION                                      */
/******************************************************************************/

    std::cout << "Building Bulge\n";
    std::cout << "  Mbulge = " << Mb << "\n";
    std::cout << "  Nbulge = " << Nbulge << "\n";
    std::cout << "  Scale Length a = " << a << "\n";
    std::cout << "  Cutoff Radius = " << bulgecut << "\n";
    
    std::vector<Vector> bulge(Nbulge, Vector(3));   //  array of bulge positions
    
    min = 0.0; max = bulgecut;              //  distribution max and min
    topbound = findtopbound(bulgedist, min, max);   //  finding topbound
    for (int i = 0; i < Nbulge; i++)        //  for each particle
    {
        r = rejectionsample(bulgedist, min, max, topbound);  //  select r from
                                            //  distribution
        bodies[Ndisk + Nhalo + i].pos = randomsphere(r);    //  randomize sphere
                                            //  position
        bodies[Ndisk + Nhalo + i].mass = Mb / (double)Nbulge;   //  setting mass
        bodies[Ndisk + Nhalo + i].id = Ndisk + Nhalo + i;   //  setting IDs
        bulge[i] = carttosphere(bodies[Ndisk + Nhalo + i].pos); //  saving copy
                                            //  in spherical coordinates
    }

    
/******************************************************************************/
/*  Approximating Cumulative Mass Distribution M(r)                           */
/******************************************************************************/
    
    dr = halocut / ((double) massbins);     //  setting separation between mass
                                            //  bins
    
    std::cout << "Approximating Cumulative Mass Distribution M(r)\n";
    std::cout << "  Number of bins = " << massbins << "\n";
    std::cout << "  dr = " << dr << "\n";
    
    std::vector <Vector> Massatr(massbins, Vector(2));  //  Array to hold
                                            //  radius and value of cumulative
                                            //  mass distribution
    
    for (int i = 0; i < massbins; i++)      //  for each mass bin
    {
        Massatr[i][1] = ((double)(i+1))*dr; //  setting radius
        Massatr[i][0] = 0.0;                //  clearing total mass
        
        for (int j = 0; j < Ndisk; j++)     //  for each disk mass
        {
            r = sqrt(disk[j][0]*disk[j][0] + disk[j][2]*disk[j][2]);    // radius
                                            //  in spherical coordinates
            if((r < (double)(i+1)*dr))      //  if radius less than bin radius
            {
                Massatr[i][0] += Md / (double)Ndisk;    //  add mass
            }
        }
        
        for (int j = 0; j < Nhalo; j++)     //  for each halo mass
        {
            r = halo[j][0];                 //  radius
            if((r < (double)(i+1)*dr))      // if radius less than bin radius
            {
                Massatr[i][0] += Mh / (double)Nhalo;    //  add mass
            }
        }
        
        for (int j = 0; j < Nbulge; j++)    // for each bulge mass
        {
            r = bulge[j][0];                //  radius
            if((r < (double)(i+1)*dr))      //  if radius less than bin radius
            {
                Massatr[i][0] += Mb / (double)Nbulge;   //  add mass
            }
        }
    }
    
    
/******************************************************************************/
/*  Setting Halo Velocities                                                   */
/******************************************************************************/
    
    std::cout << "Setting Halo Velocities\n";
    
    double v;                               //  variable to hold speed
    double vesc;                            //  variable to hold escape speed
    
    std::vector<Vector> halovels(Nhalo, Vector(3)); //  Vector to hold halo
                                            //  velocities
    
    for (int i = 0; i < Nhalo; i++)         //  for each halo mass
    {
        r = halo[i][0];                     //  radius
        
        startbin = floor(r/dr);             //  starting index is floor of r/dr
        
        vesc = sqrt(2.0*Massatr[startbin][0]/r);    //  escape velocity
        
        vr2 = 0.0;                          //  clearing radial dispersion
        for (int j = startbin; j < massbins; j++)   //  for each mass bin
        {
            vr2 += hernquisthalo(Massatr[j][1])*dr*Massatr[j][0];   //  add 
        }                                   //  contribution
        vr2 /= (hernquisthalo(r)/(r*r));    //  dividing by halo density at r
        
        
        min = 0.0;                          //  distribution min
        max = 0.95*vesc;                    //  distribution max is 0.95 of
                                            //  escape velocity
        topbound = vr2 / 2.71828;           //  topbound is vr^2 / e
        
        v = rejectionsample(halospeeddist, min, max, topbound); //  selecting
                                            //  absolute speed
        halovels[i] = randomsphere(v);      //  randomizing cartesian velocities
                                            //  on a sphere of radius v
        bodies[Ndisk + i].vel = halovels[i];//  assigning velocities as such
        
    }
    
    
/******************************************************************************/
/*  Setting Bulge Velocities                                                  */
/******************************************************************************/

    
    std::cout << "Setting Bulge Velocities\n";
    
    std::vector<Vector> bulgevels(Nbulge, Vector(3));   // Vector to hold bulge
                                             // velocities
    
    for (int i = 0; i < Nbulge; i++)        //  for each particle
    {
        r = bulge[i][0];                    //  radius
        
        startbin = floor(r / dr);           //  starting index
        
        vesc = sqrt(2.0*Massatr[startbin][0]/r);    //  escape velocity
        
        vr2 = 0.0;                          //  clearing radial dispersion
        
        for (int j = startbin; j < massbins; j++)   //  for each mass bin
        {
            vr2 += bulgedist(Massatr[j][1])*dr*Massatr[j][0];   // add
                                            //  contribution
        }
        vr2 /= bulgedist(r)/(r*r);          //  dividing by halo density at r
        
        
        min = 0.0;                          //  distribution min
        max = 0.95*vesc;                    //  max is 0.95 of escape velocity
        topbound = vr2 / 2.71828;           //  topbound is vr^2 /e
        
        v = rejectionsample(bulgedist, min, max, topbound); //  selecting absolute
                                            //  absolute speed
        bulgevels[i] = randomsphere(v);     //  randomizing constrained cartesian
                                            //  velocities
        bodies[Ndisk + Nhalo + i].vel = bulgevels[i];   //  assining data as such
    }

    
/******************************************************************************/
/*  Setting Disk Velocities                                                   */
/******************************************************************************/

    std::cout << "Setting Disk Velocities\n";
    std::cout << "  Q = " << Q << "\n";
    std::cout << "  Reference radius = " << rref << "\n";
    std::vector<Vector> diskvels(Ndisk, Vector(3)); //  Vector to hold disk
                                            //  velocities
    
    double vz2;                             //  vertical dispersion
    double vc;                              //  circular speed
    double ar;                              //  radial acceleration
    double k;                               //  epicyclic frequency
    double sigmaz2;                         //  azimuthal velocity dispersion
    double sigmaavg;                        //  average dispersion
    double Omega;                           //  angular frequency
    double A;                               //  radial dispersion constant
    int count;                              //  count variable for averaging
    Vector acc(3);                          //  vector to store acceleration
    double as = 0.25*h;
    
    
    maketree(bodies, Ntot, root);           //  making tree
    
    // NORMALIZING RADIAL DISPERSION
    std::cout << "  Normalizing Radial Distribution\n";
    dr = diskcut / 1000.0;                  //  width of annulus in which
                                            //  to average dispersion
    sigmaavg = 0.0;                         //  zeroing average
    for (int i = 0; i < Ndisk; i++)         //  for each disk particle
    {
        r = disk[i][0];                     //  radius
        if (fabs(r - rref) < dr)            //  if radius in annulus
        {                                   //  calculate epicylclic frequency
            k = epicyclicfrequency(bodies, Ntot, i, root, eps, theta, 0.05*dr);
            sigmaavg += 3.36*Sigma(r)/k;    //  calculate dispersion and add to
                                            //  average
            count += 1;                     //  up count
        }  
    }
    
    sigmaavg /= (double)count;              //  divide total by count
    sigmaavg *= Q;                          //  adjust by Q
    
    A = sigmaavg*sigmaavg / Sigma(rref);    //  setting norm constant
    
    
    //  ASSIGNING VELOCITIES
    std::cout << "  Setting particle velocities\n";
    for (int i = 0; i < Ndisk; i++)         //  for every particle
    {
        r = disk[i][0];                     //  radius
        vz2 = PI*z0*Sigma(sqrt(r*r + 2.0*as*as));    //  vertical dispersion
        diskvels[i][2] = gaussianrandom(sqrt(vz2)); //  randomizing vertical
                                            //  with this dispersion
        
        vr2 = A*Sigma(sqrt(r*r + 2.0*as*as));   //  assigning radial dispersion
        diskvels[i][0] = gaussianrandom(sqrt(vr2)); //  randomizing radial
                                            //  dispersion
        
        acc = treeforce(&bodies[i], root, eps, theta);  //  acceleration
        ar = (acc[0]*bodies[i].pos[0] + acc[1]*bodies[i].pos[1])/r; //
                                            //  radial acceleration
        Omega = sqrt(fabs(ar)/r);           //  angular frequency
        k = epicyclicfrequency(bodies, Ntot, i, root, eps, theta, dr);  //
                                            //  epicyclic frequency
        vc = Omega*r;                       //  circular speed
        v = sqrt(fabs(vc*vc + vr2*(1.0 - (k*k)/(4.0*Omega*Omega) - 2.0*r/h)));  //
                                            //  azimuthal streaming velocity
        sigmaz2 = vr2*k*k/(4.0*Omega*Omega);//  azimuthal dispersion
        v += gaussianrandom(sqrt(sigmaz2)); //  adding random azimuthal component
        diskvels[i][1] = v;                 //  assigning azimuthal velocity 
        
                                            //  transforming to cartesian coords
        bodies[i].vel[0] = diskvels[i][0]*cos(disk[i][1])
                            - diskvels[i][1]*sin(disk[i][1]);
        bodies[i].vel[1] = diskvels[i][0]*sin(disk[i][1])
                            + diskvels[i][1]*cos(disk[i][1]);
        bodies[i].vel[2] = diskvels[i][2];
    }
    
    
/******************************************************************************/
/*  Reporting Average Disk Speed                                              */
/******************************************************************************/
    
    v = 0.0;
    
    for (int i = 0; i < Ndisk; i++)
    {
        v += bodies[i].vel.norm();
    }
    
    v /= (double)Ndisk;
    
    std::cout << "Average Disk Particle Speed: " << v << "\n";
    std::cout << "Disk Size: " << diskcut << "\n";
    std::cout << "Disk Crossing Time: " << diskcut / v << "\n";
    
    
    writeinitfile(Ntot, Nhalo, bodies, "data/initfile.txt");
    
    
 
}
Esempio n. 14
0
const Vector &
Element::getResistingForceIncInertia(void) 
{
  if (index == -1) {
    this->setRayleighDampingFactors(alphaM, betaK, betaK0, betaKc);
  }

  Matrix *theMatrix = theMatrices[index]; 
  Vector *theVector = theVectors2[index];
  Vector *theVector2 = theVectors1[index];

  //
  // perform: R = P(U) - Pext(t);
  //

  (*theVector) = this->getResistingForce();

  //
  // perform: R = R - M * a
  //

  int loc = 0;
  Node **theNodes = this->getNodePtrs();
  int numNodes = this->getNumExternalNodes();

  int i;
  for (i=0; i<numNodes; i++) {
    const Vector &acc = theNodes[i]->getAccel();
    for (int i=0; i<acc.Size(); i++) {
      (*theVector2)(loc++) = acc(i);
    }
  }
  theVector->addMatrixVector(1.0, this->getMass(), *theVector2, +1.0);

  //
  // perform: R = R + (alphaM * M + betaK0 * K0 + betaK * K) * v
  //            = R + D * v
  //

  // determine the vel vector from ele nodes
  loc = 0;
  for (i=0; i<numNodes; i++) {
    const Vector &vel = theNodes[i]->getTrialVel();
    for (int i=0; i<vel.Size(); i++) {
      (*theVector2)(loc++) = vel[i];
    }
  }

  // now compute the damping matrix
  theMatrix->Zero();
  if (alphaM != 0.0)
    theMatrix->addMatrix(0.0, this->getMass(), alphaM);
  if (betaK != 0.0)
    theMatrix->addMatrix(1.0, this->getTangentStiff(), betaK);      
  if (betaK0 != 0.0)
    theMatrix->addMatrix(1.0, this->getInitialStiff(), betaK0);      
  if (betaKc != 0.0)
    theMatrix->addMatrix(1.0, *Kc, betaKc);      

  // finally the D * v
  theVector->addMatrixVector(1.0, *theMatrix, *theVector2, 1.0);

  return *theVector;
}
/*!
 * \brief Move joints synchronous
 *
 * Adjusting velocity of all joints to reach the final angules at the same time
 */
bool PowerCubeCtrl::MoveJointSpaceSync(const std::vector<double>& target)
{
	PCTRL_CHECK_INITIALIZED();
	unsigned int DOF = m_params->GetDOF();

	std::vector<std::string> errorMessages;
	PC_CTRL_STATUS status;
	getStatus(status, errorMessages);
	if ((status != PC_CTRL_OK))
	{
		m_ErrorMessage.assign("");
		for (unsigned int i = 0; i < DOF; i++)
		{
			m_ErrorMessage.append(errorMessages[i]);
			m_ErrorMessage.append("\n");
		}
		return false;
	}

	std::vector<double> vel(DOF);
	std::vector<double> acc(DOF);

	double TG = 0;

	try
	{
		/// calculate which joint takes the longest time to reach goal
		std::vector<double> times(DOF);
		for (unsigned int i = 0; i < DOF; i++)
		{
			RampCommand rm(m_positions[i], m_velocities[i], target[i], m_params->GetMaxAcc()[i],
						   m_params->GetMaxVel()[i]);
			times[i] = rm.getTotalTime();
		}

		/// determine the joint index that has the greatest value for time
		int furthest = 0;
		double max = times[0];
		for (unsigned int i = 1; i < DOF; i++)
		{
			if (times[i] > max)
			{
				max = times[i];
				furthest = i;
			}
		}

		RampCommand rm_furthest(m_positions[furthest], m_velocities[furthest], target[furthest],
                                m_params->GetMaxAcc()[furthest], m_params->GetMaxVel()[furthest]);

		double T1 = rm_furthest.T1();
		double T2 = rm_furthest.T2();
		double T3 = rm_furthest.T3();

		/// total time:
		TG = T1 + T2 + T3;

		/// calculate velocity and acceleration for all joints:
		acc[furthest] = m_params->GetMaxAcc()[furthest];
		vel[furthest] = m_params->GetMaxVel()[furthest];
		for (unsigned int i = 0; i < DOF; i++)
		{
			if (int(i) != furthest)
			{
				double a;
				double v;
				RampCommand::calculateAV(m_positions[i], m_velocities[i], target[i], TG, T3, m_params->GetMaxAcc()[i],
										 m_params->GetMaxVel()[i], a, v);

				acc[i] = a;
				vel[i] = v;
			}
		}
	}
	catch (...)
	{
		return false;
	}

	/// Send motion commands to hardware
	for (unsigned int i = 0; i < DOF; i++)
	{
		pthread_mutex_lock(&m_mutex);
		PCube_moveRamp(m_DeviceHandle, m_params->GetModuleIDs()[i], target[i], fabs(vel[i]), fabs(acc[i]));
		pthread_mutex_unlock(&m_mutex);
	}

	pthread_mutex_lock(&m_mutex);
	PCube_startMotionAll(m_DeviceHandle);
	pthread_mutex_unlock(&m_mutex);

	return true;
}
void PiecewiseConstantDiscontinuousScalarSpaceBarycentric<
    BasisFunctionType>::assignDofsImpl(const GridSegment &segment) {

  const GridView &view = this->gridView();

  std::unique_ptr<GridView> viewCoarseGridPtr = this->grid()->levelView(0);
  const GridView &viewCoarseGrid = *viewCoarseGridPtr;

  const Mapper &elementMapper = view.elementMapper();
  const Mapper &elementMapperCoarseGrid = viewCoarseGrid.elementMapper();

  int elementCount = view.entityCount(0);
  int elementCountCoarseGrid = viewCoarseGrid.entityCount(0);

  // Assign gdofs to grid vertices (choosing only those that belong to
  // the selected grid segment)
  std::vector<int> continuousDofIndices(elementCountCoarseGrid, 0);
  segment.markExcludedEntities(0, continuousDofIndices);
  int continuousDofCount_ = 0;
  for (int elementIndex = 0; elementIndex < elementCountCoarseGrid;
       ++elementIndex)
    if (acc(continuousDofIndices, elementIndex) == 0) // not excluded
      acc(continuousDofIndices, elementIndex) = continuousDofCount_++;

  // (Re)initialise DOF maps
  m_local2globalDofs.clear();
  m_local2globalDofs.resize(elementCount);
  m_global2localDofs.clear();
  m_global2localDofs.reserve(elementCount);

  // Iterate over elements
  std::unique_ptr<EntityIterator<0>> itCoarseGrid =
      viewCoarseGrid.entityIterator<0>();
  int flatLocalDofCount_ = 0;
  while (!itCoarseGrid->finished()) {
    const Entity<0> &elementCoarseGrid = itCoarseGrid->entity();
    EntityIndex elementIndexCoarseGrid =
        elementMapperCoarseGrid.entityIndex(elementCoarseGrid);

    // Iterate through refined elements
    std::unique_ptr<EntityIterator<0>> sonIt =
        elementCoarseGrid.sonIterator(this->grid()->maxLevel());
    while (!sonIt->finished()) {
      const Entity<0> &element = sonIt->entity();
      int elementIndex = elementMapper.entityIndex(element);
      std::vector<GlobalDofIndex> &globalDofs =
          acc(m_local2globalDofs, elementIndex);
      int continuousDofIndex =
          acc(continuousDofIndices, elementIndexCoarseGrid);
      if (continuousDofIndex != -1) {
        globalDofs.push_back(flatLocalDofCount_);
        m_global2localDofs.push_back(std::vector<LocalDof>());
        acc(m_global2localDofs, flatLocalDofCount_)
            .push_back(LocalDof(elementIndex, 0));
        ++flatLocalDofCount_;
      } else {
        globalDofs.push_back(-1);
      }
      sonIt->next();
    }
    itCoarseGrid->next();
  }

  // Initialize the container mapping the flat local dof indices to
  // local dof indices
  SpaceHelper<BasisFunctionType>::initializeLocal2FlatLocalDofMap(
      flatLocalDofCount_, m_local2globalDofs, m_flatLocal2localDofs);
}
Esempio n. 17
0
rspfRefPtr<rspfProperty> rspfDtedInfo::getProperty(
   const rspfString& name)const
{
   rspfRefPtr<rspfProperty> result = 0;

   //---
   // Look through dted records.
   // Must have uhl, dsi and acc records.  vol and hdr
   // are for magnetic tape only; hence, may or may not be there.
   //---
   rspfDtedVol vol(theFile, 0);
   if( vol.getErrorStatus() == rspfErrorCodes::RSPF_OK )
   {
      if (name == "dted_vol_record")
      {
         rspfContainerProperty* box = new rspfContainerProperty();
         box->setName(name);

         std::vector<rspfString> list;
         vol.getPropertyNames(list);

         std::vector< rspfRefPtr<rspfProperty> > propList;

         std::vector<rspfString>::const_iterator i = list.begin();
         while (i != list.end())
         {
            rspfRefPtr<rspfProperty> prop = vol.getProperty( (*i) );
            if (prop.valid())
            {
               propList.push_back(prop);
            }
            ++i;
         }
         box->addChildren(propList);
         result = box;
      }
   }
   if (result.valid() == false)
   {
      rspfDtedHdr hdr(theFile, vol.stopOffset());
      if( hdr.getErrorStatus() == rspfErrorCodes::RSPF_OK )
      {
         if (name == "dted_hdr_record")
         {
            rspfContainerProperty* box = new rspfContainerProperty();
            box->setName(name);
            
            std::vector<rspfString> list;
            hdr.getPropertyNames(list);
            
            std::vector< rspfRefPtr<rspfProperty> > propList;
            
            std::vector<rspfString>::const_iterator i = list.begin();
            while (i != list.end())
            {
               rspfRefPtr<rspfProperty> prop = hdr.getProperty( (*i) );
               if (prop.valid())
               {
                  propList.push_back(prop);
               }
               ++i;
            }
            box->addChildren(propList);
            result = box;
         }
      }
      if (result.valid() == false)
      {
         rspfDtedUhl uhl(theFile, hdr.stopOffset());
         if( uhl.getErrorStatus() == rspfErrorCodes::RSPF_OK )
         {
            if (name == "dted_uhl_record")
            {
               rspfContainerProperty* box = new rspfContainerProperty();
               box->setName(name);
               
               std::vector<rspfString> list;
               uhl.getPropertyNames(list);
               
               std::vector< rspfRefPtr<rspfProperty> > propList;
               
               std::vector<rspfString>::const_iterator i = list.begin();
               while (i != list.end())
               {
                  rspfRefPtr<rspfProperty> prop = uhl.getProperty( (*i) );
                  if (prop.valid())
                  {
                     propList.push_back(prop); 
                  }
                  ++i;
               }
               box->addChildren(propList);
               result = box;
            }
         }
         if (result.valid() == false)
         {
            rspfDtedDsi dsi(theFile, uhl.stopOffset());
            if( dsi.getErrorStatus() == rspfErrorCodes::RSPF_OK )
            {
               if (name == "dted_dsi_record")
               {
                  rspfContainerProperty* box =
                     new rspfContainerProperty();
                  box->setName(name);
                  
                  std::vector<rspfString> list;
                  dsi.getPropertyNames(list);
                  
                  std::vector< rspfRefPtr<rspfProperty> > propList;
                  
                  std::vector<rspfString>::const_iterator i = list.begin();
                  while (i != list.end())
                  {
                     rspfRefPtr<rspfProperty> prop =
                        dsi.getProperty( (*i) );
                     if (prop.valid())
                     {
                        propList.push_back(prop);
                     }
                     ++i;
                  }
                  box->addChildren(propList);
                  result = box;
               }
            }
            if (result.valid() == false)
            {
               rspfDtedAcc acc(theFile, dsi.stopOffset());
               if( acc.getErrorStatus() == rspfErrorCodes::RSPF_OK )
               {
                  if (name == "dted_acc_record")
                  {
                     rspfContainerProperty* box =
                        new rspfContainerProperty();
                     box->setName(name);
                     
                     std::vector<rspfString> list;
                     acc.getPropertyNames(list);
                     
                     std::vector< rspfRefPtr<rspfProperty> > propList;
                     
                     rspfRefPtr<rspfProperty> prop = 0;
                     std::vector<rspfString>::const_iterator i =
                        list.begin();
                     while (i != list.end())
                     {
                        rspfRefPtr<rspfProperty> prop =
                           acc.getProperty( (*i) );
                        if (prop.valid())
                        {
                           propList.push_back(prop);
                        }
                        ++i;
                     }
                     box->addChildren(propList);
                     result = box;
                  }
               }
            }
         }
      }
   }

   return result;
}
Esempio n. 18
0
    result_type operator()( individual_type & ind ) {
        individual_trait_acc_type acc;
        result_type res = acc( ind );

        return res;
    }
Esempio n. 19
0
ucac4Region_t *UCAC4::getRegion(int region)
{
    if (m_regionMap.contains(region))
    {
        return &m_regionMap[region];
    }

    ucac4Region_t regionData;
    ucac4Region_t *regionPtr = &regionData;

    int minRa  = (int)(SkMath::toDeg(g_dataResource->getGscRegions()->gscRegionSector[region].RaMin) * 3600. * 1000.);
    int maxRa  = (int)(SkMath::toDeg(g_dataResource->getGscRegions()->gscRegionSector[region].RaMax) * 3600. * 1000.);
    int minDec = (int)((SkMath::toDeg(g_dataResource->getGscRegions()->gscRegionSector[region].DecMin) + 90.) * 3600. * 1000.);
    int maxDec = (int)((SkMath::toDeg(g_dataResource->getGscRegions()->gscRegionSector[region].DecMax) + 90.) * 3600. * 1000.);

    if (minRa > maxRa)
    {
        maxRa = 360 * 3600 * 1000;
    }

    const double zone_height = 0.2;  // zones are .2 degrees each
    double dDecMax = SkMath::toDeg(g_dataResource->getGscRegions()->gscRegionSector[region].DecMax);
    double dDecMin = SkMath::toDeg(g_dataResource->getGscRegions()->gscRegionSector[region].DecMin);

    int zoneEnd = (int)((dDecMax + 90.) / zone_height) + 1;
    int zoneStart = (int)((dDecMin + 90.) / zone_height) + 1;

    zoneEnd = CLAMP(zoneEnd, 1, 900);
    zoneStart = CLAMP(zoneStart, 1, 900);

    if (zoneStart > zoneEnd)
    {
        qSwap(zoneStart, zoneEnd);
    }

    if (maxDec < minDec)
    {
        qSwap(maxDec, minDec);
    }

    for (int z = zoneStart; z <= zoneEnd; z++)
    {
        SkFile f(m_folder + QString("/z%1").arg(z, 3, 10, QChar('0')));
        SkFile acc(m_folder + QString("/z%1.acc").arg(z, 3, 10, QChar('0')));

        m_accList.clear();

        if (!readAccFile(acc))
        {
            if (acc.open(QFile::WriteOnly))
            {   // create accelerated index file
                if (f.open(QFile::ReadOnly))
                {
                    quint64 lastOffset = 0;
                    int lastRa = 0;
                    int index = 1;
                    int step = 2.5 * 3600 * 1000; // 2.5 deg. step
                    while (!f.atEnd())
                    {
                        UCAC4_Star_t star;

                        f.read((char *)&star, sizeof(star));

                        if (star.ra >= lastRa)
                        {
                            lastOffset = f.pos() - sizeof(star);

                            acc.write((char *)&lastRa, sizeof(lastRa));
                            acc.write((char *)&index, sizeof(index));
                            acc.write((char *)&lastOffset, sizeof(lastOffset));

                            ucac4AccFile_t acc;

                            acc.ra = lastRa;
                            acc.index = index;
                            acc.offset = lastOffset;

                            m_accList.append(acc);

                            lastRa += step;
                        }
                        index++;
                    }
                    f.close();
                    acc.close();
                }
            }
        }

        if (f.open(QFile::ReadOnly))
        {
            quint64 last = 0;
            int lastIndex = 1;
            foreach (const ucac4AccFile_t &acc, m_accList)
            {
                if (acc.ra >= minRa)
                {
                    f.seek(last);
                    break;
                }
                last = acc.offset;
                lastIndex = acc.index;
            }

            int ucac4Index = lastIndex;
            while (!f.atEnd())
            {
                UCAC4_Star_t star;

                f.read((char *)&star, sizeof(star));

                if (star.ra >= maxRa)
                {
                    break;
                }

                if (star.spd > minDec && star.spd < maxDec &&
                        star.ra > minRa)
                {
                    /*
                    skStar.rd.Ra = D2R(star.ra / 3600. / 1000.0);
                    skStar.rd.Dec = D2R((star.spd / 3600. / 1000.0) - 90.0);
                    skStar.mag = star.mag2 / 1000.0;
                    skStar.number = ucac4Index;
                    skStar.zone = z;
                    */

                    star.ucac4_zone = z;
                    star.ucac4_number = ucac4Index;

                    regionPtr->m_stars.append(star);
                }
                ucac4Index++;
            }
        }
    }
Esempio n. 20
0
void SensorFilter::update(FilteredSensorData& filteredSensorData,
                          const SensorData& theSensorData,
                          const InertiaSensorData& theInertiaSensorData,
                          const OrientationData& theOrientationData)
{
  // copy sensor data (except gyro and acc)
  Vector2<> gyro(filteredSensorData.data[SensorData::gyroX], filteredSensorData.data[SensorData::gyroY]);
  Vector3<> acc(filteredSensorData.data[SensorData::accX], filteredSensorData.data[SensorData::accY], filteredSensorData.data[SensorData::accZ]);
  (SensorData&)filteredSensorData = theSensorData;
  filteredSensorData.data[SensorData::gyroX] = gyro.x;
  filteredSensorData.data[SensorData::gyroY] = gyro.y;
  filteredSensorData.data[SensorData::accX] = acc.x;
  filteredSensorData.data[SensorData::accY] = acc.y;
  filteredSensorData.data[SensorData::accZ] = acc.z;

  // take calibrated inertia sensor data
  for(int i = 0; i < 2; ++i)
  {
    if(theInertiaSensorData.gyro[i] != InertiaSensorData::off)
      filteredSensorData.data[SensorData::gyroX + i] = theInertiaSensorData.gyro[i];
    else if(filteredSensorData.data[SensorData::gyroX + i] == SensorData::off)
      filteredSensorData.data[SensorData::gyroX + i] = 0.f;
  }
  filteredSensorData.data[SensorData::gyroZ] = 0.f;
  for(int i = 0; i < 3; ++i)
  {
    if(theInertiaSensorData.acc[i] != InertiaSensorData::off)
      filteredSensorData.data[SensorData::accX + i] = theInertiaSensorData.acc[i] / 9.80665f;
    else if(filteredSensorData.data[SensorData::accX + i] == SensorData::off)
      filteredSensorData.data[SensorData::accX + i] = 0.f;
  }

  // take orientation data
  filteredSensorData.data[SensorData::angleX] = theOrientationData.orientation.x;
  filteredSensorData.data[SensorData::angleY] = theOrientationData.orientation.y;

#ifndef RELEASE
  if(filteredSensorData.data[SensorData::gyroX] != SensorData::off)
  {
    gyroAngleXSum += filteredSensorData.data[SensorData::gyroX] * (theSensorData.timeStamp - lastIteration) * 0.001f;
    gyroAngleXSum = normalize(gyroAngleXSum);
    lastIteration = theSensorData.timeStamp;
  }
  //PLOT("module:SensorFilter:gyroAngleXSum", gyroAngleXSum);
#endif

  /*
  PLOT("module:SensorFilter:rawAngleX", theSensorData.data[SensorData::angleX]);
  PLOT("module:SensorFilter:rawAngleY", theSensorData.data[SensorData::angleY]);

  PLOT("module:SensorFilter:rawAccX", theSensorData.data[SensorData::accX]);
  PLOT("module:SensorFilter:rawAccY", theSensorData.data[SensorData::accY]);
  PLOT("module:SensorFilter:rawAccZ", theSensorData.data[SensorData::accZ]);

  PLOT("module:SensorFilter:rawGyroX", theSensorData.data[SensorData::gyroX]);
  PLOT("module:SensorFilter:rawGyroY", theSensorData.data[SensorData::gyroY]);
  PLOT("module:SensorFilter:rawGyroZ", theSensorData.data[SensorData::gyroZ]);

  PLOT("module:SensorFilter:angleX", filteredSensorData.data[SensorData::angleX]);
  PLOT("module:SensorFilter:angleY", filteredSensorData.data[SensorData::angleY]);

  PLOT("module:SensorFilter:accX", filteredSensorData.data[SensorData::accX]);
  PLOT("module:SensorFilter:accY", filteredSensorData.data[SensorData::accY]);
  PLOT("module:SensorFilter:accZ", filteredSensorData.data[SensorData::accZ]);

  PLOT("module:SensorFilter:gyroX", filteredSensorData.data[SensorData::gyroX] != float(SensorData::off) ? filteredSensorData.data[SensorData::gyroX] : 0);
  PLOT("module:SensorFilter:gyroY", filteredSensorData.data[SensorData::gyroY] != float(SensorData::off) ? filteredSensorData.data[SensorData::gyroY] : 0);
  PLOT("module:SensorFilter:gyroZ", filteredSensorData.data[SensorData::gyroZ] != float(SensorData::off) ? filteredSensorData.data[SensorData::gyroZ] : 0);

  //PLOT("module:SensorFilter:us", filteredSensorData.data[SensorData::us]);
  */
}
Esempio n. 21
0
void H264CubemapSource::getNextCubemapLoop()
{
    int64_t lastPTS = 0;
    boost::chrono::system_clock::time_point lastDisplayTime(boost::chrono::microseconds(0));
    
    while (true)
    {
        size_t pendingCubemaps;
        int frameSeqNum;
        // Get frames with the oldest frame seq # and remove the associated bucket
        std::vector<AVFrame*> frames;
        {
            boost::mutex::scoped_lock lock(frameMapMutex);
            
            if (frameMap.size() < 30)
            {
                frameMapCondition.wait(lock);
            }
            pendingCubemaps = frameMap.size();
            
            auto it = frameMap.begin();
            frameSeqNum = it->first;
            lastFrameSeqNum = frameSeqNum;
            frames = it->second;
            frameMap.erase(it);
        }
        
        StereoCubemap* cubemap;
        
        // Allocate cubemap if necessary
        if (!oldCubemap)
        {
            int width, height;
            for (AVFrame* frame : frames)
            {
                if (frame)
                {
                    width  = frame->width;
                    height = frame->height;
                    break;
                }
            }
            
            std::vector<Cubemap*> eyes;
            for (int j = 0, faceIndex = 0; j < StereoCubemap::MAX_EYES_COUNT && faceIndex < sinks.size(); j++)
            {
                std::vector<CubemapFace*> faces;
                for (int i = 0; i < Cubemap::MAX_FACES_COUNT && faceIndex < sinks.size(); i++, faceIndex++)
                {
                    Frame* content = Frame::create(width,
                                                   height,
                                                   format,
                                                   boost::chrono::system_clock::time_point(),
                                                   heapAllocator);
                    
                    CubemapFace* face = CubemapFace::create(content,
                                                            i,
                                                            heapAllocator);
                    
                    faces.push_back(face);
                }
                
                eyes.push_back(Cubemap::create(faces, heapAllocator));
            }
            
            cubemap = StereoCubemap::create(eyes, heapAllocator);
        }
        else
        {
            cubemap = oldCubemap;
        }
        
        size_t count = 0;
        // Fill cubemap making sure stereo pairs match
        for (int i = 0; i < (std::min)(frames.size(), (size_t)CUBEMAP_MAX_FACES_COUNT); i++)
        {
            AVFrame*     leftFrame = frames[i];
            CubemapFace* leftFace  = cubemap->getEye(0)->getFace(i, true);
            
            AVFrame*     rightFrame = nullptr;
            CubemapFace* rightFace  = nullptr;
            
            if (frames.size() > i + CUBEMAP_MAX_FACES_COUNT)
            {
                rightFrame = frames[i+CUBEMAP_MAX_FACES_COUNT];
                rightFace  = cubemap->getEye(1)->getFace(i, true);
            }
            
            if (matchStereoPairs && frames.size() > i + CUBEMAP_MAX_FACES_COUNT)
            {
                // check if matched
                if (!leftFrame || !rightFrame)
                {
                    // if they don't match give them back and forget about them
                    sinks[i]->returnFrame(leftFrame);
                    sinks[i + CUBEMAP_MAX_FACES_COUNT]->returnFrame(rightFrame);
                    leftFrame  = nullptr;
                    rightFrame = nullptr;
                }
            }
            
            // Fill the cubemapfaces with pixels if pixels are available
            if (leftFrame)
            {
                count++;
                leftFace->setNewFaceFlag(true);
                avpicture_layout((AVPicture*)leftFrame, (AVPixelFormat)leftFrame->format,
                                 leftFrame->width, leftFrame->height,
                                 (unsigned char*)leftFace->getContent()->getPixels(), leftFace->getContent()->getWidth() * leftFace->getContent()->getHeight() * 4);
                sinks[i]->returnFrame(leftFrame);
                if (onScheduledFrameInCubemap) onScheduledFrameInCubemap(this, i);
            }
            else
            {
                leftFace->setNewFaceFlag(false);
            }
            
            if (rightFrame)
            {
                count++;
                rightFace->setNewFaceFlag(true);
                avpicture_layout((AVPicture*)rightFrame, (AVPixelFormat)rightFrame->format,
                                 rightFrame->width, rightFrame->height,
                                 (unsigned char*)rightFace->getContent()->getPixels(), rightFace->getContent()->getWidth() * rightFace->getContent()->getHeight() * 4);
                sinks[i + CUBEMAP_MAX_FACES_COUNT]->returnFrame(rightFrame);
                if (onScheduledFrameInCubemap) onScheduledFrameInCubemap(this, i+CUBEMAP_MAX_FACES_COUNT);
            }
            else if (rightFace)
            {
                rightFace->setNewFaceFlag(false);
            }
        }
        
        // Give it to the user of this library (AlloPlayer etc.)
        if (onNextCubemap)
        {
            // calculate PTS for the cubemap (median of the individual faces' PTS)
            boost::accumulators::accumulator_set<int64_t, boost::accumulators::features<boost::accumulators::tag::median> > acc;
            for (AVFrame* frame : frames)
            {
                if (frame)
                {
                    acc(frame->pts);
                }
            }
            int64_t pts = boost::accumulators::median(acc);
            
            // Calculate time interval from until this cubemap should be displayed
            if (lastPTS == 0)
            {
                lastPTS = pts;
            }
            
            if (lastDisplayTime.time_since_epoch().count() == 0)
            {
                lastDisplayTime = boost::chrono::system_clock::now();
                lastDisplayTime += boost::chrono::seconds(5);
            }
            uint64_t ptsDiff = pts - lastPTS;
            lastDisplayTime += boost::chrono::microseconds(ptsDiff);
            lastPTS = pts;
            
            boost::chrono::milliseconds sleepDuration = boost::chrono::duration_cast<boost::chrono::milliseconds>(lastDisplayTime - boost::chrono::system_clock::now());
            
            // Wait until frame should be displayed
            //boost::this_thread::sleep_for(sleepDuration);
            
            // Display frame
            oldCubemap = onNextCubemap(this, cubemap);
		}
    }
}
Esempio n. 22
0
void AugmentationNode::augment_images(vector<cv::Mat> imgs, vector<int> labels)
{
	int	shift;
	vector< vector<cv::Mat> > 	out_imgs;
	vector< vector<int> > 		out_labels;

	out_imgs.resize(_aug_factor + 1);
	out_labels.resize(_aug_factor + 1);

	for(int k=0; k < imgs.size(); k++){
		_counter++;
		
		// Expand source image (Assuming image is square)
		//
		shift = imgs[k].cols / 2;
		cv::Mat	expImg(imgs[k].rows * 2, imgs[k].cols * 2, imgs[k].type(), 255);
		if( imgs[k].type() == CV_8UC3 ) {
			// Color image, make sure all channels set to 255
			expImg = cv::Scalar(255, 255, 255);
		}
		imgs[k].copyTo(expImg(cv::Rect(shift, shift, imgs[k].cols, imgs[k].rows)));
	
		
		// Get ROI
		float tl_row = expImg.rows/2.0F - shift;
		float tl_col = expImg.cols/2.0F - shift;
		float br_row = expImg.rows/2.0F + shift;
		float br_col = expImg.cols/2.0F + shift;
		cv::Point tl(tl_row, tl_col);
		cv::Point br(br_row, br_col);

		// Setup a rectangle to define region of interest
		cv::Rect cellROI(tl, br);

		for(int i=0; i < _aug_factor; i++) {
			_counter++;

			// Define variables
			int label = labels[k];

			// construct a trivial random generator engine from a time-based seed:
	  		unsigned seed = chrono::system_clock::now().time_since_epoch().count();
	  		default_random_engine generator(seed);
			uniform_real_distribution<double> uniform_dist(0.0, 1.0);

			double Flipx = round(uniform_dist(generator)); 				// randomly distributed reflections over x and y
			double Flipy = round(uniform_dist(generator));
			double Phi = 360 * uniform_dist(generator); 			// rotation angle uniformly distributed on [0, 2pi] radians
			double Tx = 20 * (uniform_dist(generator) - 0.5); 			// 10-pixel uniformly distributed translation
			double Ty = 20 * (uniform_dist(generator) - 0.5);
			double Shx = PI/180 * 20 * (uniform_dist(generator)-0.5);  	// shear angle uniformly distributed on [-20, 20] degrees
			double Shy = PI/180 * 20 * (uniform_dist(generator)-0.5);
			double Sx = 1/1.2 + (1.2 - 1/1.2)*uniform_dist(generator); 	// scale uniformly distributed on [1/1.2, 1.2]
			double Sy = 1/1.2 + (1.2 - 1/1.2)*uniform_dist(generator);

			cv::Mat warped_img;

			// Translation
			cv::Mat trans_M(3, 3, CV_64F);
			trans_M.at<double>(0,0) = 1;
			trans_M.at<double>(0,1) = 0;
			trans_M.at<double>(0,2) = Tx;

			trans_M.at<double>(1,0) = 0;
			trans_M.at<double>(1,1) = 1;
			trans_M.at<double>(1,2) = Ty;

			trans_M.at<double>(2,0) = 0;
			trans_M.at<double>(2,1) = 0;
			trans_M.at<double>(2,2) = 0;

			// Scaling
			cv::Mat scaling_M(3, 3, CV_64F);
			scaling_M.at<double>(0,0) = Sx;
			scaling_M.at<double>(0,1) = 0;
			scaling_M.at<double>(0,2) = 0;

			scaling_M.at<double>(1,0) = 0;
			scaling_M.at<double>(1,1) = Sy;
			scaling_M.at<double>(1,2) = 0;

			scaling_M.at<double>(2,0) = 0;
			scaling_M.at<double>(2,1) = 0;
			scaling_M.at<double>(2,2) = 0;

			// Shearing 
			cv::Mat shear_M(3, 3, CV_64F);
			shear_M.at<double>(0,0) = 1;
			shear_M.at<double>(0,1) = Shy;
			shear_M.at<double>(0,2) = 0;

			shear_M.at<double>(1,0) = Shx;
			shear_M.at<double>(1,1) = 1;
			shear_M.at<double>(1,2) = 0;

			shear_M.at<double>(2,0) = 0;
			shear_M.at<double>(2,1) = 0;
			shear_M.at<double>(2,2) = 0;

			// Rotation 
			cv::Mat rot(2, 3, CV_64F);
			cv::Point2f center(expImg.rows/2.0F, expImg.cols/2.0F);
			rot = getRotationMatrix2D(center, Phi, 1.0);	

			cv::Mat rot_M(3, 3, CV_64F, cv::Scalar(0));
			rot(cv::Rect(0,0,3,2)).copyTo(rot_M.colRange(0,3).rowRange(0,2));

			// Accumulate
			cv::Mat acc(3, 3, CV_64F);

			acc = trans_M * scaling_M * shear_M * rot_M;

			cv::Mat acc_M = acc.colRange(0,3).rowRange(0,2);
			cv::warpAffine( expImg, warped_img, acc_M, expImg.size());

			// Crop Image
			double m00 = acc_M.at<double>(0,0);
			double m01 = acc_M.at<double>(0,1);
			double m10 = acc_M.at<double>(1,0);
			double m11 = acc_M.at<double>(1,1);
			double m02 = acc_M.at<double>(0,2);
			double m12 = acc_M.at<double>(1,2); 

			int new_cx = expImg.rows/2.0F * m00 + expImg.cols/2.0F * m01 + m02;
			int new_cy = expImg.rows/2.0F * m10 + expImg.cols/2.0F * m11 + m12;

			// Get ROI
			double tl_row = new_cx - shift;
			double tl_col = new_cy - shift;
			double br_row = new_cx + shift;
			double br_col = new_cy + shift;
			cv::Point tl(tl_row, tl_col);
			cv::Point br(br_row, br_col);


			cv::Rect new_cellROI(tl, br);
			cv::Mat final_img = warped_img(new_cellROI);

			// Flip
			if( Flipx == 1 && Flipy == 1) {
				cv::flip(final_img, final_img, -1);
			}
			else if(Flipx == 0 && Flipy == 1){
				cv::flip(final_img, final_img, 1);
			}
			else if(Flipx == 1 && Flipy == 0){
				cv::flip(final_img, final_img, 0);
			}
			
        	if(!final_img.isContinuous()){
				final_img=final_img.clone();
			}

			// Accumulate
			out_imgs[i].push_back(final_img);
			out_labels[i].push_back(label);
		}

		out_imgs[_aug_factor].push_back(imgs[k]);
		out_labels[_aug_factor].push_back(labels[k]);

		// TODO - For now we are overriding the mode until we get Mode::Alternate 
		// working properly
		if( out_imgs[0].size() > _transferSize ) {
			for(int e = 0; e < out_imgs.size(); e++) {
			
				copy_to_edge(out_imgs[e], out_labels[e], e % _out_edges.size());
				// Clean
				out_imgs[e].clear();
				out_labels[e].clear();
			}
		}
	}

	// Send any leftover data
	if( out_imgs[0].size() > 0 ) {

		// TODO - For now we are overriding the mode until we get Mode::Alternate 
		// working properly
		for(int e = 0; e < out_imgs.size(); e++) {
			copy_to_edge(out_imgs[e], out_labels[e], e % _out_edges.size());
			// Clean
			out_imgs[e].clear();
			out_labels[e].clear();
		}
	}
}
Esempio n. 23
0
NewtonCollision* CreateConvexCollision (NewtonWorld* world, const dMatrix& srcMatrix, const dVector& originalSize, PrimitiveType type, int materialID__)
{
	dVector size (originalSize);

	NewtonCollision* collision = NULL;
	switch (type) 
	{
		case _NULL_PRIMITIVE:
		{
			collision = NewtonCreateNull (world); 
			break;
		}

		case _SPHERE_PRIMITIVE:
		{
			// create the collision 
			collision = NewtonCreateSphere (world, size.m_x * 0.5f, 0, NULL); 
			break;
		}

		case _BOX_PRIMITIVE:
		{
			// create the collision 
			collision = NewtonCreateBox (world, size.m_x, size.m_y, size.m_z, 0, NULL); 
			break;
		}


		case _CONE_PRIMITIVE:
		{
			dFloat r = size.m_x * 0.5f;
			dFloat h = size.m_y;

			// create the collision 
			collision = NewtonCreateCone (world, r, h, 0, NULL); 
			break;
		}

		case _CYLINDER_PRIMITIVE:
		{
			// create the collision 
			collision = NewtonCreateCylinder (world, size.m_x * 0.5f, size.m_y, 0, NULL); 
			break;
		}


		case _CAPSULE_PRIMITIVE:
		{
			// create the collision 
			collision = NewtonCreateCapsule (world, size.m_x * 0.5f, size.m_y, 0, NULL); 
			break;
		}

		case _TAPERED_CAPSULE_PRIMITIVE:
		{
			// create the collision 
			collision = NewtonCreateTaperedCapsule (world, size.m_x * 0.5f, size.m_z * 0.5f, size.m_y, 0, NULL); 
			break;
		}


		case _CHAMFER_CYLINDER_PRIMITIVE:
		{
			// create the collision 
			collision = NewtonCreateChamferCylinder (world, size.m_x * 0.5f, size.m_y, 0, NULL); 
			break;
		}

		case _TAPERED_CYLINDER_PRIMITIVE:
		{
			// create the collision 
			collision = NewtonCreateTaperedCylinder (world, size.m_x * 0.5f, size.m_z * 0.5f, size.m_y, 0, NULL); 
			break;
		}


		case _RANDOM_CONVEX_HULL_PRIMITIVE:
		{
			// Create a clouds of random point around the origin
			#define SAMPLE_COUNT 200
			dVector cloud [SAMPLE_COUNT];

			// make sure that at least the top and bottom are present
			cloud [0] = dVector ( size.m_x * 0.5f, 0.0f, 0.0f, 0.0f);
			cloud [1] = dVector (-size.m_x * 0.5f, 0.0f, 0.0f, 0.0f);
			cloud [2] = dVector ( 0.0f,  size.m_y * 0.5f, 0.0f, 0.0f); 
			cloud [3] = dVector ( 0.0f, -size.m_y * 0.5f, 0.0f, 0.0f);
			cloud [4] = dVector (0.0f, 0.0f,  size.m_z * 0.5f, 0.0f); 
			cloud [5] = dVector (0.0f, 0.0f, -size.m_z * 0.5f, 0.0f); 

			int count = 6;
			// populate the cloud with pseudo Gaussian random points
			for (int i = 6; i < SAMPLE_COUNT; i ++) {
				cloud [i].m_x = RandomVariable(size.m_x);
				cloud [i].m_y = RandomVariable(size.m_y);
				cloud [i].m_z = RandomVariable(size.m_z);
				count ++;
			}
			collision = NewtonCreateConvexHull (world, count, &cloud[0].m_x, sizeof (dVector), 0.01f, 0, NULL); 
			break;
		}

		case _REGULAR_CONVEX_HULL_PRIMITIVE:
		{
			// Create a clouds of random point around the origin
			#define STEPS_HULL 6
			//#define STEPS_HULL 3

			dVector cloud [STEPS_HULL * 4 + 256];
			int count = 0;
			dFloat radius = size.m_y;
			dFloat height = size.m_x * 0.999f;
			dFloat x = - height * 0.5f;
			dMatrix rotation (dPitchMatrix(2.0f * 3.141592f / STEPS_HULL));
			for (int i = 0; i < 4; i ++) {
				dFloat pad = ((i == 1) || (i == 2)) * 0.25f * radius;
				dVector p (x, 0.0f, radius + pad);
				x += 0.3333f * height;
				dMatrix acc (dGetIdentityMatrix());
				for (int j = 0; j < STEPS_HULL; j ++) {
					cloud[count] = acc.RotateVector(p);
					acc = acc * rotation;
					count ++;
				}
			}

			collision = NewtonCreateConvexHull (world, count, &cloud[0].m_x, sizeof (dVector), 0.02f, 0, NULL); 
			break;
		}

		case _COMPOUND_CONVEX_CRUZ_PRIMITIVE:
		{
			//dMatrix matrix (GetIdentityMatrix());
			dMatrix matrix (dPitchMatrix(15.0f * 3.1416f / 180.0f) * dYawMatrix(15.0f * 3.1416f / 180.0f) * dRollMatrix(15.0f * 3.1416f / 180.0f));
//			NewtonCollision* const collisionA = NewtonCreateBox (world, size.m_x, size.m_x * 0.25f, size.m_x * 0.25f, 0, &matrix[0][0]); 
//			NewtonCollision* const collisionB = NewtonCreateBox (world, size.m_x * 0.25f, size.m_x, size.m_x * 0.25f, 0, &matrix[0][0]); 
//			NewtonCollision* const collisionC = NewtonCreateBox (world, size.m_x * 0.25f, size.m_x * 0.25f, size.m_x, 0, &matrix[0][0]); 

matrix.m_posit = dVector (size.m_x * 0.5f, 0.0f, 0.0f, 1.0f);
NewtonCollision* const collisionA = NewtonCreateBox (world, size.m_x, size.m_x * 0.25f, size.m_x * 0.25f, 0, &matrix[0][0]); 
matrix.m_posit = dVector (0.0f, size.m_x * 0.5f, 0.0f, 1.0f);
NewtonCollision* const collisionB = NewtonCreateBox (world, size.m_x * 0.25f, size.m_x, size.m_x * 0.25f, 0, &matrix[0][0]); 
matrix.m_posit = dVector (0.0f, 0.0f, size.m_x * 0.5f, 1.0f);
NewtonCollision* const collisionC = NewtonCreateBox (world, size.m_x * 0.25f, size.m_x * 0.25f, size.m_x, 0, &matrix[0][0]); 


			collision = NewtonCreateCompoundCollision (world, 0);

			NewtonCompoundCollisionBeginAddRemove(collision);

			NewtonCompoundCollisionAddSubCollision (collision, collisionA);
			NewtonCompoundCollisionAddSubCollision (collision, collisionB);
			NewtonCompoundCollisionAddSubCollision (collision, collisionC);

			NewtonCompoundCollisionEndAddRemove(collision);	

			NewtonDestroyCollision(collisionA);
			NewtonDestroyCollision(collisionB);
			NewtonDestroyCollision(collisionC);
			break;
		}

		default: dAssert (0);
	}


	dMatrix matrix (srcMatrix);
	matrix.m_front = matrix.m_front.Scale (1.0f / dSqrt (matrix.m_front % matrix.m_front));
	matrix.m_right = matrix.m_front * matrix.m_up;
	matrix.m_right = matrix.m_right.Scale (1.0f / dSqrt (matrix.m_right % matrix.m_right));
	matrix.m_up = matrix.m_right * matrix.m_front;
	NewtonCollisionSetMatrix(collision, &matrix[0][0]);

	return collision;
}
Esempio n. 24
0
void dComplemtaritySolver::CalculateReactionsForces (int bodyCount, dBodyState** const bodyArray, int jointCount, dBilateralJoint** const jointArray, dFloat timestepSrc, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray)
{
	dJacobian stateVeloc[COMPLEMENTARITY_STACK_ENTRIES];
	dJacobian internalForces [COMPLEMENTARITY_STACK_ENTRIES];

	int stateIndex = 0;
	dVector zero(dFloat (0.0f), dFloat (0.0f), dFloat (0.0f), dFloat (0.0f));
	for (int i = 0; i < bodyCount; i ++) {
		dBodyState* const state = bodyArray[i];
		stateVeloc[stateIndex].m_linear = state->m_veloc;
		stateVeloc[stateIndex].m_angular = state->m_omega;

		internalForces[stateIndex].m_linear = zero;
		internalForces[stateIndex].m_angular = zero;

		state->m_myIndex = stateIndex;
		stateIndex ++;
		dAssert (stateIndex < int (sizeof (stateVeloc)/sizeof (stateVeloc[0])));
	}

	for (int i = 0; i < jointCount; i ++) {
		dJacobian y0;
		dJacobian y1;
		y0.m_linear = zero;
		y0.m_angular = zero;
		y1.m_linear = zero;
		y1.m_angular = zero;
		dBilateralJoint* const constraint = jointArray[i];
		int first = constraint->m_start;
		int count = constraint->m_count;
		for (int j = 0; j < count; j ++) { 
			dJacobianPair* const row = &jacobianArray[j + first];
			const dJacobianColum* const col = &jacobianColumnArray[j + first];
			dFloat val = col->m_force; 
			y0.m_linear += row->m_jacobian_IM0.m_linear.Scale(val);
			y0.m_angular += row->m_jacobian_IM0.m_angular.Scale(val);
			y1.m_linear += row->m_jacobian_IM1.m_linear.Scale(val);
			y1.m_angular += row->m_jacobian_IM1.m_angular.Scale(val);
		}
		int m0 = constraint->m_state0->m_myIndex;
		int m1 = constraint->m_state1->m_myIndex;
		internalForces[m0].m_linear += y0.m_linear;
		internalForces[m0].m_angular += y0.m_angular;
		internalForces[m1].m_linear += y1.m_linear;
		internalForces[m1].m_angular += y1.m_angular;
	}


	dFloat invTimestepSrc = dFloat (1.0f) / timestepSrc;
	dFloat invStep = dFloat (0.25f);
	dFloat timestep = timestepSrc * invStep;
	dFloat invTimestep = invTimestepSrc * dFloat (4.0f);

	int maxPasses = 5;
	dFloat firstPassCoef = dFloat (0.0f);
	dFloat maxAccNorm = dFloat (1.0e-2f);

	for (int step = 0; step < 4; step ++) {
		dJointAccelerationDecriptor joindDesc;
		joindDesc.m_timeStep = timestep;
		joindDesc.m_invTimeStep = invTimestep;
		joindDesc.m_firstPassCoefFlag = firstPassCoef;

		for (int i = 0; i < jointCount; i ++) {
			dBilateralJoint* const constraint = jointArray[i];
			joindDesc.m_rowsCount = constraint->m_count;
			joindDesc.m_rowMatrix = &jacobianArray[constraint->m_start];
			joindDesc.m_colMatrix = &jacobianColumnArray[constraint->m_start];
			constraint->JointAccelerations (&joindDesc);
		}
		firstPassCoef = dFloat (1.0f);

		dFloat accNorm = dFloat (1.0e10f);
		for (int passes = 0; (passes < maxPasses) && (accNorm > maxAccNorm); passes ++) {
			accNorm = dFloat (0.0f);
			for (int i = 0; i < jointCount; i ++) {

				dBilateralJoint* const constraint = jointArray[i];
				int index = constraint->m_start;
				int rowsCount = constraint->m_count;
				int m0 = constraint->m_state0->m_myIndex;
				int m1 = constraint->m_state1->m_myIndex;

				dVector linearM0 (internalForces[m0].m_linear);
				dVector angularM0 (internalForces[m0].m_angular);
				dVector linearM1 (internalForces[m1].m_linear);
				dVector angularM1 (internalForces[m1].m_angular);

				dBodyState* const state0 = constraint->m_state0;
				dBodyState* const state1 = constraint->m_state1;
				const dMatrix& invInertia0 = state0->m_invInertia;
				const dMatrix& invInertia1 = state1->m_invInertia;
				dFloat invMass0 = state0->m_invMass;
				dFloat invMass1 = state1->m_invMass;

				for (int k = 0; k < rowsCount; k ++) {
					dJacobianPair* const row = &jacobianArray[index];
					dJacobianColum* const col = &jacobianColumnArray[index];

					dVector JMinvIM0linear (row->m_jacobian_IM0.m_linear.Scale (invMass0));
					dVector JMinvIM1linear (row->m_jacobian_IM1.m_linear.Scale (invMass1));
					dVector JMinvIM0angular = invInertia0.UnrotateVector(row->m_jacobian_IM0.m_angular);
					dVector JMinvIM1angular = invInertia1.UnrotateVector(row->m_jacobian_IM1.m_angular);
					dVector acc (JMinvIM0linear.CompProduct(linearM0) + JMinvIM0angular.CompProduct(angularM0) + JMinvIM1linear.CompProduct(linearM1) + JMinvIM1angular.CompProduct(angularM1));

					dFloat a = col->m_coordenateAccel - acc.m_x - acc.m_y - acc.m_z - col->m_force * col->m_diagDamp;
					dFloat f = col->m_force + col->m_invDJMinvJt * a;

					dFloat lowerFrictionForce = col->m_jointLowFriction;
					dFloat upperFrictionForce = col->m_jointHighFriction;

					if (f > upperFrictionForce) {
						a = dFloat (0.0f);
						f = upperFrictionForce;
					} else if (f < lowerFrictionForce) {
						a = dFloat (0.0f);
						f = lowerFrictionForce;
					}

					accNorm = dMax (accNorm, dAbs (a));
					dFloat prevValue = f - col->m_force;
					col->m_force = f;

					linearM0 += row->m_jacobian_IM0.m_linear.Scale (prevValue);
					angularM0 += row->m_jacobian_IM0.m_angular.Scale (prevValue);
					linearM1 += row->m_jacobian_IM1.m_linear.Scale (prevValue);
					angularM1 += row->m_jacobian_IM1.m_angular.Scale (prevValue);
					index ++;
				}
				internalForces[m0].m_linear = linearM0;
				internalForces[m0].m_angular = angularM0;
				internalForces[m1].m_linear = linearM1;
				internalForces[m1].m_angular = angularM1;
			}
		}

		for (int i = 0; i < bodyCount; i ++) {
			dBodyState* const state = bodyArray[i];
			//int index = state->m_myIndex;
			dAssert (state->m_myIndex == i);
			dVector force (state->m_externalForce + internalForces[i].m_linear);
			dVector torque (state->m_externalTorque + internalForces[i].m_angular);
			state->IntegrateForce(timestep, force, torque);
		}
	}

	for (int i = 0; i < jointCount; i ++) {
		dBilateralJoint* const constraint = jointArray[i];
		int first = constraint->m_start;
		int count = constraint->m_count;
		for (int j = 0; j < count; j ++) { 
			const dJacobianColum* const col = &jacobianColumnArray[j + first];
			dFloat val = col->m_force; 
			constraint->m_jointFeebackForce[j] = val;
		}
	}

	for (int i = 0; i < jointCount; i ++) {
		dBilateralJoint* const constraint = jointArray[i];
		constraint->UpdateSolverForces (jacobianArray);
	}

	for (int i = 0; i < bodyCount; i ++) {
		dBodyState* const state = bodyArray[i];
		dAssert (state->m_myIndex == i);
		state->ApplyNetForceAndTorque (invTimestepSrc, stateVeloc[i].m_linear, stateVeloc[i].m_angular);
	}
}
/*
 * EKF Attitude Estimator main function.
 *
 * Estimates the attitude recursively once started.
 *
 * @param argc number of commandline arguments (plus command name)
 * @param argv strings containing the arguments
 */
int attitude_estimator_ekf_thread_main(int argc, char *argv[])
{

const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds

	float dt = 0.005f;
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
	float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f};					/**< Measurement vector */
	float x_aposteriori_k[12];		/**< states */
	float P_aposteriori_k[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
				     0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
				     0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,
				     0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,
				     0,   0,   0,   0,  100.f,  0,   0,   0,   0,   0,   0,   0,
				     0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,
				     0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,
				     0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,
				     0,   0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,
				     0,   0,   0,   0,   0,   0,   0,   0,  0.0f, 100.0f,   0,   0,
				     0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   100.0f,   0,
				     0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   0,   100.0f,
				    }; /**< init: diagonal matrix with big values */

	float x_aposteriori[12];
	float P_aposteriori[144];

	/* output euler angles */
	float euler[3] = {0.0f, 0.0f, 0.0f};

	float Rot_matrix[9] = {1.f,  0,  0,
			      0,  1.f,  0,
			      0,  0,  1.f
			     };		/**< init: identity matrix */

	// print text
	printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
	fflush(stdout);

	int overloadcounter = 19;

	/* Initialize filter */
	attitudeKalmanfilter_initialize();

	/* store start time to guard against too slow update rates */
	uint64_t last_run = hrt_absolute_time();

	struct sensor_combined_s raw;
	memset(&raw, 0, sizeof(raw));
	struct vehicle_gps_position_s gps;
	memset(&gps, 0, sizeof(gps));
	struct vehicle_global_position_s global_pos;
	memset(&global_pos, 0, sizeof(global_pos));
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	struct vehicle_control_mode_s control_mode;
	memset(&control_mode, 0, sizeof(control_mode));
    struct vehicle_vicon_position_s vicon_pos;  // Added by Ross Allen
    memset(&vicon_pos, 0, sizeof(vicon_pos));

	uint64_t last_data = 0;
	uint64_t last_measurement = 0;
	uint64_t last_vel_t = 0;
    
    /* Vicon parameters - Added by Ross Allen */
    bool vicon_valid = false;
    //~ static const hrt_abstime vicon_topic_timeout = 500000;		// vicon topic timeout = 0.25s

	/* current velocity */
	math::Vector<3> vel;
	vel.zero();
	/* previous velocity */
	math::Vector<3> vel_prev;
	vel_prev.zero();
	/* actual acceleration (by GPS velocity) in body frame */
	math::Vector<3> acc;
	acc.zero();
	/* rotation matrix */
	math::Matrix<3, 3> R;
	R.identity();

	/* subscribe to raw data */
	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
	/* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
	orb_set_interval(sub_raw, 3);

	/* subscribe to GPS */
	int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position));

	/* subscribe to GPS */
	int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position));

	/* subscribe to param changes */
	int sub_params = orb_subscribe(ORB_ID(parameter_update));

	/* subscribe to control mode*/
	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
    
    /* subscribe to vicon position */
    int vehicle_vicon_position_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); // Added by Ross Allen

	/* advertise attitude */
	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);

	int loopcounter = 0;

	thread_running = true;

	/* advertise debug value */
	// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
	// orb_advert_t pub_dbg = -1;

	/* keep track of sensor updates */
	uint64_t sensor_last_timestamp[3] = {0, 0, 0};

	struct attitude_estimator_ekf_params ekf_params;

	struct attitude_estimator_ekf_param_handles ekf_param_handles;

	/* initialize parameter handles */
	parameters_init(&ekf_param_handles);

	bool initialized = false;

	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };

	/* magnetic declination, in radians */
	float mag_decl = 0.0f;

	/* rotation matrix for magnetic declination */
	math::Matrix<3, 3> R_decl;
	R_decl.identity();

	/* register the perf counter */
	perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");

	/* Main loop*/
	while (!thread_should_exit) {

		struct pollfd fds[2];
		fds[0].fd = sub_raw;
		fds[0].events = POLLIN;
		fds[1].fd = sub_params;
		fds[1].events = POLLIN;
		int ret = poll(fds, 2, 1000);
        
        /* Added by Ross Allen */
        //~ hrt_abstime t = hrt_absolute_time();
        bool updated;

		if (ret < 0) {
			/* XXX this is seriously bad - should be an emergency */
		} else if (ret == 0) {
			/* check if we're in HIL - not getting sensor data is fine then */
			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);

			if (!control_mode.flag_system_hil_enabled) {
				fprintf(stderr,
					"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
			}

		} else {

			/* only update parameters if they changed */
			if (fds[1].revents & POLLIN) {
				/* read from param to clear updated flag */
				struct parameter_update_s update;
				orb_copy(ORB_ID(parameter_update), sub_params, &update);

				/* update parameters */
				parameters_update(&ekf_param_handles, &ekf_params);
			}

			/* only run filter if sensor values changed */
			if (fds[0].revents & POLLIN) {

				/* get latest measurements */
				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);

				bool gps_updated;
				orb_check(sub_gps, &gps_updated);
				if (gps_updated) {
					orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);

					if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
						mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));

						/* update mag declination rotation matrix */
						R_decl.from_euler(0.0f, 0.0f, mag_decl);
					}
				}

				bool global_pos_updated;
				orb_check(sub_global_pos, &global_pos_updated);
				if (global_pos_updated) {
					orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
				}

				if (!initialized) {
					// XXX disabling init for now
					initialized = true;

					// gyro_offsets[0] += raw.gyro_rad_s[0];
					// gyro_offsets[1] += raw.gyro_rad_s[1];
					// gyro_offsets[2] += raw.gyro_rad_s[2];
					// offset_count++;

					// if (hrt_absolute_time() - start_time > 3000000LL) {
					// 	initialized = true;
					// 	gyro_offsets[0] /= offset_count;
					// 	gyro_offsets[1] /= offset_count;
					// 	gyro_offsets[2] /= offset_count;
					// }

				} else {

					perf_begin(ekf_loop_perf);

					/* Calculate data time difference in seconds */
					dt = (raw.timestamp - last_measurement) / 1000000.0f;
					last_measurement = raw.timestamp;
					uint8_t update_vect[3] = {0, 0, 0};

					/* Fill in gyro measurements */
					if (sensor_last_timestamp[0] != raw.timestamp) {
						update_vect[0] = 1;
						// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
						sensor_last_timestamp[0] = raw.timestamp;
					}

					z_k[0] =  raw.gyro_rad_s[0] - gyro_offsets[0];
					z_k[1] =  raw.gyro_rad_s[1] - gyro_offsets[1];
					z_k[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];

					/* update accelerometer measurements */
					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
						update_vect[1] = 1;
						// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
					}

					hrt_abstime vel_t = 0;
					bool vel_valid = false;
					if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) {
						vel_valid = true;
						if (gps_updated) {
							vel_t = gps.timestamp_velocity;
							vel(0) = gps.vel_n_m_s;
							vel(1) = gps.vel_e_m_s;
							vel(2) = gps.vel_d_m_s;
						}

					} else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
						vel_valid = true;
						if (global_pos_updated) {
							vel_t = global_pos.timestamp;
							vel(0) = global_pos.vel_n;
							vel(1) = global_pos.vel_e;
							vel(2) = global_pos.vel_d;
						}
					}

					if (vel_valid) {
						/* velocity is valid */
						if (vel_t != 0) {
							/* velocity updated */
							if (last_vel_t != 0 && vel_t != last_vel_t) {
								float vel_dt = (vel_t - last_vel_t) / 1000000.0f;
								/* calculate acceleration in body frame */
								acc = R.transposed() * ((vel - vel_prev) / vel_dt);
							}
							last_vel_t = vel_t;
							vel_prev = vel;
						}

					} else {
						/* velocity is valid, reset acceleration */
						acc.zero();
						vel_prev.zero();
						last_vel_t = 0;
					}

					z_k[3] = raw.accelerometer_m_s2[0] - acc(0);
					z_k[4] = raw.accelerometer_m_s2[1] - acc(1);
					z_k[5] = raw.accelerometer_m_s2[2] - acc(2);

					/* update magnetometer measurements */
					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
						update_vect[2] = 1;
						// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
					}

					z_k[6] = raw.magnetometer_ga[0];
					z_k[7] = raw.magnetometer_ga[1];
					z_k[8] = raw.magnetometer_ga[2];

					uint64_t now = hrt_absolute_time();
					unsigned int time_elapsed = now - last_run;
					last_run = now;

					if (time_elapsed > loop_interval_alarm) {
						//TODO: add warning, cpu overload here
						// if (overloadcounter == 20) {
						// 	printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
						// 	overloadcounter = 0;
						// }

						overloadcounter++;
					}

					static bool const_initialized = false;

					/* initialize with good values once we have a reasonable dt estimate */
					if (!const_initialized && dt < 0.05f && dt > 0.001f) {
						dt = 0.005f;
						parameters_update(&ekf_param_handles, &ekf_params);

						/* update mag declination rotation matrix */
						if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
							mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));

						} else {
							mag_decl = ekf_params.mag_decl;
						}

						/* update mag declination rotation matrix */
						R_decl.from_euler(0.0f, 0.0f, mag_decl);

						x_aposteriori_k[0] = z_k[0];
						x_aposteriori_k[1] = z_k[1];
						x_aposteriori_k[2] = z_k[2];
						x_aposteriori_k[3] = 0.0f;
						x_aposteriori_k[4] = 0.0f;
						x_aposteriori_k[5] = 0.0f;
						x_aposteriori_k[6] = z_k[3];
						x_aposteriori_k[7] = z_k[4];
						x_aposteriori_k[8] = z_k[5];
						x_aposteriori_k[9] = z_k[6];
						x_aposteriori_k[10] = z_k[7];
						x_aposteriori_k[11] = z_k[8];

						const_initialized = true;
					}

					/* do not execute the filter if not initialized */
					if (!const_initialized) {
						continue;
					}

					attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
							     euler, Rot_matrix, x_aposteriori, P_aposteriori);

					/* swap values for next iteration, check for fatal inputs */
					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
						memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
						memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));

					} else {
						/* due to inputs or numerical failure the output is invalid, skip it */
						continue;
					}

					if (last_data > 0 && raw.timestamp - last_data > 30000)
						printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);

					last_data = raw.timestamp;
                    
                    /* Check Vicon for attitude data*/
                    /* Added by Ross Allen */
                    orb_check(vehicle_vicon_position_sub, &updated);

                    if (updated) {
                        orb_copy(ORB_ID(vehicle_vicon_position), vehicle_vicon_position_sub, &vicon_pos);
                        vicon_valid = vicon_pos.valid;
                    }
                    
                    //~ if (vicon_valid && (t > (vicon_pos.timestamp + vicon_topic_timeout))) {
                        //~ vicon_valid = false;
                        //~ warnx("VICON timeout");
                    //~ }

					/* send out */
					att.timestamp = raw.timestamp;

					att.roll = euler[0];
					att.pitch = euler[1];
					att.yaw = euler[2] + mag_decl;
                    
                    /* Use vicon yaw if valid and just overwrite*/
                    /* Added by Ross Allen */
                    math::Matrix<3,3> R_magCor = R_decl;	// correction for magnetic declination
                    if(vicon_valid){
                        att.yaw = vicon_pos.yaw;
                        R_magCor.identity(); 		// magnetometer not used, so no correction
                    }

					att.rollspeed = x_aposteriori[0];
					att.pitchspeed = x_aposteriori[1];
					att.yawspeed = x_aposteriori[2];
					att.rollacc = x_aposteriori[3];
					att.pitchacc = x_aposteriori[4];
					att.yawacc = x_aposteriori[5];

					att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
					att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
					att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);

					/* copy offsets */
					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));

					/* magnetic declination */

					math::Matrix<3, 3> R_body = (&Rot_matrix[0]);
					R = R_magCor * R_body;

					/* copy rotation matrix */
					memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
					att.R_valid = true;

					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
						// Broadcast
						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);

					} else {
						warnx("NaN in roll/pitch/yaw estimate!");
					}

					perf_end(ekf_loop_perf);
				}
			}
		}

		loopcounter++;
	}

	thread_running = false;

	return 0;
}
Esempio n. 26
0
jobject CRJNIEnv::toJavaTOCItem( LVTocItem * toc )
{
	TOCItemAccessor acc(*this);
	return acc.toJava( toc );
}
Esempio n. 27
0
int
expr_evaluate(double *pResult, struct expr_s *pExpr, env_f pEnv)
{
	/* Evitons quelques douloureux passages de parametres tres repetitifs
	 * en utilisant quelques fonctions imbriquees. On economisera d'autant
	 * plus d'espace sur la pile. Et c'est bon dans la mesure ou
	 * l'interpretation des expression est recursive */

	int __get_str(struct expr_s *pE, char **ppS)
	{

		if (!ppS || !pE)
			return EXPR_EVAL_ERROR;

		*ppS = NULL;
		CHK_TYPE(pE->type, return EXPR_EVAL_ERROR);

		switch (pE->type) {
		case VAL_STR_ET:
			*ppS = strdup(pE->expr.str);
			return EXPR_EVAL_DEF;
		case VAL_NUM_ET:
		case UN_NUMSUP_ET:
		case UN_NUMINF_ET:
		case UN_NUMNOT_ET:
		case UN_STRNUM_ET:
		case UN_STRLEN_ET:
		case BIN_STRCMP_ET:
		case BIN_NUMCMP_ET:
		case BIN_NUMEQ_ET:
		case BIN_NUMNEQ_ET:
		case BIN_NUMLT_ET:
		case BIN_NUMLE_ET:
		case BIN_NUMGT_ET:
		case BIN_NUMGE_ET:
		case BIN_NUMADD_ET:
		case BIN_NUMSUB_ET:
		case BIN_NUMMUL_ET:
		case BIN_NUMDIV_ET:
		case BIN_NUMMOD_ET:
		case BIN_NUMAND_ET:
		case BIN_NUMXOR_ET:
		case BIN_NUMOR_ET:
		case BIN_ROOT_ET:
		case NB_ET:
			return EXPR_EVAL_UNDEF;
		case ACC_ET:{
				accessor_f *acc = NULL;

				if (!pE->expr.acc.base)
					return EXPR_EVAL_ERROR;
				if (!pE->expr.acc.field)
					return EXPR_EVAL_ERROR;
				acc = pEnv(pE->expr.acc.base);
				if (!acc)
					return EXPR_EVAL_UNDEF;
				*ppS = acc(pE->expr.acc.field);
				if (!(*ppS))
					return EXPR_EVAL_UNDEF;
				return EXPR_EVAL_DEF;
			}
		}

		return EXPR_EVAL_ERROR;
	}

	int __main_eval(struct expr_s *pE, double *pD)
	{
		int ret;

		if (!pE || !pD || !pEnv) {
			return EXPR_EVAL_ERROR;
		}

		CHK_TYPE(pE->type, return EXPR_EVAL_ERROR);

		switch (pE->type) {
		case ACC_ET:
		case VAL_STR_ET:
			{
				char *s;

				ret = __get_str(pE, &s);
				if (ret != EXPR_EVAL_DEF)
					return ret;
				*pD = strlen(s);
				free(s);
				return EXPR_EVAL_DEF;
			}

		case VAL_NUM_ET:
			*pD = pE->expr.num;
			return EXPR_EVAL_DEF;

		case UN_NUMSUP_ET:
			ret = __main_eval(pE->expr.unary, pD);
			if (ret != EXPR_EVAL_DEF)
				return ret;
			*pD = ceil(*pD);
			return EXPR_EVAL_DEF;

		case UN_NUMINF_ET:
			ret = __main_eval(pE->expr.unary, pD);
			if (ret != EXPR_EVAL_DEF)
				return ret;
			*pD = floor(*pD);
			return EXPR_EVAL_DEF;

		case UN_NUMNOT_ET:
			ret = __main_eval(pE->expr.unary, pD);
			if (ret != EXPR_EVAL_DEF)
				return ret;
			*pD = ((int) *pD) ? 0 : 1;
			return EXPR_EVAL_DEF;

		case UN_STRNUM_ET:{
				char *pEnd = NULL;
				char *ppS = NULL;
				struct expr_s *pUnary = pE->expr.unary;

				if (!pUnary)
					return EXPR_EVAL_ERROR;
				if (pUnary->type == VAL_NUM_ET) {
					TRACE("num arg is type VAL_NUM_ET");
					*pD = pUnary->expr.num;
				}
				else if (pUnary->type == VAL_STR_ET) {
					TRACE("num arg is type VAL_STR_ET");
					*pD = strtod(pUnary->expr.str, &pEnd);
					if (pEnd == pUnary->expr.str)
						return EXPR_EVAL_UNDEF;
				}
				else if (pUnary->type == ACC_ET) {
					TRACE("num arg is type ACC_ET");
					accessor_f *acc = NULL;

					if (!pUnary->expr.acc.base)
						return EXPR_EVAL_ERROR;
					if (!pUnary->expr.acc.field)
						return EXPR_EVAL_ERROR;
					acc = pEnv(pUnary->expr.acc.base);
					if (!acc)
						return EXPR_EVAL_UNDEF;
					ppS = acc(pUnary->expr.acc.field);
					if (!ppS)
						return EXPR_EVAL_UNDEF;
					*pD = strtod(ppS, &pEnd);
					if (pEnd == ppS)
						return EXPR_EVAL_UNDEF;
					free(ppS);
					return EXPR_EVAL_DEF;
				}
				else {
					TRACE("num arg is type expr");
					return __main_eval(pUnary, pD);
				}

				/*TODO test if there remains some characters */
				return EXPR_EVAL_DEF;
			}

		case UN_STRLEN_ET:{
				char *s = NULL;

				ret = __get_str(pE->expr.unary, &s);
				if (ret != EXPR_EVAL_DEF)
					return ret;
				*pD = strlen(s);
				free(s);
				return EXPR_EVAL_DEF;
			}

		case BIN_STRCMP_ET:{
				char *s1 = NULL, *s2 = NULL;

				if (!pE->expr.bin.p1)
					return EXPR_EVAL_ERROR;
				if (!pE->expr.bin.p2)
					return EXPR_EVAL_ERROR;
				ret = __get_str(pE->expr.bin.p1, &s1);
				if (ret != EXPR_EVAL_DEF)
					return ret;
				ret = __get_str(pE->expr.bin.p2, &s2);
				if (ret != EXPR_EVAL_DEF) {
					free(s1);
					return ret;
				}
				*pD = (strcmp(s1, s2) == 0);
				free(s1);
				if (s2 != s1)
					free(s2);
				return EXPR_EVAL_DEF;
			}

		case BIN_NUMCMP_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				FPcmp(*pD, d1, d2);
				return EXPR_EVAL_DEF;
			}

		case BIN_NUMEQ_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				FPcmp(ret, d1, d2);
				*pD = (ret == 0);
				return EXPR_EVAL_DEF;
			}

		case BIN_NUMNEQ_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				FPcmp(ret, d1, d2);
				*pD = (ret != 0);
				return EXPR_EVAL_DEF;
			}

		case BIN_NUMLT_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				FPcmp(ret, d1, d2);
				*pD = (ret < 0);
				return EXPR_EVAL_DEF;
			}

		case BIN_NUMLE_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				FPcmp(ret, d1, d2);
				*pD = (ret <= 0);
				return EXPR_EVAL_DEF;
			}

		case BIN_NUMGT_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				FPcmp(ret, d1, d2);
				*pD = (ret > 0);
				return EXPR_EVAL_DEF;
			}

		case BIN_NUMGE_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				FPcmp(ret, d1, d2);
				*pD = (ret >= 0);
				return EXPR_EVAL_DEF;
			}

		case BIN_NUMADD_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				*pD = d1 + d2;
				return EXPR_EVAL_DEF;
			}

		case BIN_NUMSUB_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				*pD = d1 - d2;
				return EXPR_EVAL_DEF;
			}

		case BIN_NUMMUL_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				*pD = d1 * d2;
				return EXPR_EVAL_DEF;
			}

		case BIN_NUMDIV_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				FPcmp(ret, d2, 0);
				if (ret == 0)
					return EXPR_EVAL_DEF;
				*pD = d1 / d2;
				return EXPR_EVAL_DEF;
			}

		case BIN_NUMMOD_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				FPcmp(ret, d1, 0);
				if (ret < 0)
					return EXPR_EVAL_DEF;
				FPcmp(ret, d2, 0);
				if (ret < 0)
					return EXPR_EVAL_DEF;
				*pD = (double) ((int) d1 % (int) d2);
				return EXPR_EVAL_DEF;
			}

		case BIN_NUMAND_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				*pD = FPBOOL(d1) && FPBOOL(d2);
				return EXPR_EVAL_DEF;
			}

		case BIN_NUMXOR_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				*pD = (int) d1 ^ (int) d2;
				return EXPR_EVAL_DEF;
			}

		case BIN_NUMOR_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				*pD = d1 + d2;
				return EXPR_EVAL_DEF;
			}

		case BIN_ROOT_ET:{
				double d1 = 0, d2 = 0;

				EVAL_BINNUM(d1, d2, pE);
				TRACE("root with args [%f]/[%f]", d1, d2);
				FPcmp(ret, d1, 0);
				if (ret == 0)
					return EXPR_EVAL_UNDEF;
				FPcmp(ret, d2, 0);
				if (ret == 0) {
					*pD = 0.0;
					return EXPR_EVAL_DEF;
				}
				*pD = pow(d2, 1 / d1);
				TRACE("root with args [%f]/[%f] return result [%f]", d1, d2, *pD);
				return EXPR_EVAL_DEF;
			}
		case NB_ET:
			break;
		}

		return EXPR_EVAL_UNDEF;
	}
Esempio n. 28
0
void ConcreteGridView<DuneGridView>::getRawElementDataImpl(
    arma::Mat<CoordinateType> &vertices, arma::Mat<int> &elementCorners,
    arma::Mat<char> &auxData, std::vector<int> *domainIndices) const {
  typedef typename DuneGridView::Grid DuneGrid;
  typedef typename DuneGridView::IndexSet DuneIndexSet;
  const int dimGrid = DuneGrid::dimension;
  const int dimWorld = DuneGrid::dimensionworld;
  const int codimVertex = dimGrid;
  const int codimElement = 0;
  typedef Dune::LeafMultipleCodimMultipleGeomTypeMapper<
      DuneGrid, Dune::MCMGElementLayout> DuneElementMapper;
  typedef typename DuneGridView::template Codim<codimVertex>::Iterator
  DuneVertexIterator;
  typedef typename DuneGridView::template Codim<codimElement>::Iterator
  DuneElementIterator;
  typedef typename DuneGridView::template Codim<codimVertex>::Geometry
  DuneVertexGeometry;
  typedef typename DuneGridView::template Codim<codimElement>::Geometry
  DuneElementGeometry;
  typedef typename DuneGrid::ctype ctype;

  const DuneIndexSet &indexSet = m_dune_gv.indexSet();

  vertices.set_size(dimWorld, indexSet.size(codimVertex));
  for (DuneVertexIterator it = m_dune_gv.template begin<codimVertex>();
       it != m_dune_gv.template end<codimVertex>(); ++it) {
    size_t index = indexSet.index(*it);
    const DuneVertexGeometry &geom = it->geometry();
    Dune::FieldVector<ctype, dimWorld> vertex = geom.corner(0);
    for (int i = 0; i < dimWorld; ++i)
      vertices(i, index) = vertex[i];
  }

  const int MAX_CORNER_COUNT = dimWorld == 2 ? 2 : 4;
  DuneElementMapper elementMapper(m_dune_gv.grid());
  const int elementCount = elementMapper.size();
  elementCorners.set_size(MAX_CORNER_COUNT, elementCount);
  for (DuneElementIterator it = m_dune_gv.template begin<codimElement>();
       it != m_dune_gv.template end<codimElement>(); ++it) {
    size_t index = indexSet.index(*it);
    const Dune::GenericReferenceElement<ctype, dimGrid> &refElement =
        Dune::GenericReferenceElements<ctype, dimGrid>::general(it->type());
    const int cornerCount = refElement.size(codimVertex);
    assert(cornerCount <= MAX_CORNER_COUNT);
    for (int i = 0; i < cornerCount; ++i)
      elementCorners(i, index) = indexSet.subIndex(*it, i, codimVertex);
    for (int i = cornerCount; i < MAX_CORNER_COUNT; ++i)
      elementCorners(i, index) = -1;
  }

  auxData.set_size(0, elementCorners.n_cols);

  if (domainIndices) {
    // Somewhat inelegant: we perform a second iteration over elements,
    // this time using the BEM++ interface to Dune.
    domainIndices->resize(elementCount);
    std::unique_ptr<EntityIterator<0>> it = this->entityIterator<0>();
    const IndexSet &indexSet = this->indexSet();
    while (!it->finished()) {
      const Entity<0> &entity = it->entity();
      const int index = indexSet.entityIndex(entity);
      const int domain = entity.domain();
      acc(*domainIndices, index) = domain;
      it->next();
    }
  }
}
Esempio n. 29
0
void
Image::copyUnProcessedChannelsForPremult(const std::bitset<4> processChannels,
                                         const RectI& roi,
                                         const ImagePtr& originalImage)
{
    Q_UNUSED(processChannels); // silence warnings in release version
    assert( ( (doR == !processChannels[0]) || !(dstNComps >= 2) ) &&
            ( (doG == !processChannels[1]) || !(dstNComps >= 2) ) &&
            ( (doB == !processChannels[2]) || !(dstNComps >= 3) ) &&
            ( (doA == !processChannels[3]) || !(dstNComps == 1 || dstNComps == 4) ) );
    ReadAccess acc( originalImage.get() );
    int dstRowElements = dstNComps * _bounds.width();
    PIX* dst_pixels = (PIX*)pixelAt(roi.x1, roi.y1);
    assert(dst_pixels);
    assert(srcNComps == 1 || srcNComps == 4 || !originalPremult); // only A or RGBA can be premult
    assert(dstNComps == 1 || dstNComps == 4 || !premult); // only A or RGBA can be premult

    for ( int y = roi.y1; y < roi.y2; ++y, dst_pixels += (dstRowElements - (roi.x2 - roi.x1) * dstNComps) ) {
        for (int x = roi.x1; x < roi.x2; ++x, dst_pixels += dstNComps) {
            const PIX* src_pixels = originalImage ? (const PIX*)acc.pixelAt(x, y) : 0;
            PIX srcA = src_pixels ? maxValue : 0; /* be opaque for anything that doesn't contain alpha */
            if ( ( (srcNComps == 1) || (srcNComps == 4) ) && src_pixels ) {
#             ifdef DEBUG
                assert(src_pixels[srcNComps - 1] == src_pixels[srcNComps - 1]); // check for NaN
#             endif
                srcA = src_pixels[srcNComps - 1];
            }

#        ifdef NATRON_COPY_CHANNELS_UNPREMULT
            // Repremult R G and B if output is premult and alpha was modified.
            // We do not consider it a good thing, since the user explicitely deselected the channels, and expects
            // to get the values from input instead.
#           define DOCHANNEL(c)                                                    \
    if (srcNComps == 1 || !src_pixels || c >= srcNComps) {      \
        dst_pixels[c] = 0;                                      \
    } \
    else if (originalPremult) {                               \
        if (srcA == 0) {                                        \
            dst_pixels[c] = src_pixels[c];         /* don't try to unpremult, just copy */ \
        } \
        else if (premult) {                                   \
            if (doA) {                                          \
                dst_pixels[c] = src_pixels[c];         /* dst will have same alpha as src, just copy src */ \
            } \
            else {                                            \
                dst_pixels[c] = (src_pixels[c] / (float)srcA) * dstAorig;         /* dst keeps its alpha, unpremult src and repremult */ \
            }                                                   \
        } \
        else {                                                \
            dst_pixels[c] = (src_pixels[c] / (float)srcA) * maxValue;         /* dst is not premultiplied, unpremult src */ \
        }                                                       \
    } \
    else {                                                    \
        if (premult) {                                          \
            if (doA) {                                          \
                dst_pixels[c] = (src_pixels[c] / (float)maxValue) * srcA;         /* dst will have same alpha as src, just premult src with its alpha */ \
            } \
            else {                                            \
                dst_pixels[c] = (src_pixels[c] / (float)maxValue) * dstAorig;         /* dst keeps its alpha, premult src with dst's alpha */ \
            }                                                   \
        } \
        else {                                                \
            dst_pixels[c] = src_pixels[c];         /* neither src nor dst is not premultiplied */ \
        }                                                       \
    }

            PIX dstAorig = maxValue;
#         else // !NATRON_COPY_CHANNELS_UNPREMULT
               // Just copy the channels, after all if the user unchecked a channel,
               // we do not want to change the values behind his back.
               // Rather we display a warning in  the GUI.
#           define DOCHANNEL(c) dst_pixels[c] = (!src_pixels || c >= srcNComps) ? 0 : src_pixels[c];
#         endif // !NATRON_COPY_CHANNELS_UNPREMULT

            if ( (dstNComps == 1) || (dstNComps == 4) ) {
#             ifdef DEBUG
                assert(dst_pixels[dstNComps - 1] == dst_pixels[dstNComps - 1]); // check for NaN
#             endif
#             ifdef NATRON_COPY_CHANNELS_UNPREMULT
                dstAorig = dst_pixels[dstNComps - 1];
#             endif // NATRON_COPY_CHANNELS_UNPREMULT
            }
            if (doR) {
#             ifdef DEBUG
                assert(!src_pixels || src_pixels[0] == src_pixels[0]); // check for NaN
                assert(dst_pixels[0] == dst_pixels[0]); // check for NaN
#             endif
                DOCHANNEL(0);
#             ifdef DEBUG
                assert(dst_pixels[0] == dst_pixels[0]); // check for NaN
#             endif
            }
            if (doG) {
#             ifdef DEBUG
                assert(!src_pixels || src_pixels[1] == src_pixels[1]); // check for NaN
                assert(dst_pixels[1] == dst_pixels[1]); // check for NaN
#             endif
                DOCHANNEL(1);
#             ifdef DEBUG
                assert(dst_pixels[1] == dst_pixels[1]); // check for NaN
#             endif
            }
            if (doB) {
#             ifdef DEBUG
                assert(!src_pixels || src_pixels[2] == src_pixels[2]); // check for NaN
                assert(dst_pixels[2] == dst_pixels[2]); // check for NaN
#             endif
                DOCHANNEL(2);
#             ifdef DEBUG
                assert(dst_pixels[2] == dst_pixels[2]); // check for NaN
#             endif
            }
            if (doA) {
#             ifdef NATRON_COPY_CHANNELS_UNPREMULT
                if (premult) {
                    if (dstAorig != 0) {
                        // unpremult, then premult
                        if ( (dstNComps >= 2) && !doR ) {
                            dst_pixels[0] = (dst_pixels[0] / (float)dstAorig) * srcA;
#                         ifdef DEBUG
                            assert(dst_pixels[0] == dst_pixels[0]); // check for NaN
#                         endif
                        }
                        if ( (dstNComps >= 2) && !doG ) {
                            dst_pixels[1] = (dst_pixels[1] / (float)dstAorig) * srcA;
#                         ifdef DEBUG
                            assert(dst_pixels[1] == dst_pixels[1]); // check for NaN
#                         endif
                        }
                        if ( (dstNComps >= 2) && !doB ) {
                            dst_pixels[2] = (dst_pixels[2] / (float)dstAorig) * srcA;
#                         ifdef DEBUG
                            assert(dst_pixels[2] == dst_pixels[2]); // check for NaN
#                         endif
                        }
                    }
                }
#             endif // NATRON_COPY_CHANNELS_UNPREMULT
                if ( (dstNComps == 1) || (dstNComps == 4) ) {
                    dst_pixels[dstNComps - 1] = srcA;
#                 ifdef DEBUG
                    assert(dst_pixels[dstNComps - 1] == dst_pixels[dstNComps - 1]); // check for NaN
#                 endif
                }
            }
        }
    }
} // Image::copyUnProcessedChannelsForPremult
Esempio n. 30
0
void inv(A&& a, C&& c) {
    // The inverse of a permutation matrix is its transpose
    if (is_permutation_matrix(a)) {
        c = transpose(a);
        return;
    }

    const auto n = etl::dim<0>(a);

    // Use forward propagation for lower triangular matrix
    if (is_lower_triangular(a)) {
        c = 0;

        // The column in c
        for (size_t s = 0; s < n; ++s) {
            // The row in a
            for (size_t row = 0; row < n; ++row) {
                auto b = row == s ? 1.0 : 0.0;

                if (row == 0) {
                    c(0, s) = b / a(0, 0);
                } else {
                    value_t<A> acc(0);

                    // The column in a
                    for (size_t col = 0; col < row; ++col) {
                        acc += a(row, col) * c(col, s);
                    }

                    c(row, s) = (b - acc) / a(row, row);
                }
            }
        }

        return;
    }

    // Use backward propagation for upper triangular matrix
    if (is_upper_triangular(a)) {
        c = 0;

        // The column in c
        for (int64_t s = n - 1; s >= 0; --s) {
            // The row in a
            for (int64_t row = n - 1; row >= 0; --row) {
                auto b = row == s ? 1.0 : 0.0;

                if (row == int64_t(n) - 1) {
                    c(row, s) = b / a(row, row);
                } else {
                    value_t<A> acc(0);

                    // The column in a
                    for (int64_t col = n - 1; col > row; --col) {
                        acc += a(row, col) * c(col, s);
                    }

                    c(row, s) = (b - acc) / a(row, row);
                }
            }
        }

        return;
    }

    auto L = force_temporary_dim_only(a);
    auto U = force_temporary_dim_only(a);
    auto P = force_temporary_dim_only(a);

    etl::lu(a, L, U, P);

    c = inv(U) * inv(L) * P;
}