viennacl::vector<T>
element_prod(vector_base<T> const & v1, vector_expression<const V2, const V3, OP> const & proxy)
{
viennacl::vector<T> temp = proxy;
temp = element_prod(v1, temp);
return temp;
}
viennacl::vector<T>
element_prod(vector_expression<const V1, const V2, OP> const & proxy, vector_base<T> const & v2)
{
viennacl::vector<T> temp = proxy;
temp = element_prod(temp, v2);
return temp;
}
viennacl::vector<typename viennacl::result_of::cpu_value_type<V1>::type>
element_prod(vector_expression<const V1, const V2, OP1> const & proxy1,
vector_expression<const V3, const V4, OP2> const & proxy2)
{
typedef vector<typename viennacl::result_of::cpu_value_type<V1>::type> VectorType;
VectorType temp1 = proxy1;
VectorType temp2 = proxy2;
temp1 = element_prod(temp1, temp2);
return temp1;
}
ublas::vector<value_type>
heavysideFunction::operator()( nodes_type const& pointsOnRef ) const
{
ublas::vector<value_type> result( pointsOnRef.size2() );
std::fill( result.begin(), result.end(), 1.0 );
nodes_type pointsHat( transformToReal( pointsOnRef ) );
molecule_type::atoms_const_iterator_type atom( M_molecule->begin() );
int i;
if ( atom == M_molecule->end() ) return result;
node_type Ellipse( pbeqspace_type::Dim );
node_type point( pbeqspace_type::Dim );
value_type r, dr, dr2;
for ( i = 0; i < pointsOnRef.size2(); ++i )
{
point = element_prod( *M_stretch,column( pointsHat,i ) ) + ( *M_translation );
node_type mine( column( pointsHat,i ) );
//std::cout << "mine = " << mine(0) << " "<< mine(1) << " " << mine(2) << std::endl;
//std::cout << "point = " << point(0) << " "<< point(1) << " " << point(2) << std::endl;
for ( atom = M_molecule->begin(); atom != M_molecule->end(); ++atom )
{
Ellipse = point - atom->center();
r = norm_2( Ellipse );
if ( r <= atom->radius() - M_sW )
{
result( i ) = 0;
break;
}
if ( M_sW == 0 || r >= atom->radius() + M_sW )
continue;
dr = r - atom->radius() + M_sW;
dr2 = dr*dr;
result( i ) *= - M_4sW3 * ( dr*dr2 ) + M_34sW2 * dr2;
}
//std::cout << "result(" << i << ") = " << result(i) << std::endl;
}
return result;
}
ublas::vector<double> mvnormpdf(const ublas::matrix<double>& x, const ublas::vector<double>& mu, const ublas::matrix<double>& Omega) {
//! Multivariate normal density
size_t p = x.size1();
size_t n = x.size2();
double f = sqrt(det(Omega))/pow(2.0*PI, p/2.0);
// cout << "O: " << Omega << "\n";
// cout << "f: " << f << "\n";
ublas::matrix<double> e(p, n);
e.assign(x - outer_prod(mu, ublas::scalar_vector<double>(n, 1)));
e = element_prod(e, prod(Omega, e));
return ublas::apply_to_all<functor::exp<double> > (-(ublas::matrix_sum(e, 0))/2.0)*f;
}
vector<double> HenleyTest::ComposeSeries(HenleyTest& subsys1, HenleyTest& subsys2, int type)
{
vector<double> result;
if(type == resulttype::type_smp)
{
result = vector<double>(subsys1._phi.size());
//same as 1-(1-Q1)(1-Q2)
//subsystem unavailability
result.assign( subsys1._phi + subsys2._phi -
element_prod(subsys1._phi, subsys2._phi) );
}
else
{
result = vector<double>(subsys1._phiMC.size());
result.assign( subsys1._phiMC + subsys2._phiMC -
element_prod(subsys1._phiMC, subsys2._phiMC) );
}
return result;
}
sequence<decltype(T()*S())> element_prod( const sequence<T>& X, const sequence<S>& Y )
{
typedef decltype(T()*S()) R;
// If any vector is empty
if( X.size() == 0 || Y.size() == 0 )
return sequence<R>();
// Overlapping interval
int ta = max(X.t1(),Y.t1());
int tb = min(X.t2(),Y.t2());
// If they do not overlap
if( ta > tb )
return sequence<R>();
// They do overlap
vec<R> v = element_prod(
X.buffer()( range( ta-X.t1(), tb-X.t1()+1 ) ),
Y.buffer()( range( ta-Y.t1(), tb-Y.t1()+1 ) ) );
return sequence<R>( v, ta );
}
heavysideFunction::value_type
heavysideFunction::operator()( node_type const& pointHat ) const
{
node_type Ellipse( pbeqspace_type::Dim );
node_type point( pbeqspace_type::Dim );
point = element_prod( *M_stretch,pointHat ) + ( *M_translation );
molecule_type::atoms_const_iterator_type atom( M_molecule->begin() );
for ( ; atom != M_molecule->end(); ++atom )
{
Ellipse = point - atom->center();
//if ( norm_inf(Ellipse) > atom->radius() ) continue;
if ( norm_2( Ellipse ) < atom->radius2() )
{
return 0;
}
}
return 1;
}
void
LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff,
bool preservative, int mask)
{
boost::numeric::ublas::matrix<double> ranges(2, scan_in.ranges.size());
// Fill the ranges matrix
for (unsigned int index = 0; index < scan_in.ranges.size(); index++)
{
ranges(0,index) = (double) scan_in.ranges[index];
ranges(1,index) = (double) scan_in.ranges[index];
}
//Do the projection
// NEWMAT::Matrix output = NEWMAT::SP(ranges, getUnitVectors(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment));
boost::numeric::ublas::matrix<double> output = element_prod(ranges, getUnitVectors_(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment, scan_in.ranges.size()));
//Stuff the output cloud
cloud_out.header = scan_in.header;
cloud_out.points.resize (scan_in.ranges.size());
// Define 4 indices in the channel array for each possible value type
int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1;
cloud_out.channels.resize(0);
// Check if the intensity bit is set
if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0)
{
int chan_size = cloud_out.channels.size();
cloud_out.channels.resize (chan_size + 1);
cloud_out.channels[0].name = "intensities";
cloud_out.channels[0].values.resize (scan_in.intensities.size());
idx_intensity = 0;
}
// Check if the index bit is set
if (mask & channel_option::Index)
{
int chan_size = cloud_out.channels.size();
cloud_out.channels.resize (chan_size +1);
cloud_out.channels[chan_size].name = "index";
cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
idx_index = chan_size;
}
// Check if the distance bit is set
if (mask & channel_option::Distance)
{
int chan_size = cloud_out.channels.size();
cloud_out.channels.resize (chan_size + 1);
cloud_out.channels[chan_size].name = "distances";
cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
idx_distance = chan_size;
}
if (mask & channel_option::Timestamp)
{
int chan_size = cloud_out.channels.size();
cloud_out.channels.resize (chan_size + 1);
cloud_out.channels[chan_size].name = "stamps";
cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
idx_timestamp = chan_size;
}
if (range_cutoff < 0)
range_cutoff = scan_in.range_max;
else
range_cutoff = std::min(range_cutoff, (double)scan_in.range_max);
unsigned int count = 0;
for (unsigned int index = 0; index< scan_in.ranges.size(); index++)
{
if (preservative || ((ranges(0,index) < range_cutoff) && (ranges(0,index) >= scan_in.range_min))) //if valid or preservative
{
cloud_out.points[count].x = output(0,index);
cloud_out.points[count].y = output(1,index);
cloud_out.points[count].z = 0.0;
//double x = cloud_out.points[count].x;
//double y = cloud_out.points[count].y;
//if(x*x + y*y < scan_in.range_min * scan_in.range_min){
// ROS_INFO("(%.2f, %.2f)", cloud_out.points[count].x, cloud_out.points[count].y);
//}
// Save the original point index
if (idx_index != -1)
cloud_out.channels[idx_index].values[count] = index;
// Save the original point distance
if (idx_distance != -1)
cloud_out.channels[idx_distance].values[count] = ranges (0, index);
// Save intensities channel
if (scan_in.intensities.size() >= index)
{ /// \todo optimize and catch length difference better
if (idx_intensity != -1)
cloud_out.channels[idx_intensity].values[count] = scan_in.intensities[index];
}
// Save timestamps to seperate channel if asked for
if( idx_timestamp != -1)
cloud_out.channels[idx_timestamp].values[count] = (float)index*scan_in.time_increment;
count++;
}
}
//downsize if necessary
cloud_out.points.resize (count);
for (unsigned int d = 0; d < cloud_out.channels.size(); d++)
cloud_out.channels[d].values.resize(count);
};
////////////////////////////////////////////////////////////////////
// Calculate a stabilizing control effort
////////////////////////////////////////////////////////////////////
void calculate_u(ublas_vector &D, ublas_vector &open_loop_dx_dt, const double &V_dot_target, boost::numeric::ublas::matrix<double> &dx_dot_du)
{
u.clear();
// Find the largest |D_i|
// It will be the base for the u_i calculations.
// If all D_i's are ~0, use V2.
int largest_D_index = index_norm_inf(D);
if ( fabs(D[largest_D_index]) > switch_threshold ) // Use V1
{
ublas_vector P = element_prod(x-setpoint,open_loop_dx_dt); // x_i*x_i_dot
// Start with the u that has the largest effect
if ( fabs( D[largest_D_index] + ( sum( element_prod(D,D) ) - pow(D[largest_D_index],2.0) )/D[largest_D_index]) > 0.0001 )
u(largest_D_index) = (V_dot_target-sum(P)) /
( D[largest_D_index] + ( sum( element_prod(D,D) ) - pow(D[largest_D_index],2.0) )/D[largest_D_index] );
// Now scale the other u's (if any) according to u_max*Di/D_max
for ( int i=0; i<num_inputs; i++ )
if ( i != largest_D_index ) // Skip this entry, we already did it
{
//if ( fabs(D[i]) > 0.1*fabs(D[largest_D_index]) ) // If this D has a significant effect. Otherwise this u will remain zero
//{
u[i] = u[largest_D_index]*D[i]/D[largest_D_index];
//}
}
} // End of V1 calcs
else // Use V2
{
//ublas_vector dV2_dx = (x-setpoint)*(0.9+0.1*sum( element_prod(x-setpoint,x-setpoint) ));
ublas_vector dV2_dx = (x-setpoint)*(0.9+0.1*tanh(g)+0.05*sum( element_prod(x-setpoint,x-setpoint) )*pow(cosh(g),-2));
// The first entry in dV2_dx is unique because the step is specified in x1, so replace the previous
dV2_dx[0] = (x[0]-setpoint[0])*(0.9+0.1*tanh(g))+
0.05*sum(element_prod(x-setpoint,x-setpoint))*(pow((x[0]-setpoint[0]),-1)*tanh(g)+(x[0]-setpoint[0])*pow(cosh(g),-2));
//MATLAB equivalent:
//P_star = dV2_dx.*f_at_u_0(1:num_states);
ublas_vector P_star = element_prod(dV2_dx, open_loop_dx_dt);
//MATLAB equivalent:
//D_star = zeros(num_inputs,1);
//for i=1:num_inputs
// for j=1:num_states
// D_star(i) = D_star[i]+dV2_dx(j)*dx_dot_du(i,j);
// end
//end
ublas_vector D_star(num_inputs);
for (int i=0; i<num_inputs-1; i++)
for (int j=0; j<num_inputs-1; j++)
D_star[i] = D_star[i]+dV2_dx[j]*dx_dot_du(i,j);
// The first input is unique.
// MATLAB equivalent:
//u(epoch,1) = (V_dot_target-sum(P_star)) / ...
//( D_star(1) + (sum( D_star.*D_star )- D_star(1)^2) /...
//D_star(1) );
u[0] = (V_dot_target-sum(P_star)) /
( D_star[0] + (sum( element_prod(D_star,D_star) )-pow(D_star[0],2)) / D_star[0] );
// For the other inputs
// MATLAB equivalent:
//for i=2:num_inputs
// u(epoch,i) = u(epoch,1)*D_star(i)/D_star(1);
//end
for (int i=1; i<num_inputs; i++)
{
//u[i] = u[0]*D_star[i]/D_star(0);
u[i] = u[0]*D_star[i]/0.0;
}
// Check for NaN (caused by D_star(1)==0).
// It means the system is likely uncontrollable.
// MATLAB equivalent:
//if ~isfinite( u(epoch,:) )
// u(epoch,:)= zeros(num_inputs,1);
//end
for (int i=0; i<num_inputs; i++)
{
if ( (boost::math::isnan)(u[i]) )
{
u.clear(); // Reset u to zeros
//ROS_INFO("isnan");
break; // Short circuit, could save time for long u vectors
}
}
} // End of V2 calcs
}
Mat& Mat::ElemProd(const Mat &a) {
mexAssert(mat_.size1() == a.size1() && mat_.size2() == a.size2(),
"In 'Mat::ElemProd' the matrices are of the different size");
mat_ = element_prod(mat_, a.mat_);
return *this;
}
int test_main (int, char *[])
{
element_prod(matrix_2x1(), matrix_2x2());
return 0;
}
sequence<decltype(T()*S())> element_div( const sequence<T>& X, const sequence<S>& Y )
{
return element_prod( X, element_inv(Y) );
}
