C++ DblVector::size方法代码示例

// x given
int FitLine(const DblVector& PointsY, const DblVector& PointsX, double& a, double& b)
{
	assert(PointsX.size()==PointsY.size());
	DblMatrix S;
	S.Assign(3, 2, 0.0);
	for(unsigned int i=0; i<PointsX.size(); i++)
	{
		S[0][0] += PointsX[i] * PointsX[i];
		S[0][1] += PointsX[i];
		S[0][2] += PointsX[i] * PointsY[i];
		S[1][2] += PointsY[i]; 
	}
	S[1][1] = (double)PointsX.size();
	S.Diagonalize();
	DblVector Solutions;
	int Ret = S.SolveLinearCramer(Solutions);
	if(Ret) 
	{
		a = Solutions[0];
		b = Solutions[1];
	}
	else std::cout << "Error in line fitting" << std::endl;
	
	return Ret;
}

int FitLineIt(const DblVector& PointsY, const DblVector& PointsX,
			  double& a, double& b, int It, double ErrWidth)
{
	// base case
	if(It==0) return 0;
	std::cout << "Line fitting no samples: " << PointsY.size() << std::endl;

	// fitting
	FitLine(PointsY, PointsX, a, b);

	double Error = 0.0;
	for(int i=0; i<(int)PointsY.size(); i++)
	{
		double Val0 = a*PointsX[i]+b;
		double Val1 = PointsY[i]; 
		Error += dblsqr(Val1-Val0);
		//std::cout << Val1 << " " << Val0 << std::endl;
	}
	Error /= (double)PointsY.size();
	Error = sqrt(Error);

	DblVector px, py; int Cnt=0;
	for(int k=0; k<(int)PointsX.size(); k++)
	{
		double Val = a*PointsX[k]+b;
		double Dist = sqrt(dblsqr(PointsY[k] - Val));
		if(Dist<Error*ErrWidth)
		{
			px.push_back(PointsX[k]);
			py.push_back(PointsY[k]);
			Cnt++;
		}
	}
	return FitLineIt(py, px, a, b, It-1, ErrWidth);
}

int FitQuadraticSurfaceIt(const DblVector& PX, const DblVector& PY,
							const DblVector& PZ, DblVector& Parameters,
							int It, double ErrWidth)
{

	// base case
	if(It==0) return 0;
	std::cout << "Quad surface fitting no samples: " << PX.size() << std::endl;

	// fitting
	FitQuadraticSurface(PX, PY, PZ, Parameters);
	double Error = GetQuadraticSurfaceError(PX, PY, PZ, Parameters);
	
	DblVector px, py, pz; int Cnt=0;
	for(int k=0; k<(int)PX.size(); k++)
	{
		double Val = QuadraticSurfaceVal(PX[k], PY[k], Parameters);
		double Dist = sqrt((PZ[k] - Val) * (PZ[k] - Val));
		if(Dist<Error*ErrWidth)
		{
			px.push_back(PX[k]);
			py.push_back(PY[k]);
			pz.push_back(PZ[k]);
			Cnt++;
		}
		
	}
	return FitQuadraticSurfaceIt(px, py, pz, Parameters, It-1, ErrWidth);
}

// x given 
int FitParabola(const DblVector& PointsY, const DblVector& PointsX, double& a, double& b, double& c)
{
	// fit parabola
	assert(PointsX.size()==PointsY.size());
	DblMatrix S;
	S.Assign(4, 3, 0.0);
	for(unsigned int i=0; i<PointsX.size(); i++)
	{
		S[0][0] += PointsX[i] * PointsX[i] * PointsX[i] * PointsX[i];
		S[0][1] += PointsX[i] * PointsX[i] * PointsX[i];
		S[0][2] += PointsX[i] * PointsX[i];
		S[0][3] += PointsX[i] * PointsX[i] * PointsY[i];
		S[1][2] += PointsX[i];
		S[1][3] += PointsX[i] * PointsY[i];
		S[2][3] += PointsY[i];
		
	}
	S[1][1] = S[0][2];
	S[2][2] = (double)PointsX.size();
	S.Diagonalize();
	DblVector Solutions;
	int Ret = S.SolveLinearCramer(Solutions);
	if(Ret==-1) return -1;
	else
	{
		a = Solutions[0];
		b = Solutions[1];
		c = Solutions[2];
	}
	return Ret;
}

double GetQuadraticSurfaceError(const DblVector& PX, const DblVector& PY,
							const DblVector& PZ, DblVector& Parameters)
{
	double Sum=0.0;
	for(int i=0; i<(int)PX.size(); i++)
	{
		double Val = QuadraticSurfaceVal(PX[i], PY[i], Parameters);
		Sum += (PZ[i] - Val) * (PZ[i] - Val);
	}
	return sqrt(Sum/(double)PX.size());
}

