Example #1
0
///
/// @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) );
    }
Example #3
0
///
/// @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 ;
}
Example #4
0
  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)) );
  }
Example #5
0
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;
}
Example #7
0
/* ...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);
}
Example #8
0
///
/// @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 ;
}
Example #9
0
File: touch.c Project: cjgd/cutils
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); 
}
Example #10
0
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(',')
        ;
}
Example #11
0
  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;
    }
Example #14
0
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);
	}

}
Example #15
0
///
/// @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;
    }
Example #17
0
  void testStrDup() {
    WINWCHAR a[] = { _x('a'), _x('b'), _x('c'), 0 };

    WINWCHAR *b = WinWStrDupFromWinWStr(a);

    CPPUNIT_ASSERT_EQUAL( 0, WinWStrCmp(a, b) );

    free(b);
  }
Example #18
0
  void testStrDup() {
    WCHAR a[] = { _x('a'), _x('b'), _x('c'), 0 };

    WCHAR *b = _wcsdup(a);

    CPPUNIT_ASSERT_EQUAL( 0, wcscmp(a, b) );

    delete [] b;
  }
Example #19
0
///
/// @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 ;
}
Example #20
0
  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);
  }
Example #21
0
 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
   }
 }
Example #22
0
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'}}
}
Example #24
0
/* ...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;
}
Example #25
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();
}
Example #27
0
///
/// @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;
    }
Example #29
0
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));
 }