C++ vector_double类代码示例

void harp::spec_desisim::values ( vector_double & data ) const {

  data.resize ( nglobal_ );
  data.clear();

  fitsfile * fp;

  fits::open_read ( fp, path_ );

  // read the object flux

  fits::img_seek ( fp, objhdu_ );

  fits::img_read ( fp, data, false );

  // read the sky flux and sum

  vector_double skyflux ( data.size() );

  fits::img_seek ( fp, skyhdu_ );

  fits::img_read ( fp, skyflux, false );

  fits::close ( fp );

  for ( size_t i = 0; i < data.size(); ++i ) {
    data[i] += skyflux[i];
  }

  return;
}

void harp::spec_sim::inv_variance ( vector_double & data ) const {

  data.resize ( size_ );
  data.clear();

  return;
}

/*---------------------------------------------------------------
	Fills in the representative sector
		field in the gap structure:
  ---------------------------------------------------------------*/
void  CHolonomicND::calcRepresentativeSectorForGap(
	TGap					&gap,
	const poses::CPoint2D	&target,
	const vector_double		&obstacles)
{
    int     sector;
    int     sectors_to_be_wide = round( WIDE_GAP_SIZE_PERCENT * obstacles.size());
	int		TargetSector = direction2sector( atan2(target.y(),target.x()), obstacles.size() );

    if ( (gap.end-gap.ini) < sectors_to_be_wide )
    {
#if	1
		sector = round(0.5f*gap.ini+0.5f*gap.end);
#else
		double	min_dist_obs_near_ini=1, min_dist_obs_near_end=1;
		int		i;
		for ( i= gap.ini;i>=max(0,gap.ini-2);i--)
			min_dist_obs_near_ini = min(min_dist_obs_near_ini, obstacles[i]);
		for ( i= gap.end;i<=min((int)obstacles.size()-1,gap.end+2);i++)
			min_dist_obs_near_end = min(min_dist_obs_near_end, obstacles[i]);
		sector = round((min_dist_obs_near_ini*gap.ini+min_dist_obs_near_end*gap.end)/(min_dist_obs_near_ini+min_dist_obs_near_end));
#endif
    }
    else
    {
        // Para gaps anchos que NO contengan al target, cerca del borde
        //  mas cercano a este:
		//if ( TargetSector < gap.ini || TargetSector > gap.end )
  //      {
            int     dir;
            int     dist_ini = abs( TargetSector - gap.ini );
            int     dist_end = abs( TargetSector - gap.end );
            if (dist_ini<dist_end) {
                    sector = gap.ini;
                    dir = +1; }
            else {
                    sector = gap.end;
                    dir = -1; }
            sector = sector + dir * sectors_to_be_wide/2 ;
    //    }
    //    else
    //    {
    //        // Es un valle ancho con el Target dentro:
    //        // Buscar la maxima "distance" en un rango cerca del target:
    //        int     ini = max( gap.ini, TargetSector - sectors_to_be_wide / 2 );
    //        int     end = min( TargetSector + sectors_to_be_wide / 2, gap.end);

    //        sector = TargetSector;
    //        for (int i = ini;i<=end;i++)
				//if ( obstacles[i] > obstacles[sector] )
				//	sector = i;
    //    }
    }

	keep_max(sector, 0);
	keep_min(sector, (int)obstacles.size()-1);

    gap.representative_sector = sector;
}

/*---------------------------------------------------------------
		getAsVector
---------------------------------------------------------------*/
void CPose2D::getAsVector(vector_double &v) const
{
	v.resize(3);
	v[0]=m_coords[0];
	v[1]=m_coords[1];
	v[2]=m_phi;
}