// fit a two dimensional quadratic surface
int FitQuadraticSurface(const DblVector& PX, const DblVector& PY,
							const DblVector& PZ, DblVector& Parameters)
{
	// fit parabola
	assert(PX.size()==PY.size());
	DblMatrix S;
	S.Assign(7, 6, 0.0);
	for(unsigned int i=0; i<PX.size(); i++)
	{
		double X2 = PX[i] * PX[i];
		double Y2 = PY[i] * PY[i];
		double X3 = X2 * PX[i];
		double Y3 = Y2 * PY[i];
		double X4 = X3 * PX[i];
		double Y4 = Y3 * PY[i];
		
		S[0][1] += PY[i];
		S[1][1] += Y2;
		
		S[0][2] += PX[i];
		S[1][2] += PX[i]*PY[i];
		S[2][2] += X2;

		S[0][3] += PX[i]*PY[i];
		S[1][3] += PX[i]*Y2;
		S[2][3] += X2*PY[i];
		S[3][3] += X2*Y2;

		S[0][4] += Y2;
		S[1][4] += Y3;
		S[2][4] += PX[i]*Y2;
		S[3][4] += PX[i]*Y3;
		S[4][4] += Y4;

		S[0][5] += X2;
		S[1][5] += X2*PY[i];
		S[2][5] += X3;
		S[3][5] += X3*PY[i];
		S[4][5] += X2*Y2;
		S[5][5] += X4;

		// function values
		S[0][6]+=PZ[i];
		S[1][6]+=PY[i]*PZ[i];
		S[2][6]+=PX[i]*PZ[i];
		S[3][6]+=PX[i]*PY[i]*PZ[i];
		S[4][6]+=Y2*PZ[i];
		S[5][6]+=X2*PZ[i];

	}
	S[0][0] = (double)PX.size();
	S.Diagonalize();
	return S.SolveLinearCramer(Parameters);
}

SortedVector::SortedVector(const DblVector& v)
{
	clear();
	for(unsigned int i=0; i<v.size(); i++)
		push_back(SortStruct(v[i], i));
	std::sort(begin(), end(), SortStructGreater);
}

void DblVector::MulElem(const DblVector& B, DblVector& Res) const
{
	assert(this->size()==B.size());
	Res.clear();
	for(unsigned int i=0; i<this->size(); i++)
		Res.push_back((*this)[i]*B[i]);
}

unsigned long FitNormalizedPolynomial(const DblVector& y, const DblVector& x, int degree, double* coefficients, double* max, double* min)
{

	if(y.size() != x.size())
	{
		std::cerr << "ipa_Utils::FitPolynomial: Error" << std::endl;
		std::cerr << "\t ... vector 'y' and vector 'x' must have same size." << std::endl;
		return ipa_Utils::RET_FAILED;
	}

	/// Extract min and max from 32bit distance values
	/// to scale x-values
	(*min)=DBL_MAX;
	(*max)=-DBL_MAX;
	for(unsigned int k=0; k<x.size(); k++)
	{
		double d = x[k];
		if(d < (*min)) (*min)=d;
		if(d > (*max)) (*max)=d;
	}
	double diffMaxMin = (*max) - (*min);

	/// Normalize data points, to assert numeric stability during Matrix inversion
	std::vector<double> y_Normalized;
	std::vector<double> x_Normalized;
	for(unsigned int k=0; k<x.size(); k++)
	{
		x_Normalized.push_back((x[k]-(*min))/diffMaxMin); 
		y_Normalized.push_back(y[k]/diffMaxMin);
	}

	/// Compute normalized polynomial coefficients through least squares fitting
	double* c;
	c = Wm4::PolyFit2<double>((int)x.size(), &x_Normalized[0], &y_Normalized[0], degree);

	for (int i = 0; i <= degree; i++)
	{
		coefficients[i] = c[i];
	}

	delete[] c;

	return ipa_Utils::RET_OK;
}

unsigned long FitPolynomial(const DblVector& y, const DblVector& x, int degree, double* coefficients)
{

	if(y.size() != x.size())
	{
		std::cerr << "ipa_Utils::FitPolynomial: Error" << std::endl;
		std::cerr << "\t ... vector 'y' and vector 'x' must have same size." << std::endl;
		return ipa_Utils::RET_FAILED;
	}

	/// Compute normalized polynomial coefficients through least squares fitting
	double* c;
	c = Wm4::PolyFit2<double>((int)x.size(), &x[0], &y[0], degree);

	for (int i = 0; i <= degree; i++)
	{
		coefficients[i] = c[i];
	}
	delete[] c;

	return ipa_Utils::RET_OK;
}

