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");
}
}
