void Filter::setCoordinates (ComplexVector& zeros, ComplexVector& poles) { this->zeros->clear(); delete this->zeros; this->poles->clear(); delete this->poles; this->zeros = &zeros; this->poles = &poles; delete[] zeroBuffer.buffer; delete[] zeroBuffer.coefficients; delete[] poleBuffer.buffer; delete[] poleBuffer.coefficients; zeroBuffer.position = 0; zeroBuffer.size = (int)zeros.size() + 1; zeroBuffer.buffer = new float[zeroBuffer.size]; zeroBuffer.coefficients = getCoefficients(1, zeros); poleBuffer.position = 0; poleBuffer.size = (int)poles.size() + 1; poleBuffer.buffer = new float[poleBuffer.size]; poleBuffer.coefficients = getCoefficients(1, poles); }
//------------------------------------------------------------------------------ // serialize() -- print the value of this object to the output stream sout. //------------------------------------------------------------------------------ std::ostream& Polynomial::serialize(std::ostream& sout, const int i, const bool slotsOnly) const { int j = 0; if (!slotsOnly) { sout << "( " << getFactoryName() << std::endl; j = 4; } int mm = getDegree() + 1; if (mm > 0) { const double* aa = getCoefficients(); indent(sout,i+j); sout << "coefficients: [ "; for (int i = 0; i < mm; i++) { std::cout << aa[i] << " "; } sout << " ]" << std::endl; } BaseClass::serialize(sout, i + j, true); if (!slotsOnly) { indent(sout, i); sout << ")" << std::endl; } return sout; }
//================================================================== Filter::Filter(int sampleRate) { this->sampleRate = sampleRate; zeros = new ComplexVector(); poles = new ComplexVector(); zeroBuffer.position = 0; zeroBuffer.size = 1; zeroBuffer.buffer = new float[zeroBuffer.size]; zeroBuffer.coefficients = getCoefficients(1, *zeros); poleBuffer.position = 0; poleBuffer.size = 1; poleBuffer.buffer = new float[poleBuffer.size]; poleBuffer.coefficients = getCoefficients(1, *poles); }
/** * @brief It segments a cloud using the planes and boundaries previously calculated. A point is considered to be part of a valid object if it is above the plane, * inside the limits of the planes and it is not part of any of the planes. * * @param cloud Point cloud to segment. * @param [out] clusterIndices Valid indices after the segmentation. */ void MultiplePlaneSegmentation::segment(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud, std::vector<pcl::PointIndices> &clusterIndices) { std::vector<pcl::ModelCoefficients> coefficients; getCoefficients(coefficients); std::vector<std::vector<pcl::PointXYZRGBA>> boundaries; getBoundaries(boundaries); // Cloud containing the points without the planes. pcl::PointCloud<pcl::PointXYZRGBA>::Ptr remainingCloud = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>(*cloud)); // -1 -> part of a plane, 0 -> not part of an object, 1 -> part of an object. std::vector<char> mask = std::vector<char>(cloud->points.size(), 0); assert(coefficients.size() == boundaries.size()); for(int i = 0; i < coefficients.size(); i++) { Eigen::Vector4f planeCoef = Eigen::Vector4f(coefficients[i].values.data()); std::vector<pcl::PointXYZRGBA> planeBoundary = boundaries[i]; #pragma omp parallel for firstprivate(planeCoef, planeBoundary) shared(cloud, mask) num_threads(4) for(size_t j = 0; j < cloud->points.size(); j++) { // Calculate the distance from the point to the plane normal as the dot product // D =(P-A).N/|N| // If the x value of the pointcloud or it is marked as a point in a plane it is not needed to // make further calculations, we don't want this point. if(isnan(cloud->points[j].x) or mask[j] == -1) continue; Eigen::Vector4f pt(cloud->points[j].x, cloud->points[j].y, cloud->points[j].z, 1); float distance = planeCoef.dot(pt); if (distance >= -0.02) { if (isInlier(cloud, j , planeBoundary, planeCoef)) { if (distance <= 0.02) { // If the point is at a distance less than X, then the point is in the plane, we mark it properly. mask[j] = -1; } else { // The point is not marked as being part of an object nor plane, if it is above it we mark it as object. mask[j] = 1; } } } } } // Parse inliers. pcl::PointIndices::Ptr inliers = pcl::PointIndices::Ptr(new pcl::PointIndices()); inliers->indices.resize(cloud->points.size()); int nr_p = 0; for(int i = 0; i < mask.size(); i++) { if(mask[i] == 1) inliers->indices[nr_p++] = i; } inliers->indices.resize(nr_p); // Clustering clusterIndices = std::vector<pcl::PointIndices>(); clustering(cloud, inliers, 0.03, 200, clusterIndices); }
void Filter::addPole (Complex coordinate) { poles->push_back(coordinate); delete[] poleBuffer.buffer; delete[] poleBuffer.coefficients; poleBuffer.position = 0; poleBuffer.size = (int)poles->size() + 1; poleBuffer.buffer = new float[poleBuffer.size]; poleBuffer.coefficients = getCoefficients(1, *poles); }
void Filter::addZero (Complex coordinate) { zeros->push_back(coordinate); delete[] zeroBuffer.buffer; delete[] zeroBuffer.coefficients; zeroBuffer.position = 0; zeroBuffer.size = (int)zeros->size() + 1; zeroBuffer.buffer = new float[zeroBuffer.size]; zeroBuffer.coefficients = getCoefficients(1, *zeros); }
std::vector<double> ParametersInterpreter<NMSmodelT>::getSubjectParameters() const{ std::vector<double> x; for (typename ParametersMap::const_iterator it(parameters_.begin()); it != parameters_.end(); ++it) { std::vector<double> coefficients, groupedCoefficients; getCoefficients(it->first, coefficients); groupValues(it->second.muscleGroups, coefficients, groupedCoefficients); x.insert(x.end(), groupedCoefficients.begin(), groupedCoefficients.end()); } return x; }
void ParametersInterpreter<NMSmodelT>::getUpperLowerBounds(std::vector<double>& upperBounds, std::vector<double>& lowerBounds) const { upperBounds.clear(); lowerBounds.clear(); for (typename ParametersMap::const_iterator it(parameters_.begin()); it != parameters_.end(); ++it) { if (it->second.boundaryType == Parameter::RelativeToSubjectValue) { std::vector<double> coefficients, groupedCoefficients; getCoefficients(it->first, coefficients); groupValues(it->second.muscleGroups, coefficients, groupedCoefficients); for (std::vector<double>::const_iterator cIt(groupedCoefficients.begin()); cIt != groupedCoefficients.end(); ++cIt) { double upperBoundValue = *cIt * (it->second.upperLimit); double lowerBoundValue = *cIt * (it->second.lowerLimit); upperBounds.push_back(upperBoundValue); lowerBounds.push_back(lowerBoundValue); } } else { upperBounds.insert(upperBounds.end(), it->second.size, it->second.upperLimit); lowerBounds.insert(lowerBounds.end(), it->second.size, it->second.lowerLimit); } } }
void ParametersInterpreter<NMSmodelT>::setSubjectParameters(const std::vector<double>& x) { unsigned count = 0; for (typename ParametersMap::const_iterator it(parameters_.begin()); it != parameters_.end(); ++it) { unsigned noCoefficients = it->second.size; std::vector<double>::const_iterator startIt(x.begin() + count); std::vector<double>::const_iterator endIt(x.begin() + count + noCoefficients); std::vector<double> groupedCoefficients(startIt, endIt); std::vector<double> coefficients; getCoefficients(it->first, coefficients); distributeValues(it->second.muscleGroups, coefficients, groupedCoefficients); setCoefficients(it->first, coefficients); count += noCoefficients; } }
/** * \brief This function initializes the hardware * * * \details Sensors are set up, polynomials are calculated and other general preparations are made */ void setup(void) { initBluetoothStorage();//initialize space for variables used for Bluetooth Communication //-------------------Dave Setup--------------------- DAVE_STATUS_t status; status = DAVE_Init();//Initialization of DAVE APPs if (status == DAVE_STATUS_FAILURE) { /* Placeholder for error handler code. The while loop below can be replaced with an user error handler. */ XMC_DEBUG("DAVE APPs initialization failed\n"); while (1U); } disableIRQ();//disables all Interrupts delay(2000);//waits 2000ms enableIRQ();//enables configurated Interrupts setup_STATE_LEDs();//setup Status-LED's WatchRC_Init();//initialize RC-Watchdog setup_UART_Trigger_limits();//setup Trigger-Limits of UART-FIFO PressureFIR = Initialize_FIR_Filter(PressureFIR, MOVING_AVERAGE);//initialize FIR Filter setupDPS310I2C();//initialize DPS310 getCoefficients();//get Coefficients of DPS310 setupDPS310();//setup DPS Hardware MPU9150_Setup();//configures the IMU delay(3000);//wait 3000ms to wait for ESC's to startup // initialize controller polynomials polynoms.b_roll[0]=parameter.P_roll-parameter.I_roll*parameter.T-parameter.P_roll*parameter.N_roll*parameter.T+parameter.N_roll*parameter.I_roll*parameter.T*parameter.T+parameter.D_roll*parameter.N_roll; polynoms.b_roll[1]=parameter.I_roll*parameter.T-2*parameter.P_roll+parameter.P_roll*parameter.N_roll*parameter.T-2*parameter.D_roll*parameter.N_roll; polynoms.b_roll[2]=parameter.P_roll+parameter.D_roll*parameter.N_roll; polynoms.a_roll[0]=1-parameter.N_roll*parameter.T; polynoms.a_roll[1]=parameter.N_roll*parameter.T-2; polynoms.b_pitch[0]=parameter.P_pitch-parameter.I_pitch*parameter.T-parameter.P_pitch*parameter.N_pitch*parameter.T+parameter.N_pitch*parameter.I_pitch*parameter.T*parameter.T+parameter.D_pitch*parameter.N_pitch; polynoms.b_pitch[1]=parameter.I_pitch*parameter.T-2*parameter.P_pitch+parameter.P_pitch*parameter.N_pitch*parameter.T-2*parameter.D_pitch*parameter.N_pitch; polynoms.b_pitch[2]=parameter.P_pitch+parameter.D_pitch*parameter.N_pitch; polynoms.a_pitch[0]=1-parameter.N_pitch*parameter.T; polynoms.a_pitch[1]=parameter.N_pitch*parameter.T-2; TIMER_Start(&Control_Timer);//start Timer for Controller }
void gradientAt(double const * p, double * result) const { TheaArray<double> const & coeffs = getCoefficients(); if (!coeffs.empty()) fastCopy(&coeffs[0], &coeffs[0] + coeffs.size(), result); }
/*Wrapper to get directly the coeffs in the Chebyshev basis from a polynomial in the monomial basis given by a *node We return in n = deg(f)+1; */ void getChebCoeffsFromPolynomial(sollya_mpfi_t**coeffs, int *n, node *f, sollya_mpfi_t x, mp_prec_t prec){ sollya_mpfi_t z1, z2, ui, vi; node **coefficients; int d,i; sollya_mpfi_t *p, *c; mpfr_t u,v; if (isPolynomial(f) ){ getCoefficients(&d, &coefficients, f); *n=d+1; *coeffs= (sollya_mpfi_t *)safeMalloc((d+1)*sizeof(sollya_mpfi_t)); p=safeMalloc((d+1)*sizeof(sollya_mpfi_t)); c=safeMalloc((d+1)*sizeof(sollya_mpfi_t)); for (i=0;i<d+1;i++){ sollya_mpfi_init2((*coeffs)[i],prec); sollya_mpfi_init2(p[i],prec); sollya_mpfi_init2(c[i],prec); if (coefficients[i]!= NULL) mpfi_set_node(p[i],coefficients[i], prec); else sollya_mpfi_set_ui(p[i],0); } for (i=0;i<d+1;i++) { if (coefficients[i] != NULL) free_memory(coefficients[i]); } safeFree(coefficients); /*Here we have the coeffs of the polynomial in p, over the interval x=[a,b]*/ /*we need to compute the polynomial over [-1,1]*/ /*we make the change of variable: x= y*(b-a)/2 + (b+a)/2, hence for y \in [-1,1] we have x\in [a,b]*/ /* we compute P(x)=Q(y)*/ sollya_mpfi_init2(ui, prec); sollya_mpfi_init2(vi, prec); mpfr_init2(u, prec); mpfr_init2(v, prec); sollya_mpfi_init2(z1, prec); sollya_mpfi_init2(z2, prec); sollya_mpfi_get_left(u,x); sollya_mpfi_get_right(v,x); sollya_mpfi_set_fr(ui,u); sollya_mpfi_set_fr(vi,v); sollya_mpfi_add(z2,ui,vi); sollya_mpfi_sub(z1,vi,ui); sollya_mpfi_div_ui(z1,z1,2); sollya_mpfi_div_ui(z2,z2,2); getTranslatedPolyCoeffs(c, p, d+1, z1,z2); getPolyCoeffsChebBasis(*coeffs, c, d+1); /*cleaning*/ for (i=0;i<d+1;i++){ sollya_mpfi_clear(p[i]); sollya_mpfi_clear(c[i]); } safeFree(p); safeFree(c); sollya_mpfi_clear(ui); sollya_mpfi_clear(vi); mpfr_clear(u); mpfr_clear(v); sollya_mpfi_clear(z1); sollya_mpfi_clear(z2); } else{ printMessage(1,SOLLYA_MSG_ERROR_IN_CHEBYSHEVFORM_NOT_A_POLYNOMIAL, "The given function is not a polynomial, no modification is made.\n"); } }