void DblVector::MulElemTo(const DblVector& B)
{
	assert(this->size()==B.size());
	for(unsigned int i=0; i<this->size(); i++)
		(*this)[i] *= B[i];
}

void DblVector::SubFromVec(const DblVector& B)
{
	assert(this->size()==B.size());
	for(unsigned int i=0; i<this->size(); i++)
		(*this)[i] -= B[i];
}

bool abs_min_linear(
    size_t            level   ,
    size_t            n       ,
    size_t            m       ,
    size_t            s       ,
    const DblVector&  g_hat   ,
    const DblVector&  g_jac   ,
    const DblVector&  bound   ,
    const DblVector&  epsilon ,
    const SizeVector& maxitr  ,
    DblVector&        delta_x )
// END PROTOTYPE
{   using std::fabs;
    bool ok    = true;
    double inf = std::numeric_limits<double>::infinity();
    //
    CPPAD_ASSERT_KNOWN(
        level <= 4,
        "abs_min_linear: level is not less that or equal 4"
    );
    CPPAD_ASSERT_KNOWN(
        size_t(epsilon.size()) == 2,
        "abs_min_linear: size of epsilon not equal to 2"
    );
    CPPAD_ASSERT_KNOWN(
        size_t(maxitr.size()) == 2,
        "abs_min_linear: size of maxitr not equal to 2"
    );
    CPPAD_ASSERT_KNOWN(
        m == 1,
        "abs_min_linear: m is not equal to 1"
    );
    CPPAD_ASSERT_KNOWN(
        size_t(delta_x.size()) == n,
        "abs_min_linear: size of delta_x not equal to n"
    );
    CPPAD_ASSERT_KNOWN(
        size_t(bound.size()) == n,
        "abs_min_linear: size of bound not equal to n"
    );
    CPPAD_ASSERT_KNOWN(
        size_t(g_hat.size()) == m + s,
        "abs_min_linear: size of g_hat not equal to m + s"
    );
    CPPAD_ASSERT_KNOWN(
        size_t(g_jac.size()) == (m + s) * (n + s),
        "abs_min_linear: size of g_jac not equal to (m + s)*(n + s)"
    );
    CPPAD_ASSERT_KNOWN(
        size_t(bound.size()) == n,
        "abs_min_linear: size of bound is not equal to n"
    );
    if( level > 0 )
    {   std::cout << "start abs_min_linear\n";
        CppAD::abs_print_mat("bound", n, 1, bound);
        CppAD::abs_print_mat("g_hat", m + s, 1, g_hat);
        CppAD::abs_print_mat("g_jac", m + s, n + s, g_jac);

    }
    // partial y(x, u) w.r.t x (J in reference)
    DblVector py_px(n);
    for(size_t j = 0; j < n; j++)
        py_px[ j ] = g_jac[ j ];
    //
    // partial y(x, u) w.r.t u (Y in reference)
    DblVector py_pu(s);
    for(size_t j = 0; j < s; j++)
        py_pu[ j ] = g_jac[ n + j ];
    //
    // partial z(x, u) w.r.t x (Z in reference)
    DblVector pz_px(s * n);
    for(size_t i = 0; i < s; i++)
    {   for(size_t j = 0; j < n; j++)
        {   pz_px[ i * n + j ] = g_jac[ (n + s) * (i + m) + j ];
        }
    }
    // partial z(x, u) w.r.t u (L in reference)
    DblVector pz_pu(s * s);
    for(size_t i = 0; i < s; i++)
    {   for(size_t j = 0; j < s; j++)
        {   pz_pu[ i * s + j ] = g_jac[ (n + s) * (i + m) + n + j ];
        }
    }
    // initailize delta_x
    for(size_t j = 0; j < n; j++)
        delta_x[j] = 0.0;
    //
    // value of approximation for g(x, u) at current delta_x
    DblVector g_tilde = CppAD::abs_eval(n, m, s, g_hat, g_jac, delta_x);
    //
    // value of sigma at delta_x = 0; i.e., sign( z(x, u) )
    CppAD::vector<double> sigma(s);
    for(size_t i = 0; i < s; i++)
        sigma[i] = CppAD::sign( g_tilde[m + i] );
    //
    // current set of cutting planes
    DblVector C(maxitr[0] * n), c(maxitr[0]);
    //
    //
    size_t n_plane = 0;
//.........这里部分代码省略.........