void fixGPStimestamp(CObservationGPSPtr &obs, vector_double &time_changes, std::map<std::string,double> &DeltaTimes )
{
	if (!obs->has_GGA_datum && !obs->has_RMC_datum) return;

	CObservationGPS::TUTCTime	theTime;
	bool  hasTime=false;

	if (obs->has_GGA_datum && obs->GGA_datum.fix_quality>0)
	{
		theTime = obs->GGA_datum.UTCTime;
		hasTime = true;
	}
	else
	if (obs->has_RMC_datum && obs->RMC_datum.validity_char=='A' )
	{
		theTime = obs->RMC_datum.UTCTime;
		hasTime = true;
	}

	 // The last known delta_time for this sensor name
	if (DeltaTimes.find( obs->sensorLabel )==DeltaTimes.end())
		DeltaTimes[obs->sensorLabel] = 0;

	double &DeltaTime = DeltaTimes[obs->sensorLabel];

	if ( hasTime )
	{
		TTimeParts	timparts;
		mrpt::system::timestampToParts( obs->timestamp, timparts);

		DeltaTime  = 3600*theTime.hour + 60*theTime.minute  + theTime.sec;
		DeltaTime -= 3600*timparts.hour + 60*timparts.minute + timparts.second;

		if (theTime.hour < timparts.hour-2)
		{
			// The GPS time is one day ahead the "timestamp"
			DeltaTime += 3600*24;
		}
		else if (timparts.hour > theTime.hour+2)
		{
			// The "timstamp" is one day ahead the GPS time:
			DeltaTime -= 3600*24;
		}

		// Instead of delta, just replace:
		timparts.hour 		= theTime.hour;
		timparts.minute 	= theTime.minute;
		timparts.second 	= theTime.sec;

		obs->timestamp = buildTimestampFromParts(timparts);
	}
	else
	{
		// Use last delta
		obs->timestamp += mrpt::system::secondsToTimestamp(DeltaTime);
	}

	// Fix timestamp:
	time_changes.push_back( DeltaTime );
}

// The error function F(x):
void myFunction( const vector_double &x, const vector_double &y, vector_double &out_f)
{
	out_f.resize(1);

	// 1-cos(x+1) *cos(x*y+1)
	out_f[0] = 1 - cos(x[0]+1) * cos(x[0]*x[1]+1);
}

void ffff(const vector_double &x,const CQuaternionDouble &Q, vector_double &OUT)
{
	OUT.resize(3);
	CQuaternionDouble q(x[0],x[1],x[2],x[3]);
	q.normalize();
	q.rpy(OUT[2],OUT[1],OUT[0]);
}

void harp::eigen_decompose ( matrix_double const & invcov, vector_double & D, matrix_double & W, bool regularize ) {

  D.resize ( invcov.size1() );
  W.resize ( invcov.size1(), invcov.size2() );
  matrix_double temp ( invcov );

  int nfound;

  boost::numeric::ublas::vector < int > support ( 2 * invcov.size1() );

  boost::numeric::bindings::lapack::syevr ( 'V', 'A', boost::numeric::bindings::lower ( temp ), 0.0, 0.0, 0, 0, 0.0, nfound, D, W, support );

  if ( regularize ) {
    
    double min = 1.0e100;
    double max = -1.0e100;

    for ( size_t i = 0; i < D.size(); ++i ) {
      if ( D[i] < min ) {
        min = D[i];
      }
      if ( D[i] > max ) {
        max = D[i];
      }
    }

    double rcond = min / max;

    // pick some delta that is bigger than machine precision, but still tiny
    double epsilon = 10.0 * std::numeric_limits < double > :: epsilon();

    if ( rcond < epsilon ) {

      double reg = max * epsilon - min;
      //cerr << "REG offset = " << reg << " for min / max = " << min << " / " << max << endl;

      for ( size_t i = 0; i < D.size(); ++i ) {
        D[i] += reg;
      }

    }

  }

  return;
}

void harp::apply_inverse_norm ( vector_double const & S, vector_double & vec ) {

  for ( size_t i = 0; i < vec.size(); ++i ) {
    vec[i] /= S[i];
  }

  return;
}

void harp::spec_desisim::lambda ( vector_double & lambda_vals ) const {

  lambda_vals.resize ( nlambda_ );

  for ( size_t i = 0; i < nlambda_; ++i ) {
    lambda_vals[i] = crval + cdelt * (double)i;
  }

  return;
}

