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);
}
}
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;
}
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;
}
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(¢[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(¢[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(¢[i*k], k, n[i]);
memcpy(&cb[i*k], ¢[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;
}
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);
}
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);
}
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);
}
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;
}
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);
}
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");
}
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);
}
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;
}
result_type operator()( individual_type & ind ) {
individual_trait_acc_type acc;
result_type res = acc( ind );
return res;
}
ucac4Region_t *UCAC4::getRegion(int region)
{
if (m_regionMap.contains(region))
{
return &m_regionMap[region];
}
ucac4Region_t regionData;
ucac4Region_t *regionPtr = ®ionData;
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++;
}
}
}
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]);
*/
}
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);
}
}
}
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();
}
}
}
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;
}
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;
}
jobject CRJNIEnv::toJavaTOCItem( LVTocItem * toc )
{
TOCItemAccessor acc(*this);
return acc.toJava( toc );
}
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;
}
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();
}
}
}
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
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;
}