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