void harp::column_norm ( matrix_double const & mat, vector_double & S ) {

  S.resize( mat.size1() );
  S.clear();

  for ( size_t i = 0; i < mat.size2(); ++i ) {
    for ( size_t j = 0; j < mat.size1(); ++j ) {
      S[ j ] += mat( j, i );
    }
  }

  // Invert

  for ( size_t i = 0; i < S.size(); ++i ) {
    S[i] = 1.0 / S[i];
  }

  return;
}

/*---------------------------------------------------------------
					getHistogramNormalized
 ---------------------------------------------------------------*/
void CHistogram::getHistogramNormalized( vector_double &x, vector_double &hits ) const
{
	const size_t N = m_bins.size();
	linspace(m_min,m_max,N, x);

	hits.resize(N);
	const double K=m_binSizeInv/m_count;
	for (size_t i=0;i<N;i++)
		hits[i]=K*m_bins[i];
}

/** Returns a 1x7 vector with [x y z qr qx qy qz] */
void CPose3DQuat::getAsVector(vector_double &v) const
{
	v.resize(7);
	v[0] = m_coords[0];
	v[1] = m_coords[1];
	v[2] = m_coords[2];
	v[3] = m_quat[0];
	v[4] = m_quat[1];
	v[5] = m_quat[2];
	v[6] = m_quat[3];
}

void harp::sparse_mv_trans ( matrix_double_sparse const & AT, vector_double const & in, vector_double & out ) {

  // FIXME:  for now, we just use the (unthreaded) boost sparse matrix-vector product.  If this
  // operation dominates the cost in any way, we can add a threaded implementation here.

  size_t nrows = AT.size1();
  size_t ncols = AT.size2();

  if ( in.size() != nrows ) {
    std::ostringstream o;
    o << "length of input vector (" << in.size() << ") does not match number of rows in transposed matrix (" << nrows << ")";
    HARP_THROW( o.str().c_str() );
  }

  out.resize ( ncols );

  boost::numeric::ublas::axpy_prod ( in, AT, out, true );

  return;
}

/*---------------------------------------------------------------
	Fills in the representative sector
		field in the gap structure:
  ---------------------------------------------------------------*/
void  CHolonomicND::calcRepresentativeSectorForGap(
	TGap                        & gap,
	const mrpt::math::TPoint2D  & target,
	const vector_double         & obstacles)
{
	int sector;
	const unsigned int sectors_to_be_wide = round( options.WIDE_GAP_SIZE_PERCENT * obstacles.size());
	const unsigned int target_sector = direction2sector( atan2(target.y,target.x), obstacles.size() );

	if ( (gap.end-gap.ini) < sectors_to_be_wide )	//Select the intermediate sector
	{
#if	1
		sector = round(0.5f*gap.ini+0.5f*gap.end);
#else
		double	min_dist_obs_near_ini=1, min_dist_obs_near_end=1;
		int		i;
		for ( i= gap.ini;i>=max(0,gap.ini-2);i--)
			min_dist_obs_near_ini = min(min_dist_obs_near_ini, obstacles[i]);
		for ( i= gap.end;i<=min((int)obstacles.size()-1,gap.end+2);i++)
			min_dist_obs_near_end = min(min_dist_obs_near_end, obstacles[i]);
		sector = round((min_dist_obs_near_ini*gap.ini+min_dist_obs_near_end*gap.end)/(min_dist_obs_near_ini+min_dist_obs_near_end));
#endif
	}
	else	//Select a sector close to the target but spaced "sectors_to_be_wide/2" from it
	{
		unsigned int dist_ini = mrpt::utils::abs_diff(target_sector, gap.ini );
		unsigned int dist_end = mrpt::utils::abs_diff(target_sector, gap.end );

		if (dist_ini > 0.5*obstacles.size())
			dist_ini = obstacles.size() - dist_ini;
		if (dist_end > 0.5*obstacles.size())
			dist_end = obstacles.size() - dist_end;

		int dir;
		if (dist_ini<dist_end) {
				sector = gap.ini;
				dir = +1; }
		else {
				sector = gap.end;
				dir = -1; }

		sector = sector + dir*static_cast<int>(sectors_to_be_wide)/2;
	}

	keep_max(sector, 0);
	keep_min(sector, static_cast<int>(obstacles.size())-1 );

	gap.representative_sector = sector;
}