///
/// @par Detailed description
/// ...
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
void
sasio::Files::
resize_array()
{
int nf = _number_of_frames() ;
Eigen::Array<float,Eigen::Dynamic,Eigen::Dynamic> temp_x(_natoms(),nf) ;
Eigen::Array<float,Eigen::Dynamic,Eigen::Dynamic> temp_y(_natoms(),nf) ;
Eigen::Array<float,Eigen::Dynamic,Eigen::Dynamic> temp_z(_natoms(),nf) ;
temp_x = _x() ;
temp_y = _y() ;
temp_z = _z() ;
_x().resize(_natoms(),_number_of_frames()+1) ;
_y().resize(_natoms(),_number_of_frames()+1) ;
_z().resize(_natoms(),_number_of_frames()+1) ;
// see: slice.cpp in retired_ideas/play_with_eigen/
// mat1.block(i,j,rows,cols)
// a.block(0, 0, 3, 1) = b.block(0,0,3,1) ;
_x().block(0, 0, _natoms(), nf) = temp_x.block(0,0,_natoms(),nf) ;
_y().block(0, 0, _natoms(), nf) = temp_y.block(0,0,_natoms(),nf) ;
_z().block(0, 0, _natoms(), nf) = temp_z.block(0,0,_natoms(),nf) ;
return ;
}
void testStrCpy() {
WCHAR a[] = { _x('t'), _x('e'), _x('s'), _x('t'), 0 };
WCHAR b[5];
CPPUNIT_ASSERT_EQUAL( (WCHAR*) b, (WCHAR*) winchar_strcpy(b, a) );
CPPUNIT_ASSERT_EQUAL( 0, memcmp(a, b, 5) );
}
///
/// @par Detailed description
/// ...
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
void
sasio::Files::
read_dcd_step(FILE *dcd_infile, int &frame, int &natoms, int &reverseEndian)
{
int charmm = 0 ;
int num_fixed = 0 ;
int result ;
std::vector<float> vector_x(natoms) ;
std::vector<float> vector_y(natoms) ;
std::vector<float> vector_z(natoms) ;
result = read_dcdstep(dcd_infile,natoms,&vector_x[0],&vector_y[0],&vector_z[0],num_fixed,frame,reverseEndian,charmm) ;
if(_x().cols() < frame)
{
std::cout << "resizing array: cols() = " << _x().cols() << " frame = " << frame << std::endl ;
resize_array() ;
}
for(int i = 0 ; i < _natoms() ; ++i)
{
_x()(i,frame) = vector_x[i] ;
_y()(i,frame) = vector_y[i] ;
_z()(i,frame) = vector_z[i] ;
}
_number_of_frames()++ ;
return ;
}
void testStrCpy() {
WINWCHAR a[] = { _x('t'), _x('e'), _x('s'), _x('t'), 0 };
WINWCHAR b[5];
CPPUNIT_ASSERT_EQUAL( b, WinWStrCpy(b, a) );
CPPUNIT_ASSERT_EQUAL( 0, memcmp(a, b, 5 * sizeof(WINWCHAR)) );
}
static void
saved_rack_parse_jackrack (jack_rack_t * jack_rack, saved_rack_t * saved_rack, const char * filename, xmlNodePtr jackrack)
{
xmlNodePtr node;
xmlChar *content;
saved_plugin_t * saved_plugin;
#ifdef WIN32
xmlFreeFunc xmlFree = NULL;
xmlMemGet( &xmlFree, NULL, NULL, NULL);
#endif
for (node = jackrack->children; node; node = node->next)
{
if (xmlStrcmp (node->name, _x("channels")) == 0)
{
content = xmlNodeGetContent (node);
saved_rack->channels = strtoul (_s(content), NULL, 10);
xmlFree (content);
}
else if (xmlStrcmp (node->name, _x("samplerate")) == 0)
{
content = xmlNodeGetContent (node);
saved_rack->sample_rate = strtoul (_s(content), NULL, 10);
xmlFree (content);
}
else if (xmlStrcmp (node->name, _x("plugin")) == 0)
{
saved_plugin = g_malloc0 (sizeof (saved_plugin_t));
saved_rack->plugins = g_slist_append (saved_rack->plugins, saved_plugin);
saved_rack_parse_plugin (jack_rack, saved_rack, saved_plugin, filename, node);
}
}
}
void BlockLocalPositionEstimator::updateSSParams()
{
// input noise covariance matrix
_R.setZero();
_R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
_R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
_R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get();
// process noise power matrix
_Q.setZero();
float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get();
float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get();
_Q(X_x, X_x) = pn_p_sq;
_Q(X_y, X_y) = pn_p_sq;
_Q(X_z, X_z) = pn_p_sq;
_Q(X_vx, X_vx) = pn_v_sq;
_Q(X_vy, X_vy) = pn_v_sq;
_Q(X_vz, X_vz) = pn_v_sq;
// technically, the noise is in the body frame,
// but the components are all the same, so
// ignoring for now
float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get();
_Q(X_bx, X_bx) = pn_b_sq;
_Q(X_by, X_by) = pn_b_sq;
_Q(X_bz, X_bz) = pn_b_sq;
// terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity
float pn_t_noise_density =
_pn_t_noise_density.get() +
(_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
}
/* ...release buffer set */
static inline void sview_release_buffers(app_data_t *app, GstBuffer **buffers)
{
int i;
pthread_mutex_lock(&app->lock);
/* ...drop the buffers - they are heads of the rendering queues */
for (i = 0; i < CAMERAS_NUMBER; i++)
{
GQueue *queue = &app->render[i];
GstBuffer *buffer;
/* ...queue cannot be empty */
BUG(g_queue_is_empty(queue), _x("inconsistent state of camera-%d"), i);
/* ...remove head of the queue */
buffer = g_queue_pop_head(queue);
/* ...buffer must be at the head of the queue */
BUG(buffers[i] != buffer, _x("invalid queue head: %p != %p"), buffers[i], buffer);
/* ...return buffer to a pool */
gst_buffer_unref(buffer);
/* ...check if queue gets empty */
(g_queue_is_empty(queue) ? app->frames ^= 1 << i : 0);
}
pthread_mutex_unlock(&app->lock);
}
///
/// @par Detailed description
/// ...
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
void
sasio::Files::
read_dcd(std::string &dcd_input_file_name)
{
int input_natoms, input_nset, input_reverseEndian ;
FILE *dcd_file_pointer ;
dcd_file_pointer = sasio::open_dcd_read(dcd_input_file_name, input_natoms, input_nset, input_reverseEndian) ;
int input_frame = 0 ;
_number_of_frames() = 0 ;
_x().setZero(_natoms(), input_nset);
_y().setZero(_natoms(), input_nset);
_z().setZero(_natoms(), input_nset);
std::cout << "_x().cols() = " << _x().cols() << std::endl ;
std::cout << "input_nset = " << input_nset << std::endl ;
for(int i = 0 ; i < input_nset ; ++i)
{
std::cout << "." ;
read_dcd_step(dcd_file_pointer, i, input_natoms, input_reverseEndian) ;
}
int result = close_dcd_read(dcd_file_pointer) ;
std::cout << std::endl ;
return ;
}
int
main(int argc, char *argv[])
{
struct utimbuf buf, *bufp;
char *ref_file, *stamp;
char *pg = argv[0];
int i;
ref_file = stamp = 0;
#define _x() ++argv; --argc; if (argc <= 0) goto x
_x(); if (strcmp(*argv, "-r") == 0) {
_x();
ref_file = *argv;
_x();
}
if (strcmp(*argv, "-t") == 0) {
_x();
stamp = *argv;
_x();
}
bufp = &buf;
if (ref_file) {
struct stat xx;
if (lstat(ref_file, &xx) == -1) goto y;
buf.modtime = xx.st_mtime;
buf.actime = xx.st_atime;
} else if (stamp) {
/* parse MMDDhhmm[[CC]YY][.ss] */
struct tm xx;
char *s; char *end;
char temp[3];
end = stamp + strlen(stamp);
s=stamp;if(s>=end)goto o;strncpy(temp,s,2);xx.tm_mon=atoi(s);
s+=2;if(s>=end)goto o;strncpy(temp,s,2);xx.tm_mday=atoi(s);
s+=2;if(s>=end)goto o;strncpy(temp,s,2);xx.tm_hour=atoi(s);
s+=2;if(s>=end)goto o;strncpy(temp,s,2);xx.tm_min=atoi(s);
s+=7;if(s>=end)goto o;strncpy(temp,s,2);xx.tm_sec=atoi(s);
o:
buf.actime = buf.modtime = mktime(&xx);
} else
bufp = 0;
for (i=0; i<argc; i++) {
utime(argv[i], bufp);
}
exit(0);
x:
printf("usage: %s [-r file] [-t MMDDhhmm[.ss]] files...\n", pg);
exit(1);
y:
perror("foo");
exit(1);
}
wkt_generator<OutputIterator, Geometry>::wkt_generator(bool single)
: wkt_generator::base_type(wkt)
{
boost::spirit::karma::uint_type uint_;
boost::spirit::karma::_val_type _val;
boost::spirit::karma::_1_type _1;
boost::spirit::karma::lit_type lit;
boost::spirit::karma::_a_type _a;
boost::spirit::karma::_b_type _b;
boost::spirit::karma::_c_type _c;
boost::spirit::karma::_r1_type _r1;
boost::spirit::karma::eps_type eps;
boost::spirit::karma::string_type kstring;
wkt = point | linestring | polygon
;
point = &uint_(mapnik::geometry_type::types::Point)[_1 = _type(_val)]
<< kstring[ phoenix::if_ (single) [_1 = "Point("]
.else_[_1 = "("]]
<< point_coord [_1 = _first(_val)] << lit(')')
;
linestring = &uint_(mapnik::geometry_type::types::LineString)[_1 = _type(_val)]
<< kstring[ phoenix::if_ (single) [_1 = "LineString("]
.else_[_1 = "("]]
<< coords
<< lit(')')
;
polygon = &uint_(mapnik::geometry_type::types::Polygon)[_1 = _type(_val)]
<< kstring[ phoenix::if_ (single) [_1 = "Polygon("]
.else_[_1 = "("]]
<< coords2
<< lit("))")
;
point_coord = &uint_ << coordinate << lit(' ') << coordinate
;
polygon_coord %= ( &uint_(mapnik::SEG_MOVETO)
<< eps[_r1 += 1][_a = _x(_val)][ _b = _y(_val)]
<< kstring[ if_ (_r1 > 1) [_1 = "),("]
.else_[_1 = "("]]
|
&uint_(mapnik::SEG_LINETO)
<< lit(',') << eps[_a = _x(_val)][_b = _y(_val)]
)
<< coordinate[_1 = _a]
<< lit(' ')
<< coordinate[_1 = _b]
;
coords2 %= *polygon_coord(_a,_b,_c)
;
coords = point_coord % lit(',')
;
}
void testFromTchar() {
WCHAR test[] = { _x('t'), _x('e'), _x('s'), _x('t'), 0 };
WCHAR *dyn = wcsdup_fromansi("test");
CPPUNIT_ASSERT_EQUAL( 0, memcmp(test, dyn, 5) );
free(dyn);
}
void testFromAnsi() {
WCHAR test[] = { _x('t'), _x('e'), _x('s'), _x('t'), 0 };
WCHAR *dyn = winchar_fromansi("test");
CPPUNIT_ASSERT_EQUAL( 0, memcmp(test, dyn, 5) );
delete [] dyn;
}
void testToAnsi() {
WCHAR test[] = { _x('t'), _x('e'), _x('s'), _x('t'), 0 };
char *dyn = winchar_toansi(test);
CPPUNIT_ASSERT_EQUAL( 0, strcmp("test", dyn) );
delete [] dyn;
}
void BlockLocalPositionEstimator::flowCorrect()
{
// measure flow
Vector<float, n_y_flow> y;
if (flowMeasure(y) != OK) { return; }
// flow measurement matrix and noise matrix
Matrix<float, n_y_flow, n_x> C;
C.setZero();
C(Y_flow_x, X_x) = 1;
C(Y_flow_y, X_y) = 1;
Matrix<float, n_y_flow, n_y_flow> R;
R.setZero();
R(Y_flow_x, Y_flow_x) =
_flow_xy_stddev.get() * _flow_xy_stddev.get();
R(Y_flow_y, Y_flow_y) =
_flow_xy_stddev.get() * _flow_xy_stddev.get();
// residual
Vector<float, 2> r = y - C * _x;
// residual covariance, (inverse)
Matrix<float, n_y_flow, n_y_flow> S_I =
inv<float, n_y_flow>(C * _P * C.transpose() + R);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_flow]) {
if (_flowFault < FAULT_MINOR) {
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta));
_flowFault = FAULT_MINOR;
}
} else if (_flowFault) {
_flowFault = FAULT_NONE;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK");
}
if (_flowFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_flow> K =
_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
correctionLogic(dx);
_x += dx;
_P -= K * C * _P;
} else {
// reset flow integral to current estimate of position
// if a fault occurred
_flowX = _x(X_x);
_flowY = _x(X_y);
}
}
///
/// @par Detailed description
/// ...
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
int
sascalc::Prop::
self_overlap(float &cutoff, int &frame){
int check = 0 ; // overlapped == 1
// default == no overlap will return == 0
float dist ;
float this_x , that_x ;
float this_y , that_y ;
float this_z , that_z ;
float dx2, dy2, dz2 ;
int count = 1 ;
for(int i = 0 ; i != _natoms()-1 ; ++i)
{
this_x = _x()(i,frame) ;
this_y = _y()(i,frame) ;
this_z = _z()(i,frame) ;
for(int j = i+1 ; j != _natoms() ; ++j)
{
that_x = _x()(j,frame) ;
that_y = _y()(j,frame) ;
that_z = _z()(j,frame) ;
dx2 = (this_x - that_x)*(this_x - that_x) ;
dy2 = (this_y - that_y)*(this_y - that_y) ;
dz2 = (this_z - that_z)*(this_z - that_z) ;
dist = sqrt(dx2+dy2+dz2) ;
if(dist < cutoff)
{
std::cout << "count = " << count << std::endl ;
std::cout << "this_atom = " << _atom_name()[count-1] << std::endl ;
std::cout << "that_atom = " << _atom_name()[count] << std::endl ;
std::cout << "dist = " << dist << std::endl ;
std::cout << "this_x = " << this_x << std::endl ;
std::cout << "this_y = " << this_y << std::endl ;
std::cout << "this_z = " << this_z << std::endl ;
std::cout << "that_x = " << that_x << std::endl ;
std::cout << "that_y = " << that_y << std::endl ;
std::cout << "that_z = " << that_z << std::endl ;
check = 1 ;
return check ;
}
}
// std::cout << count << " " ;
count++ ;
}
std::cout << std::endl ;
return check ;
}
void testStrDup() {
WCHAR a[] = { _x('a'), _x('b'), _x('c'), 0 };
WCHAR *b = winchar_strdup(a);
CPPUNIT_ASSERT_EQUAL( 0, winchar_strcmp(a, b) );
delete [] b;
}
void testStrDup() {
WINWCHAR a[] = { _x('a'), _x('b'), _x('c'), 0 };
WINWCHAR *b = WinWStrDupFromWinWStr(a);
CPPUNIT_ASSERT_EQUAL( 0, WinWStrCmp(a, b) );
free(b);
}
void testStrDup() {
WCHAR a[] = { _x('a'), _x('b'), _x('c'), 0 };
WCHAR *b = _wcsdup(a);
CPPUNIT_ASSERT_EQUAL( 0, wcscmp(a, b) );
delete [] b;
}
///
/// @par Detailed description
/// ...
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
void
sascalc::Prop::
calc_pmi(int &frame)
{
std::vector<float> com = calc_com(frame) ;
_x().col(frame) -= com[0] ;
_y().col(frame) -= com[1] ;
_z().col(frame) -= com[2] ;
float Ixx = (_atom_mass()*(_y().col(frame)*_y().col(frame) + _z().col(frame)*_z().col(frame))).sum() ;
float Iyy = (_atom_mass()*(_x().col(frame)*_x().col(frame) + _z().col(frame)*_z().col(frame))).sum() ;
float Izz = (_atom_mass()*(_x().col(frame)*_x().col(frame) + _y().col(frame)*_y().col(frame))).sum() ;
float Ixy = (-_atom_mass()*(_x().col(frame)*_y().col(frame))).sum() ;
float Ixz = (-_atom_mass()*(_x().col(frame)*_z().col(frame))).sum() ;
float Iyz = (-_atom_mass()*(_y().col(frame)*_z().col(frame))).sum() ;
float Iyx = (-_atom_mass()*(_y().col(frame)*_x().col(frame))).sum() ;
float Izx = (-_atom_mass()*(_z().col(frame)*_x().col(frame))).sum() ;
float Izy = (-_atom_mass()*(_z().col(frame)*_y().col(frame))).sum() ;
Eigen::Matrix3f I ;
I << Ixx, Ixy, Ixz,
Iyx, Iyy, Iyz,
Izx, Izy, Izz;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(I);
if (eigensolver.info() != Eigen::Success)
{
std::cout << "PMI calculation failed" << std::endl ;
return ;
}
_uk() = eigensolver.eigenvalues() ; // .col(0) ;
_ak() = eigensolver.eigenvectors() ;
// std::cout << "The eigenvalues of I are:\n" << eigensolver.eigenvalues() << std::endl;
// std::cout << "Here's a matrix whose columns are eigenvectors of I \n"
// << "corresponding to these eigenvalues:\n"
// << eigensolver.eigenvectors() << std::endl;
_x().col(frame) += com[0] ;
_y().col(frame) += com[1] ;
_z().col(frame) += com[2] ;
return ;
}
void testFromTchar() {
unsigned short test[] = { _x('t'), _x('e'), _x('s'), _x('t'), 0 };
WINWCHAR *dyn = WinWStrDupFromTChar(_T("test"));
CPPUNIT_ASSERT_EQUAL( 0, memcmp(test, dyn, 5) );
free(dyn);
dyn = WinWStrDupFromChar("test");
CPPUNIT_ASSERT_EQUAL( 0, memcmp(test, dyn, 5) );
free(dyn);
dyn = WinWStrDupFromWC(L"test");
CPPUNIT_ASSERT_EQUAL( 0, memcmp(test, dyn, 5) );
free(dyn);
}
void Compute(gmp_randstate_t r) const {
// The algorithm is sample x and z from the exponential distribution; if
// (x-1)^2 < 2*z, return (random sign)*x; otherwise repeat. Probability
// of acceptance is sqrt(pi/2) * exp(-1/2) = 0.7602.
while (true) {
_edist(_x, r);
_edist(_z, r);
for (mp_size_t k = 1; ; ++k) {
_x.ExpandTo(r, k - 1);
_z.ExpandTo(r, k - 1);
mpfr_prec_t prec = std::max(mpfr_prec_t(MPFR_PREC_MIN), k * bits);
mpfr_set_prec(_xf, prec);
mpfr_set_prec(_zf, prec);
// Try for acceptance first; so compute upper limit on (y-1)^2 and
// lower limit on 2*z.
if (_x.UInteger() == 0) {
_x(_xf, MPFR_RNDD);
mpfr_ui_sub(_xf, 1u, _xf, MPFR_RNDU);
} else {
_x(_xf, MPFR_RNDU);
mpfr_sub_ui(_xf, _xf, 1u, MPFR_RNDU);
}
mpfr_sqr(_xf, _xf, MPFR_RNDU);
_z(_zf, MPFR_RNDD);
mpfr_mul_2ui(_zf, _zf, 1u, MPFR_RNDD);
if (mpfr_cmp(_xf, _zf) < 0) { // (y-1)^2 < 2*z, so accept
if (_x.Boolean(r)) _x.Negate(); // include a random sign
return;
}
// Try for rejection; so compute lower limit on (y-1)^2 and upper
// limit on 2*z.
if (_x.UInteger() == 0) {
_x(_xf, MPFR_RNDU);
mpfr_ui_sub(_xf, 1u, _xf, MPFR_RNDD);
} else {
_x(_xf, MPFR_RNDD);
mpfr_sub_ui(_xf, _xf, 1u, MPFR_RNDD);
}
mpfr_sqr(_xf, _xf, MPFR_RNDD);
_z(_zf, MPFR_RNDU);
mpfr_mul_2ui(_zf, _zf, 1u, MPFR_RNDU);
if (mpfr_cmp(_xf, _zf) > 0) // (y-1)^2 > 2*z, so reject
break;
// Otherwise repeat with more precision
}
// Reject and start over with a new y and z
}
}
int ZMatQRSolver_NR3::solve( double *b, double *x ) {
// solve Ax = b, must call decompose() first.
VecDoub _b( rows, b, 1 );
VecDoub _x( cols, x, 1 );
pNRqr->solve( _b, _x );
return 1;
}
namespace Namespace {
template<char...> int operator"" _x();
int k = _x(); // expected-error {{undeclared identifier '_x'}}
int _y(unsigned long long);
int k2 = 123_y; // expected-error {{no matching literal operator for call to 'operator "" _y'}}
}
/* ...process new input buffer submitted from camera */
static int sview_input_process(void *data, int i, GstBuffer *buffer)
{
app_data_t *app = data;
BUG(i >= CAMERAS_NUMBER, _x("invalid camera index: %d"), i);
TRACE(DEBUG, _b("camera-%d: input buffer received"), i);
/* ...get queue access lock */
pthread_mutex_lock(&app->lock);
/* ...check if playback is enabled */
if ((app->flags & APP_FLAG_EOS) == 0)
{
/* ...place buffer into main rendering queue (take ownership) */
g_queue_push_tail(&app->render[i], buffer), gst_buffer_ref(buffer);
/* ...indicate buffer is available */
app->frames &= ~(1 << i);
/* ...schedule processing if all buffers are ready */
if ((app->frames & ((1 << CAMERAS_NUMBER) - 1)) == 0)
{
/* ...all buffers available; trigger surround-view scene processing */
window_schedule_redraw(app->window);
}
}
/* ...release queue access lock */
pthread_mutex_unlock(&app->lock);
return 0;
}
/* ...start object-detection track */
static track_desc_t * __app_objdet_track(app_data_t *app)
{
track_desc_t *track;
if (app->flags & APP_FLAG_LIVE)
{
track = objdet_track_live();
}
else if (app->flags & APP_FLAG_NEXT)
{
track = objdet_track_next();
}
else if (app->flags & APP_FLAG_PREV)
{
track = objdet_track_prev();
}
else
{
track = objdet_track_current();
}
BUG(!track, _x("invalid state"));
/* ...select detector configuration */
app->od_cfg = track->od_cfg;
/* ...set global camera configuration parameters */
ldwConfigurationFilePath = track->camera_cfg;
/* ...set window rendering hook */
app_main_info.redraw = objdet_redraw;
return track;
}
void BlockLocalPositionEstimator::publishEstimatorStatus()
{
_pub_est_status.get().timestamp = _timeStamp;
for (int i = 0; i < n_x; i++) {
_pub_est_status.get().states[i] = _x(i);
_pub_est_status.get().covariances[i] = _P(i, i);
}
_pub_est_status.get().n_states = n_x;
_pub_est_status.get().nan_flags = 0;
_pub_est_status.get().health_flags =
((_baroFault > FAULT_NONE) << SENSOR_BARO)
+ ((_gpsFault > FAULT_NONE) << SENSOR_GPS)
+ ((_lidarFault > FAULT_NONE) << SENSOR_LIDAR)
+ ((_flowFault > FAULT_NONE) << SENSOR_FLOW)
+ ((_sonarFault > FAULT_NONE) << SENSOR_SONAR)
+ ((_visionFault > FAULT_NONE) << SENSOR_VISION)
+ ((_mocapFault > FAULT_NONE) << SENSOR_MOCAP);
_pub_est_status.get().timeout_flags =
(_baroInitialized << SENSOR_BARO)
+ (_gpsInitialized << SENSOR_GPS)
+ (_flowInitialized << SENSOR_FLOW)
+ (_lidarInitialized << SENSOR_LIDAR)
+ (_sonarInitialized << SENSOR_SONAR)
+ (_visionInitialized << SENSOR_VISION)
+ (_mocapInitialized << SENSOR_MOCAP);
_pub_est_status.update();
}
///
/// @par Detailed description
/// ...
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
std::vector<float>
sascalc::Prop::
calc_com(int &frame)
{
if(_total_mass() <= 0.0) {
calc_mass() ;
std::cout << "com tot mass = " << _total_mass() << std::endl ;
}
float comx = 0.0, comy = 0.0, comz = 0.0 ;
try
{
comx = (_atom_mass()*_x().col(frame)).sum() / _total_mass() ;
comy = (_atom_mass()*_y().col(frame)).sum() / _total_mass() ;
comz = (_atom_mass()*_z().col(frame)).sum() / _total_mass() ;
}
catch(std::overflow_error e) {
std::cout << "failed in calc_com" << std::endl ;
std::cout << "total_mass = " << _total_mass() << std::endl ;
std::cout << e.what() << " -> ";
std::vector<float> com = {comx, comy, comz} ;
return com ;
}
std::vector<float> com = {comx, comy, comz} ;
return com ;
}
/**
* @brief Definition of (possibly non-linear) measurement function
*
* This function maps the system state to the measurement that is expected
* to be received from the sensor assuming the system is currently in the
* estimated state.
*
* @param [in] x The system state in current time-step
* @returns The (predicted) sensor measurement for the system state
*/
M h(const S& _x) const
{
M measurement;
// Vehicle measurement vector (x,y, theta, theta_dot)-vector
// This uses the Eigen template method to get the first 4 elements of the vector
// for now we are considering a linear observation model
// moreover, we already get our measures as we expect to be
measurement.Z_x() = _x(0); // position along the x axis
measurement.Z_y() = _x(1); // position along the y axis
measurement.Z_theta() = _x(4); // heading of the vehicle
measurement.Z_theta_dot() = _x(5); // yaw rate, i.e. rotational velocity of the vehicle
return measurement;
}
int ZMatLUSolver_NR3::solve( double *B, double *x ) {
// solve Ax = B, must call decompose() first.
VecDoub b( rows, B, 1 );
VecDoub _x( cols, x, 1 );
pNRlu->solve( b, _x );
return 1;
}
void
cardinality(Home home, SetVar x, unsigned int i, unsigned int j) {
Set::Limits::check(i, "Set::cardinality");
Set::Limits::check(j, "Set::cardinality");
if (home.failed()) return;
Set::SetView _x(x);
GECODE_ME_FAIL(_x.cardMin(home, i));
GECODE_ME_FAIL(_x.cardMax(home, j));
}