void LaserScanMatcher::PointCloudToLDP(const PointCloudT::ConstPtr& cloud,
LDP& ldp)
{
double max_d2 = cloud_res_ * cloud_res_;
PointCloudT cloud_f;
cloud_f.points.push_back(cloud->points[0]);
for (unsigned int i = 1; i < cloud->points.size(); ++i)
{
const PointT& pa = cloud_f.points[cloud_f.points.size() - 1];
const PointT& pb = cloud->points[i];
double dx = pa.x - pb.x;
double dy = pa.y - pb.y;
double d2 = dx*dx + dy*dy;
if (d2 > max_d2)
{
cloud_f.points.push_back(pb);
}
}
unsigned int n = cloud_f.points.size();
ldp = ld_alloc_new(n);
for (unsigned int i = 0; i < n; i++)
{
// calculate position in laser frame
if (is_nan(cloud_f.points[i].x) || is_nan(cloud_f.points[i].y))
{
ROS_WARN("Laser Scan Matcher: Cloud input contains NaN values. \
Please use a filtered cloud input.");
}
else
{
static inline A0 log10(const A0& a0)
{
A0 x, fe, x2, y;
kernel_log(a0, fe, x, x2, y);
y = amul(y, -Half<A0>(), x2);
// multiply log of fraction by log10(e) and base 2 exponent by log10(2)
A0 z = mul(x+y, single_constant<A0, 0x3a37b152>());//7.00731903251827651129E-4f // log10(e)lo
z = amul(z, y, single_constant<A0, 0x3ede0000>()); //4.3359375E-1f // log10(e)hi
z = amul(z, x, single_constant<A0, 0x3ede0000>());
z = amul(z, fe, single_constant<A0, 0x39826a14>());//3.0078125E-1f // log10(2)hi
z = amul(z, fe, single_constant<A0, 0x3e9a0000>());//2.48745663981195213739E-4f // log10(2)lo
A0 y1 = a0-rec(abs(a0)); // trick to reduce selection testing
return seladd(is_inf(y1), b_or(z, b_or(is_ltz(a0), is_nan(a0))),y1);
}
int_adapter operator-(const int_type rhs) const
{
if(is_special())
{
if (is_nan())
{
return int_adapter<int_type>(not_a_number());
}
if (is_infinity())
{
return *this;
}
}
return int_adapter<int_type>(value_ - rhs);
}
void purify(LDP ld, double threshold_min, double threshold_max) {
for(int i=0;i<ld->nrays;i++) {
if(!ld->valid[i]) continue;
double rho = ld->readings[i];
if( is_nan(rho) | (rho < threshold_min) | (rho > threshold_max) ) {
ld->readings[i] = GSL_NAN;
ld->valid[i] = 0;
ld->alpha[i] = GSL_NAN;
ld->alpha_valid[i] = 0;
}
}
}
void menu_floatinput_set(struct menu_item *item, float value)
{
if (is_nan(value) || !isnormal(value))
value = 0.f;
struct item_data *data = item->data;
data->value = value;
int sig_sign = signbit(value) ? -1 : 1;
value = fabsf(value);
int exp = value == 0.f ? 0.f : floorf(log10f(value));
int sig = value / pow(10., exp - (data->sig_precis - 1)) + 0.5;
int exp_sign = exp < 0 ? -1 : 1;
exp *= exp_sign;
char *p = data->item->text;
if (sig_sign == 1)
data->item->text[0] = data->sig_sign->text[0] = '+';
else {
data->item->text[0] = data->sig_sign->text[0] = '-';
*p++ = '-';
}
for (int i = data->sig_precis - 1; i >= 0; --i) {
int x = i;
if (i > 0)
++x;
int c = int_to_char(sig % 10);
sig /= 10;
data->sig_digits[i]->text[0] = c;
p[x] = c;
}
p[1] = '.';
p += data->sig_precis + 1;
*p++ = 'e';
if (exp_sign == 1)
data->item->text[3 + data->sig_precis] = data->exp_sign->text[0] = '+';
else {
data->item->text[3 + data->sig_precis] = data->exp_sign->text[0] = '-';
*p++ = '-';
}
for (int i = data->exp_precis - 1; i >= 0; --i) {
int c = int_to_char(exp % 10);
exp /= 10;
data->exp_digits[i]->text[0] = c;
p[i] = c;
}
p[data->exp_precis] = 0;
}
// compute exp(ax)
static inline A0 expa(const A0& a0)
{
A0 hi, lo, x;
A0 k = reduc_t::reduce(a0, hi, lo, x);
A0 c = reduc_t::approx(x);
bA0 ge = reduc_t::isgemaxlog(a0);
bA0 le = reduc_t::isleminlog(a0);
A0 z = nt2::if_zero_else(le,
finalize_t::finalize(a0, x, c, k, hi, lo)
);
#ifdef BOOST_SIMD_NO_INVALIDS
return z;
#else
return if_nan_else(is_nan(a0), nt2::if_else(ge, nt2::Inf<A0>(), z));
#endif
}
template <typename T> void Support<T>::print_imaginary( T i, char c, std::ostream & out ) {
if ( is_pos_zero( i ) ) {
out << " + 0" << c;
} else if ( is_neg_zero( i ) ) {
out << " - 0" << c;
} else if ( is_nan( i ) ) {
out << " + NaN" << c;
} else if ( i == 1.0 ) {
out << " + " << c;
} else if ( i == -1.0 ) {
out << " - " << c;
} else if ( i > 0 ) {
out << " + " << i << c;
} else {
out << " - " << (-i) << c;
}
}
float deemphasis_wfm_ff (float* input, float* output, int input_size, float tau, int sample_rate, float last_output)
{
/*
typical time constant (tau) values:
WFM transmission in USA: 75 us -> tau = 75e-6
WFM transmission in EU: 50 us -> tau = 50e-6
More info at: http://www.cliftonlaboratories.com/fm_receivers_and_de-emphasis.htm
Simulate in octave: tau=75e-6; dt=1/48000; alpha = dt/(tau+dt); freqz([alpha],[1 -(1-alpha)])
*/
float dt = 1.0/sample_rate;
float alpha = dt/(tau+dt);
if(is_nan(last_output)) last_output=0.0; //if last_output is NaN
output[0]=alpha*input[0]+(1-alpha)*last_output;
for (int i=1;i<input_size;i++) //@deemphasis_wfm_ff
output[i]=alpha*input[i]+(1-alpha)*output[i-1]; //this is the simplest IIR LPF
return output[input_size-1];
}
static inline A0 log(const A0& a0)
{
A0 x, fe, x2, y;
kernel_log(a0, fe, x, x2, y);
y = madd(fe, single_constant<A0, 0xb95e8083>(), y);
y = madd(Mhalf<A0>(), x2, y);
A0 z = x + y;
// std::cout << "fe " << fe << std::endl;
// std::cout << "z " << z << std::endl;
// std::cout << "a0 " << a0 << std::endl;
// std::cout << "rec(a0) " << rec(a0) << std::endl;
A0 y1 = a0-rec(abs(a0));// trick to reduce selection testing
A0 y2 = madd(single_constant<A0, 0x3f318000>(), fe, z);
// std::cout << "y1 " << y1 << std::endl;
// std::cout << "y2 " << y2 << std::endl;
return seladd(is_inf(y1),b_or(y2, b_or(is_ltz(a0), is_nan(a0))),y1);
}
/** Scales this vector to make it a unit vector (within rounding error).
*
* The current version tries to handle infinite coordinates gracefully,
* but it's not clear that any callers need that.
*
* \pre \f$this \neq (0, 0)\f$
* \pre Neither component is NaN.
* \post \f$-\epsilon<\left|this\right|-1<\epsilon\f$
*/
void Point::normalize() {
double len = hypot(_pt[0], _pt[1]);
if(len == 0) return;
if(is_nan(len)) return;
static double const inf = std::numeric_limits<double>::infinity();
if(len != inf) {
*this /= len;
} else {
unsigned n_inf_coords = 0;
/* Delay updating pt in case neither coord is infinite. */
Point tmp;
for ( unsigned i = 0 ; i < 2 ; ++i ) {
if ( _pt[i] == inf ) {
++n_inf_coords;
tmp[i] = 1.0;
} else if ( _pt[i] == -inf ) {
++n_inf_coords;
tmp[i] = -1.0;
} else {
tmp[i] = 0.0;
}
}
switch (n_inf_coords) {
case 0: {
/* Can happen if both coords are near +/-DBL_MAX. */
*this /= 4.0;
len = hypot(_pt[0], _pt[1]);
assert(len != inf);
*this /= len;
break;
}
case 1: {
*this = tmp;
break;
}
case 2: {
*this = tmp * sqrt(0.5);
break;
}
}
}
}
// Floating Multiply-Add Single
static void
fmadds(ThreadState *state, Instruction instr)
{
double a, b, c, d;
a = state->fpr[instr.frA].paired0;
b = state->fpr[instr.frB].paired0;
c = state->fpr[instr.frC].paired0;
state->fpscr.vxsnan = is_signalling_nan(a) || is_signalling_nan(b) || is_signalling_nan(c);
state->fpscr.vxisi = is_infinity(a * c) || is_infinity(c);
state->fpscr.vximz = is_infinity(a * c) && is_zero(c);
d = a * c;
if (is_nan(d)) {
if (is_nan(a)) {
d = make_quiet(a);
} else if (is_nan(b)) {
d = make_quiet(b);
} else if (is_nan(c)) {
d = make_quiet(c);
} else {
d = make_nan<double>();
}
} else {
if (is_infinity(d) && is_infinity(b) && !(is_infinity(a) || is_infinity(c))) {
d = b;
} else {
d = d + b;
if (is_nan(d)) {
if (is_nan(b)) {
d = make_quiet(b);
} else {
d = make_nan<double>();
}
}
}
}
updateFPSCR(state);
updateFPRF(state, d);
state->fpr[instr.frD].paired0 = static_cast<float>(d);
if (instr.rc) {
updateFloatConditionRegister(state);
}
}
static void
check_fpe_traps(void)
{
int traps = 0;
if (setjmp(jbuff) == 0) {
div_by(44.0, 0.0);
message("division by zero does not generate an exception");
} else {
traps = 1;
message("division by zero generates an exception");
catch_FPEs(); /* set again if sysV */
}
if (setjmp(jbuff) == 0) {
overflow(1000.0);
message("overflow does not generate an exception");
} else {
traps |= 2;
message("overflow generates an exception");
catch_FPEs();
}
if (traps == 0) {
double maybe_nan;
maybe_nan = sqrt(-8.0);
if (is_nan(maybe_nan)) {
message("math library supports ieee754");
} else {
traps |= 4;
message("math library does not support ieee754");
}
}
exit(traps);
}
bool is_special() const
{
return(is_infinity() || is_nan());
}
int compute_next_estimate(struct sm_params*params,
const double x_old[3], double x_new[3])
{
LDP laser_ref = params->laser_ref;
LDP laser_sens = params->laser_sens;
struct gpc_corr c[laser_sens->nrays];
int i; int k=0;
for(i=0;i<laser_sens->nrays;i++) {
if(!laser_sens->valid[i])
continue;
if(!ld_valid_corr(laser_sens,i))
continue;
int j1 = laser_sens->corr[i].j1;
int j2 = laser_sens->corr[i].j2;
c[k].valid = 1;
if(laser_sens->corr[i].type == corr_pl) {
c[k].p[0] = laser_sens->points[i].p[0];
c[k].p[1] = laser_sens->points[i].p[1];
c[k].q[0] = laser_ref->points[j1].p[0];
c[k].q[1] = laser_ref->points[j1].p[1];
/** TODO: here we could use the estimated alpha */
double diff[2];
diff[0] = laser_ref->points[j1].p[0]-laser_ref->points[j2].p[0];
diff[1] = laser_ref->points[j1].p[1]-laser_ref->points[j2].p[1];
double one_on_norm = 1 / sqrt(diff[0]*diff[0]+diff[1]*diff[1]);
double normal[2];
normal[0] = +diff[1] * one_on_norm;
normal[1] = -diff[0] * one_on_norm;
double cos_alpha = normal[0];
double sin_alpha = normal[1];
c[k].C[0][0] = cos_alpha*cos_alpha;
c[k].C[1][0] =
c[k].C[0][1] = cos_alpha*sin_alpha;
c[k].C[1][1] = sin_alpha*sin_alpha;
/* sm_debug("k=%d, i=%d sens_phi: %fdeg, j1=%d j2=%d, alpha_seg=%f, cos=%f sin=%f \n", k,i,
rad2deg(laser_sens->theta[i]), j1,j2, atan2(sin_alpha,cos_alpha), cos_alpha,sin_alpha);*/
#if 0
/* Note: it seems that because of numerical errors this matrix might be
not semidef positive. */
double det = c[k].C[0][0] * c[k].C[1][1] - c[k].C[0][1] * c[k].C[1][0];
double trace = c[k].C[0][0] + c[k].C[1][1];
int semidef = (det >= 0) && (trace>0);
if(!semidef) {
/* printf("%d: Adjusting correspondence weights\n",i);*/
double eps = -det;
c[k].C[0][0] += 2*sqrt(eps);
c[k].C[1][1] += 2*sqrt(eps);
}
#endif
} else {
c[k].p[0] = laser_sens->points[i].p[0];
c[k].p[1] = laser_sens->points[i].p[1];
projection_on_segment_d(
laser_ref->points[j1].p,
laser_ref->points[j2].p,
laser_sens->points_w[i].p,
c[k].q);
/* Identity matrix */
c[k].C[0][0] = 1;
c[k].C[1][0] = 0;
c[k].C[0][1] = 0;
c[k].C[1][1] = 1;
}
double factor = 1;
/* Scale the correspondence weight by a factor concerning the
information in this reading. */
if(params->use_ml_weights) {
int have_alpha = 0;
double alpha = 0;
if(!is_nan(laser_ref->true_alpha[j1])) {
alpha = laser_ref->true_alpha[j1];
have_alpha = 1;
} else if(laser_ref->alpha_valid[j1]) {
alpha = laser_ref->alpha[j1];;
have_alpha = 1;
} else have_alpha = 0;
if(have_alpha) {
double pose_theta = x_old[2];
/** Incidence of the ray
Note that alpha is relative to the first scan (not the world)
and that pose_theta is the angle of the second scan with
respect to the first, hence it's ok. */
double beta = alpha - (pose_theta + laser_sens->theta[i]);
factor = 1 / square(cos(beta));
} else {
static int warned_before = 0;
if(!warned_before) {
sm_error("Param use_ml_weights was active, but not valid alpha[] or true_alpha[]."
"Perhaps, if this is a single ray not having alpha, you should mark it as inactive.\n");
sm_error("Writing laser_ref: \n");
ld_write_as_json(laser_ref, stderr);
warned_before = 1;
}
}
}
/* Weight the points by the sigma in laser_sens */
if(params->use_sigma_weights) {
if(!is_nan(laser_sens->readings_sigma[i])) {
factor *= 1 / square(laser_sens->readings_sigma[i]);
} else {
static int warned_before = 0;
if(!warned_before) {
sm_error("Param use_sigma_weights was active, but the field readings_sigma[] was not filled in.\n");
sm_error("Writing laser_sens: \n");
ld_write_as_json(laser_sens, stderr);
}
}
}
c[k].C[0][0] *= factor;
c[k].C[1][0] *= factor;
c[k].C[0][1] *= factor;
c[k].C[1][1] *= factor;
k++;
}
/* TODO: use prior for odometry */
double std = 0.11;
const double inv_cov_x0[9] =
{1/(std*std), 0, 0,
0, 1/(std*std), 0,
0, 0, 0};
int ok = gpc_solve(k, c, 0, inv_cov_x0, x_new);
if(!ok) {
sm_error("gpc_solve_valid failed\n");
return 0;
}
double old_error = gpc_total_error(c, k, x_old);
double new_error = gpc_total_error(c, k, x_new);
sm_debug("\tcompute_next_estimate: old error: %f x_old= %s \n", old_error, friendly_pose(x_old));
sm_debug("\tcompute_next_estimate: new error: %f x_new= %s \n", new_error, friendly_pose(x_new));
sm_debug("\tcompute_next_estimate: new error - old_error: %g \n", new_error-old_error);
double epsilon = 0.000001;
if(new_error > old_error + epsilon) {
sm_error("\tcompute_next_estimate: something's fishy here! Old error: %lf new error: %lf x_old %lf %lf %lf x_new %lf %lf %lf\n",old_error,new_error,x_old[0],x_old[1],x_old[2],x_new[0],x_new[1],x_new[2]);
}
return 1;
}
/**
* Return the nearest integral value that is not larger in
* magnitude than the specified argument.
*
* @param[in] x Argument.
* @return The truncated argument.
*/
inline double trunc(double x) {
if (is_nan(x))
return std::numeric_limits<double>::quiet_NaN();
return boost::math::trunc(x, boost_policy_t());
}
static bool
fmaSingle(cpu::Core *state, Instruction instr, float *result)
{
double a, b, c;
if (slotAB == 0) {
a = state->fpr[instr.frA].paired0;
b = state->fpr[instr.frB].paired0;
} else {
a = state->fpr[instr.frA].paired1;
b = state->fpr[instr.frB].paired1;
}
if (slotC == 0) {
c = state->fpr[instr.frC].paired0;
} else {
c = state->fpr[instr.frC].paired1;
}
const double addend = (flags & FMASubtract) ? -b : b;
const bool vxsnan = is_signalling_nan(a) || is_signalling_nan(b) || is_signalling_nan(c);
const bool vximz = (is_infinity(a) && is_zero(c)) || (is_zero(a) && is_infinity(c));
const bool vxisi = (!vximz && !is_nan(a) && !is_nan(c)
&& (is_infinity(a) || is_infinity(c)) && is_infinity(b)
&& (std::signbit(a) ^ std::signbit(c)) != std::signbit(addend));
state->fpscr.vxsnan |= vxsnan;
state->fpscr.vxisi |= vxisi;
state->fpscr.vximz |= vximz;
if ((vxsnan || vxisi || vximz) && state->fpscr.ve) {
return false;
}
float d;
if (is_nan(a)) {
d = make_quiet(truncate_double(a));
} else if (is_nan(b)) {
d = make_quiet(truncate_double(b));
} else if (is_nan(c)) {
d = make_quiet(truncate_double(c));
} else if (vxisi || vximz) {
d = make_nan<float>();
} else {
if (slotC == 0) {
roundForMultiply(&a, &c); // Not necessary for slot 1.
}
double d64 = std::fma(a, c, addend);
if (state->fpscr.rn == espresso::FloatingPointRoundMode::Nearest) {
d = roundFMAResultToSingle(d64, a, addend, c);
} else {
d = static_cast<float>(d64);
}
if (possibleUnderflow<float>(d)) {
const int oldRound = fegetround();
fesetround(FE_TOWARDZERO);
volatile double addendTemp = addend;
volatile float dummy;
dummy = (float)std::fma(a, c, addendTemp);
fesetround(oldRound);
}
if (flags & FMANegate) {
d = -d;
}
}
*result = d;
return true;
}
static bool
psArithSingle(cpu::Core *state, Instruction instr, float *result)
{
double a, b;
if (slotA == 0) {
a = state->fpr[instr.frA].paired0;
} else {
a = state->fpr[instr.frA].paired1;
}
if (slotB == 0) {
b = state->fpr[op == PSMul ? instr.frC : instr.frB].paired0;
} else {
b = state->fpr[op == PSMul ? instr.frC : instr.frB].paired1;
}
const bool vxsnan = is_signalling_nan(a) || is_signalling_nan(b);
bool vxisi, vximz, vxidi, vxzdz, zx;
switch (op) {
case PSAdd:
vxisi = is_infinity(a) && is_infinity(b) && std::signbit(a) != std::signbit(b);
vximz = false;
vxidi = false;
vxzdz = false;
zx = false;
break;
case PSSub:
vxisi = is_infinity(a) && is_infinity(b) && std::signbit(a) == std::signbit(b);
vximz = false;
vxidi = false;
vxzdz = false;
zx = false;
break;
case PSMul:
vxisi = false;
vximz = (is_infinity(a) && is_zero(b)) || (is_zero(a) && is_infinity(b));
vxidi = false;
vxzdz = false;
zx = false;
break;
case PSDiv:
vxisi = false;
vximz = false;
vxidi = is_infinity(a) && is_infinity(b);
vxzdz = is_zero(a) && is_zero(b);
zx = !(vxzdz || vxsnan) && is_zero(b);
break;
}
state->fpscr.vxsnan |= vxsnan;
state->fpscr.vxisi |= vxisi;
state->fpscr.vximz |= vximz;
state->fpscr.vxidi |= vxidi;
state->fpscr.vxzdz |= vxzdz;
state->fpscr.zx |= zx;
const bool vxEnabled = (vxsnan || vxisi || vximz || vxidi || vxzdz) && state->fpscr.ve;
const bool zxEnabled = zx && state->fpscr.ze;
if (vxEnabled || zxEnabled) {
return false;
}
float d;
if (is_nan(a)) {
d = make_quiet(truncate_double(a));
} else if (is_nan(b)) {
d = make_quiet(truncate_double(b));
} else if (vxisi || vximz || vxidi || vxzdz) {
d = make_nan<float>();
} else {
switch (op) {
case PSAdd:
d = static_cast<float>(a + b);
break;
case PSSub:
d = static_cast<float>(a - b);
break;
case PSMul:
if (slotB == 0) {
roundForMultiply(&a, &b); // Not necessary for slot 1.
}
d = static_cast<float>(a * b);
break;
case PSDiv:
d = static_cast<float>(a / b);
break;
}
if (possibleUnderflow<float>(d)) {
const int oldRound = fegetround();
fesetround(FE_TOWARDZERO);
volatile double bTemp = b;
volatile float dummy;
switch (op) {
case PSAdd:
dummy = static_cast<float>(a + bTemp);
break;
case PSSub:
dummy = static_cast<float>(a - bTemp);
break;
case PSMul:
dummy = static_cast<float>(a * bTemp);
break;
case PSDiv:
dummy = static_cast<float>(a / bTemp);
break;
}
fesetround(oldRound);
}
}
*result = d;
return true;
}
/*
* Implements general numeric sort (-g).
*/
static int
gnumcoll(struct key_value *kv1, struct key_value *kv2,
size_t offset __unused)
{
double d1, d2;
int err1, err2;
bool empty1, empty2, key1_read, key2_read;
d1 = d2 = 0;
err1 = err2 = 0;
key1_read = key2_read = false;
if (debug_sort) {
bwsprintf(stdout, kv1->k, "; k1=<", ">");
bwsprintf(stdout, kv2->k, "; k2=<", ">");
}
if (kv1->hint->status == HS_UNINITIALIZED) {
errno = 0;
d1 = bwstod(kv1->k, &empty1);
err1 = errno;
if (empty1)
kv1->hint->v.gh.notnum = true;
else if (err1 == 0) {
kv1->hint->v.gh.d = d1;
kv1->hint->v.gh.nan = is_nan(d1);
kv1->hint->status = HS_INITIALIZED;
} else
kv1->hint->status = HS_ERROR;
key1_read = true;
}
if (kv2->hint->status == HS_UNINITIALIZED) {
errno = 0;
d2 = bwstod(kv2->k, &empty2);
err2 = errno;
if (empty2)
kv2->hint->v.gh.notnum = true;
else if (err2 == 0) {
kv2->hint->v.gh.d = d2;
kv2->hint->v.gh.nan = is_nan(d2);
kv2->hint->status = HS_INITIALIZED;
} else
kv2->hint->status = HS_ERROR;
key2_read = true;
}
if (kv1->hint->status == HS_INITIALIZED &&
kv2->hint->status == HS_INITIALIZED) {
if (kv1->hint->v.gh.notnum)
return ((kv2->hint->v.gh.notnum) ? 0 : -1);
else if (kv2->hint->v.gh.notnum)
return (+1);
if (kv1->hint->v.gh.nan)
return ((kv2->hint->v.gh.nan) ?
cmp_nans(kv1->hint->v.gh.d, kv2->hint->v.gh.d) :
-1);
else if (kv2->hint->v.gh.nan)
return (+1);
d1 = kv1->hint->v.gh.d;
d2 = kv2->hint->v.gh.d;
if (d1 < d2)
return (-1);
else if (d1 > d2)
return (+1);
else
return (0);
}
if (!key1_read) {
errno = 0;
d1 = bwstod(kv1->k, &empty1);
err1 = errno;
}
if (!key2_read) {
errno = 0;
d2 = bwstod(kv2->k, &empty2);
err2 = errno;
}
/* Non-value case: */
if (empty1)
return (empty2 ? 0 : -1);
else if (empty2)
return (+1);
/* NAN case */
if (is_nan(d1))
return (is_nan(d2) ? cmp_nans(d1, d2) : -1);
else if (is_nan(d2))
return (+1);
/* Infinities */
if (err1 == ERANGE || err2 == ERANGE) {
/* Minus infinity case */
if (huge_minus(d1, err1)) {
if (huge_minus(d2, err2)) {
if (d1 < d2)
return (-1);
if (d1 > d2)
return (+1);
return (0);
} else
return (-1);
} else if (huge_minus(d2, err2)) {
if (huge_minus(d1, err1)) {
if (d1 < d2)
return (-1);
if (d1 > d2)
return (+1);
return (0);
} else
return (+1);
}
/* Plus infinity case */
if (huge_plus(d1, err1)) {
if (huge_plus(d2, err2)) {
if (d1 < d2)
return (-1);
if (d1 > d2)
return (+1);
return (0);
} else
return (+1);
} else if (huge_plus(d2, err2)) {
if (huge_plus(d1, err1)) {
if (d1 < d2)
return (-1);
if (d1 > d2)
return (+1);
return (0);
} else
return (-1);
}
}
if (d1 < d2)
return (-1);
if (d1 > d2)
return (+1);
return (0);
}
void meshsurf3::smoothMesh( std::vector<vec3d> &vertices, std::vector<vec3d> &normals, std::vector<vec3i> &faces, const levelset3 *solid, FLOAT64 dpx, uint iterations ) {
std::vector<vec3d> old_vertices = vertices;
std::vector<vec3d> old_normals = normals;
std::vector<std::vector<uint> > connections;
std::vector<FLOAT64> areas;
connections.resize(vertices.size());
areas.resize(vertices.size());
for( uint n=0; n<vertices.size(); n++ ) {
areas[n] = 0.0;
}
for( uint n=0; n<faces.size(); n++ ) {
for( uint i=0; i<3; i++ ) {
connections[faces[n][i]].push_back(faces[n][(i+1)%3]);
}
FLOAT64 area = ((vertices[faces[n][1]]-vertices[faces[n][0]])^(vertices[faces[n][2]]-vertices[faces[n][0]])).len();
for( uint j=0; j<3; j++ ) {
areas[faces[n][j]] += area / 3.0;
}
}
// Smooth out
for( uint k=0; k<iterations; k++ ) {
for( uint n=0; n<vertices.size(); n++ ) {
if( solid->evalLevelset(vertices[n]) < dpx ) continue;
FLOAT64 wsum = 0.0;
vec3d pos;
// Add self
FLOAT64 w = is_nan(areas[n]) ? 0.0 : areas[n]+1e-8;
if( ! is_nan(old_vertices[n]) && w ) {
pos += w*old_vertices[n];
wsum += w;
}
// Add neighbors
for( uint m=0; m<connections[n].size(); m++ ) {
uint idx = connections[n][m];
if( ! is_nan(old_vertices[idx]) ) {
FLOAT64 w = is_nan(areas[idx]) ? 0.0 : areas[idx]+1e-8;
if( w ) {
pos += w*old_vertices[idx];
wsum += w;
}
}
}
if( wsum ) vertices[n] = pos / wsum;
}
old_vertices = vertices;
for( uint n=0; n<normals.size(); n++ ) {
if( solid->evalLevelset(vertices[n]) < dpx ) continue;
FLOAT64 wsum = 0.0;
vec3d normal;
// Add self
FLOAT64 w = is_nan(areas[n]) ? 0.0 : areas[n]+1e-8;
if( ! is_nan(old_normals[n]) && w ) {
normal += w*old_normals[n];
wsum += w;
}
// Add neighbors
for( uint m=0; m<connections[n].size(); m++ ) {
uint idx = connections[n][m];
if( ! is_nan(old_normals[idx]) ) {
FLOAT64 w = is_nan(areas[idx]) ? 0.0 : areas[idx]+1e-8;
if( w ) {
normal += w*old_normals[idx];
wsum += w;
}
}
}
if( wsum ) normals[n] = normal.normal();
}
old_normals = normals;
}
removeNan(vertices,normals,faces,connections,areas);
}
inline bool is_valid( const T &v) {
return ! is_inf(v) && ! is_nan(v);
}
// Force already-denormal float value to zero
static inline void
sanitize_denormal(float& value) {
if (is_nan(value)) {
value = 0.f;
}
}
bool runTests(const std::string &path)
{
uint32_t testsFailed = 0, testsPassed = 0;
uint32_t baseAddress = mem::MEM2Base;
Instruction bclr = encodeInstruction(InstructionID::bclr);
bclr.bo = 0x1f;
mem::write(baseAddress + 4, bclr.value);
fs::FileSystem filesystem;
fs::FolderEntry entry;
fs::HostPath base = path;
filesystem.mountHostFolder("/tests", base, fs::Permissions::Read);
auto folder = filesystem.openFolder("/tests");
while (folder->read(entry)) {
std::ifstream file(base.join(entry.name).path(), std::ifstream::in | std::ifstream::binary);
cereal::BinaryInputArchive cerealInput(file);
TestFile testFile;
// Parse test file with cereal
testFile.name = entry.name;
cerealInput(testFile);
// Run tests
gLog->info("Checking {}", testFile.name);
for (auto &test : testFile.tests) {
bool failed = false;
if (!TEST_FMADDSUB) {
auto data = espresso::decodeInstruction(test.instr);
switch (data->id) {
case InstructionID::fmadd:
case InstructionID::fmadds:
case InstructionID::fmsub:
case InstructionID::fmsubs:
case InstructionID::fnmadd:
case InstructionID::fnmadds:
case InstructionID::fnmsub:
case InstructionID::fnmsubs:
failed = true;
break;
}
if (failed) {
continue;
}
}
// Setup core state from test input
cpu::CoreRegs *state = cpu::this_core::state();
memset(state, 0, sizeof(cpu::CoreRegs));
state->cia = 0;
state->nia = baseAddress;
state->xer = test.input.xer;
state->cr = test.input.cr;
state->fpscr = test.input.fpscr;
state->ctr = test.input.ctr;
for (auto i = 0; i < 4; ++i) {
state->gpr[i + 3] = test.input.gpr[i];
state->fpr[i + 1].paired0 = test.input.fr[i];
}
// Execute test
mem::write(baseAddress, test.instr.value);
cpu::jit::clearCache();
cpu::this_core::executeSub();
// Check XER (all bits)
if (state->xer.value != test.output.xer.value) {
gLog->error("Test failed, xer expected {:08X} found {:08X}", test.output.xer.value, state->xer.value);
failed = true;
}
// Check Condition Register (all bits)
if (state->cr.value != test.output.cr.value) {
gLog->error("Test failed, cr expected {:08X} found {:08X}", test.output.cr.value, state->cr.value);
failed = true;
}
// Check FPSCR
if (TEST_FPSCR) {
if (!TEST_FPSCR_FR) {
state->fpscr.fr = 0;
test.output.fpscr.fr = 0;
}
if (!TEST_FPSCR_UX) {
state->fpscr.ux = 0;
test.output.fpscr.ux = 0;
}
auto state_fpscr = state->fpscr.value;
auto test_fpscr = test.output.fpscr.value;
if (state_fpscr != test_fpscr) {
gLog->error("Test failed, fpscr {:08X} found {:08X}", test.output.fpscr.value, state->fpscr.value);
compareFPSCR(test.input.fpscr, state->fpscr, test.output.fpscr);
failed = true;
}
}
// Check CTR
if (state->ctr != test.output.ctr) {
gLog->error("Test failed, ctr expected {:08X} found {:08X}", test.output.ctr, state->ctr);
failed = true;
}
// Check all GPR
for (auto i = 0; i < 4; ++i) {
auto reg = i + hwtest::GPR_BASE;
auto value = state->gpr[reg];
auto expected = test.output.gpr[i];
if (value != expected) {
gLog->error("Test failed, r{} expected {:08X} found {:08X}", reg, expected, value);
failed = true;
}
}
// Check all FPR
for (auto i = 0; i < 4; ++i) {
auto reg = i + hwtest::FPR_BASE;
auto value = state->fpr[reg].value;
auto expected = test.output.fr[i];
if (!is_nan(value) && !is_nan(expected) && !is_infinity(value) && !is_infinity(expected)) {
double dval = value / expected;
if (dval < 0.999 || dval > 1.001) {
gLog->error("Test failed, f{} expected {:16f} found {:16f}", reg, expected, value);
failed = true;
}
} else {
if (is_nan(value) && is_nan(expected)) {
auto bits = get_float_bits(value);
bits.sign = get_float_bits(expected).sign;
value = bits.v;
}
if (bit_cast<uint64_t>(value) != bit_cast<uint64_t>(expected)) {
gLog->error("Test failed, f{} expected {:16X} found {:16X}", reg, bit_cast<uint64_t>(expected), bit_cast<uint64_t>(value));
failed = true;
}
}
}
if (failed) {
Disassembly dis;
// Print disassembly
disassemble(test.instr, dis, baseAddress);
gLog->debug(dis.text);
// Print all test fields
gLog->debug("{:08x} Input Hardware Interp", test.instr.value);
for (auto field : dis.instruction->read) {
printTestField(field, test.instr, &test.input, &test.output, state);
}
for (auto field : dis.instruction->write) {
printTestField(field, test.instr, &test.input, &test.output, state);
}
for (auto field : dis.instruction->flags) {
printTestField(field, test.instr, &test.input, &test.output, state);
}
gLog->debug("");
++testsFailed;
} else {
++testsPassed;
}
}
}
gLog->info("Passed: {}, Failed: {}", testsPassed, testsFailed);
return true;
}
uint32_t decSingleIsPositive (const decSingle* _0) noexcept
{
// see decBasic.c, decNumberLocal.h
const uint32_t word = *((const uint32_t*)_0);
return !is_signed (word) && !is_zero (word) && !is_nan (word);
}
void IterativeSolver::execute()
{
RDM::RDSolver& mysolver = solver().as_type< RDM::RDSolver >();
/// @todo this configuration sould be in constructor but does not work there
configure_option_recursively( "iterator", this->uri() );
// access components (out of loop)
CActionDirector& boundary_conditions =
access_component( "cpath:../BoundaryConditions" ).as_type<CActionDirector>();
CActionDirector& domain_discretization =
access_component( "cpath:../DomainDiscretization" ).as_type<CActionDirector>();
CAction& synchronize = mysolver.actions().get_child("Synchronize").as_type<CAction>();
Component& cnorm = post_actions().get_child("ComputeNorm");
cnorm.configure_option("Field", mysolver.fields().get_child( RDM::Tags::residual() ).follow()->uri() );
// iteration loop
Uint iter = 1; // iterations start from 1 ( max iter zero will do nothing )
property("iteration") = iter;
while( ! stop_condition() ) // non-linear loop
{
// (1) the pre actions - cleanup residual, pre-process something, etc
pre_actions().execute();
// (2) domain discretization
domain_discretization.execute();
// (3) apply boundary conditions
boundary_conditions.execute();
// (4) update
update().execute();
// (5) update
synchronize.execute();
// (6) the post actions - compute norm, post-process something, etc
post_actions().execute();
// output convergence info
/// @todo move current rhs as a prpoerty of the iterate or solver components
if( Comm::PE::instance().rank() == 0 )
{
Real rhs_norm = cnorm.properties().value<Real>("Norm");
CFinfo << "iter [" << std::setw(4) << iter << "]"
<< "L2(rhs) [" << std::setw(12) << rhs_norm << "]" << CFendl;
if ( is_nan(rhs_norm) || is_inf(rhs_norm) )
throw FailedToConverge(FromHere(),
"Solution diverged after "+to_str(iter)+" iterations");
}
// raise signal that iteration is done
raise_iteration_done();
// increment iteration
property("iteration") = ++iter; // update the iteration number
}
}
int main(int argc, const char * argv[]) {
sm_set_program_name(argv[0]);
csm_options_banner("ld_noise: Adds noise to readings in a scan");
struct ld_noise_params p;
struct csm_option* ops = csm_options_allocate(20);
csm_options_double(ops, "discretization", &p.discretization, 0.0,
"Size of discretization (disabled if 0)");
csm_options_double(ops, "sigma", &p.sigma, 0.0,
"Std deviation of gaussian noise (disabled if 0)");
csm_options_int(ops, "lambertian", &p.lambertian, 0,
"Use lambertian model cov = sigma^2 / cos(beta^2) where beta is the incidence. Need have alpha or true_alpha.");
csm_options_int(ops, "seed", &p.seed, 0,
"Seed for random number generator (if 0, use GSL_RNG_SEED env. variable).");
csm_options_string(ops, "in", &p.file_input, "stdin", "Input file ");
csm_options_string(ops, "out", &p.file_output, "stdout", "Output file ");
if(!csm_options_parse_args(ops, argc, argv)) {
fprintf(stderr, "A simple program for adding noise to sensor scans.\n\nUsage:\n");
csm_options_print_help(ops, stderr);
return -1;
}
FILE * in = open_file_for_reading(p.file_input);
if(!in) return -3;
FILE * out = open_file_for_writing(p.file_output);
if(!out) return -2;
gsl_rng_env_setup();
gsl_rng * rng = gsl_rng_alloc (gsl_rng_ranlxs0);
if(p.seed != 0)
gsl_rng_set(rng, (unsigned int) p.seed);
LDP ld; int count = 0;
while( (ld = ld_from_json_stream(in))) {
if(!ld_valid_fields(ld)) {
sm_error("Invalid laser data (#%d in file)\n", count);
continue;
}
int i;
for(i=0;i<ld->nrays;i++) {
if(!ld->valid[i]) continue;
double * reading = ld->readings + i;
if(p.sigma > 0) {
double add_sigma = p.sigma;
if(p.lambertian) {
int have_alpha = 0;
double alpha = 0;
if(!is_nan(ld->true_alpha[i])) {
alpha = ld->true_alpha[i];
have_alpha = 1;
} else if(ld->alpha_valid[i]) {
alpha = ld->alpha[i];;
have_alpha = 1;
} else have_alpha = 0;
if(have_alpha) {
/* Recall that alpha points outside the surface */
double beta = (alpha+M_PI) - ld->theta[i];
add_sigma = p.sigma / cos(beta);
} else {
sm_error("Because lambertian is active, I need either true_alpha[] or alpha[]");
ld_write_as_json(ld, stderr);
return -1;
}
}
*reading += gsl_ran_gaussian(rng, add_sigma);
if(is_nan(ld->readings_sigma[i])) {
ld->readings_sigma[i] = add_sigma;
} else {
ld->readings_sigma[i] = sqrt(square(add_sigma) + square(ld->readings_sigma[i]));
}
}
if(p.discretization > 0)
*reading -= fmod(*reading , p.discretization);
}
ld_write_as_json(ld, out);
ld_free(ld);
}
return 0;
}
template<class Extension,class Info>
struct call<minnum_,
tag::simd_(tag::arithmetic_,Extension),Info>
{
template<class Sig> struct result;
template<class This,class A0>
struct result<This(A0,A0)>
: meta::strip<A0>{};//
NT2_FUNCTOR_CALL_DISPATCH(
2,
typename nt2::meta::scalar_of<A0>::type,
(2, (real_,arithmetic_))
)
NT2_FUNCTOR_CALL_EVAL_IF(2, real_)
{
const A0 a = select(is_nan(a0),a1,a0);
const A0 b = select(is_nan(a1),a0,a1);
return nt2::min(a, b);
}
NT2_FUNCTOR_CALL_EVAL_IF(2, arithmetic_)
{
return nt2::min(a0, a1);
}
};
} }
#endif
bool is_nan(glm::vec3 vec) {
return is_nan(vec.x) || is_nan(vec.y) || is_nan(vec.z);
}
uint32_t decSingleIsNaN (const decSingle* _num) noexcept
{
return is_nan (*((const uint32_t*)_num));
}
//==========================================================================
// Return matrix condition number
//==========================================================================
base_t cond() const
{
base_t r = w_(1)/w_(nt2::min(m_, n_));
return is_nan(r) ? Inf<base_t>() : r;
}
static bool is_nan( vec3d p ) {
return is_nan(p[0]) || is_nan(p[1]) || is_nan(p[2]);
}