template<typename MatrixType> void generalized_eigensolver_real(const MatrixType& m)
{
typedef typename MatrixType::Index Index;
/* this test covers the following files:
GeneralizedEigenSolver.h
*/
Index rows = m.rows();
Index cols = m.cols();
typedef typename MatrixType::Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
typedef Matrix<RealScalar, MatrixType::RowsAtCompileTime, 1> RealVectorType;
typedef typename std::complex<typename NumTraits<typename MatrixType::Scalar>::Real> Complex;
MatrixType a = MatrixType::Random(rows,cols);
MatrixType b = MatrixType::Random(rows,cols);
MatrixType a1 = MatrixType::Random(rows,cols);
MatrixType b1 = MatrixType::Random(rows,cols);
MatrixType spdA = a.adjoint() * a + a1.adjoint() * a1;
MatrixType spdB = b.adjoint() * b + b1.adjoint() * b1;
// lets compare to GeneralizedSelfAdjointEigenSolver
GeneralizedSelfAdjointEigenSolver<MatrixType> symmEig(spdA, spdB);
GeneralizedEigenSolver<MatrixType> eig(spdA, spdB);
VERIFY_IS_EQUAL(eig.eigenvalues().imag().cwiseAbs().maxCoeff(), 0);
VectorType realEigenvalues = eig.eigenvalues().real();
std::sort(realEigenvalues.data(), realEigenvalues.data()+realEigenvalues.size());
VERIFY_IS_APPROX(realEigenvalues, symmEig.eigenvalues());
}
void DiscretePolicyManager::UpdateCallback( const ros::TimerEvent& event )
{
StampedFeatures inputs;
if( !_inputStreams.ReadStream( event.current_real, inputs ) )
{
ROS_WARN_STREAM( "Could not read input stream." );
return;
}
// Generate probability mass function values
_networkInput.SetOutput( inputs.features );
_networkInput.Invalidate();
_networkInput.Foreprop();
VectorType pmf = _network->GetProbabilitySource().GetOutput();
// Sample from PMF
std::vector<double> weights( pmf.data(), pmf.data() + pmf.size() );
std::vector<unsigned int> draws;
NaiveWeightedSample( weights, 1, draws, _engine );
unsigned int actionIndex = draws[0];
// Convert single index into multiple indices
std::vector<unsigned int> indices;
indices = multibase_long_div( actionIndex, _interface->GetOutputSizes() );
_interface->SetOutput( indices );
// TODO name policy
DiscreteParamAction action( event.current_real,
inputs.features,
actionIndex );
_actionPub.publish( action.ToMsg() );
}
void deep_network::set_parameter(const VectorType ¶meter_)
{
int start_idx = 0;
for (auto & layer : this->layers)
{
layer.W = Map<const MatrixType>(parameter_.data()+ start_idx,layer.get_input_dim(),layer.get_output_dim()) ;
start_idx += layer.get_input_dim() * layer.get_output_dim();
layer.b = Map<const VectorType>(parameter_.data()+ start_idx, layer.get_output_dim());
start_idx += layer.get_output_dim();
}
}
VectorType deep_network::get_parameter() {
int total_param_num = 0;
for (auto & layer : this->layers)
{
total_param_num += (layer.get_input_dim() + 1) * layer.get_output_dim();
}
VectorType Wb = VectorType::Zero(total_param_num);
int start_idx = 0;
for (auto & layer : this->layers)
{
Map<MatrixType>(Wb.data()+ start_idx,layer.get_input_dim(),layer.get_output_dim()) = layer.W;
start_idx += layer.get_input_dim() * layer.get_output_dim();
Map<VectorType>(Wb.data()+ start_idx, layer.get_output_dim()) = layer.b;
start_idx += layer.get_output_dim();
}
return Wb;
}
clsparseStatus
scan(VectorType& output, const VectorType& input,
clsparseControl control, bool exclusive)
{
typedef typename VectorType::size_type SizeType; //check for cl_ulong
typedef typename VectorType::value_type T;
if (!clsparseInitialized)
{
return clsparseNotInitialized;
}
//check opencl elements
if (control == nullptr)
{
return clsparseInvalidControlObject;
}
assert (input.size() == output.size());
SizeType num_elements = input.size();
//std::cout << "num_elements = " << num_elements << std::endl;
SizeType KERNEL02WAVES = 4;
SizeType KERNEL1WAVES = 4;
SizeType WAVESIZE = control->wavefront_size;
SizeType kernel0_WgSize = WAVESIZE*KERNEL02WAVES;
SizeType kernel1_WgSize = WAVESIZE*KERNEL1WAVES;
SizeType kernel2_WgSize = WAVESIZE*KERNEL02WAVES;
SizeType numElementsRUP = num_elements;
SizeType modWgSize = (numElementsRUP & ((kernel0_WgSize*2)-1));
if( modWgSize )
{
numElementsRUP &= ~modWgSize;
numElementsRUP += (kernel0_WgSize*2);
}
//2 element per work item
SizeType numWorkGroupsK0 = numElementsRUP / (kernel0_WgSize*2);
SizeType sizeScanBuff = numWorkGroupsK0;
modWgSize = (sizeScanBuff & ((kernel0_WgSize*2)-1));
if( modWgSize )
{
sizeScanBuff &= ~modWgSize;
sizeScanBuff += (kernel0_WgSize*2);
}
cl::Context ctx = control->getContext();
clsparse::vector<T> preSumArray(control, sizeScanBuff,
0, CL_MEM_READ_WRITE, false);
clsparse::vector<T> preSumArray1(control, sizeScanBuff,
0, CL_MEM_READ_WRITE, false);
clsparse::vector<T> postSumArray(control, sizeScanBuff,
0, CL_MEM_READ_WRITE, false);
T operator_identity = 0;
//std::cout << "operator_identity = " << operator_identity << std::endl;
//scan in blocks
{
//local mem size
std::size_t lds = kernel0_WgSize * 2 * sizeof(T);
std::string params = std::string()
+ " -DVALUE_TYPE=" + OclTypeTraits<T>::type
+ " -DWG_SIZE=" + std::to_string(kernel0_WgSize)
+ " -D" + ElementWiseOperatorTrait<OP>::operation;
if (sizeof(clsparseIdx_t) == 8)
{
std::string options = std::string()
+ " -DSIZE_TYPE=" + OclTypeTraits<cl_ulong>::type;
params.append(options);
}
else
{
std::string options = std::string()
+ " -DSIZE_TYPE=" + OclTypeTraits<cl_uint>::type;
params.append(options);
}
if(typeid(T) == typeid(cl_double))
{
params.append(" -DDOUBLE");
if (!control->dpfp_support)
{
#ifndef NDEBUG
std::cerr << "Failure attempting to run double precision kernel on device without DPFP support." << std::endl;
#endif
return clsparseInvalidDevice;
}
}
cl::Kernel kernel = KernelCache::get(control->queue, "scan",
"per_block_inclusive_scan", params);
KernelWrap kWrapper(kernel);
kWrapper << input.data()
<< operator_identity
<< (SizeType)input.size()
<< cl::Local(lds)
<< preSumArray.data()
<< preSumArray1.data()
<< (int) exclusive;
cl::NDRange global(numElementsRUP/2);
cl::NDRange local (kernel0_WgSize);
cl_int status = kWrapper.run(control, global, local);
CLSPARSE_V(status, "Error: per_block_inclusive_scan");
if (status != CL_SUCCESS)
{
return clsparseInvalidKernelExecution;
}
}
{
//local mem size
std::size_t lds = kernel0_WgSize * sizeof(T);
SizeType workPerThread = sizeScanBuff / kernel1_WgSize;
std::string params = std::string()
+ " -DVALUE_TYPE=" + OclTypeTraits<T>::type
+ " -DWG_SIZE=" + std::to_string(kernel1_WgSize)
+ " -D" + ElementWiseOperatorTrait<OP>::operation;
if (sizeof(clsparseIdx_t) == 8)
{
std::string options = std::string()
+ " -DSIZE_TYPE=" + OclTypeTraits<cl_ulong>::type;
params.append(options);
}
else
{
std::string options = std::string()
+ " -DSIZE_TYPE=" + OclTypeTraits<cl_uint>::type;
params.append(options);
}
if(typeid(T) == typeid(cl_double))
{
params.append(" -DDOUBLE");
if (!control->dpfp_support)
{
#ifndef NDEBUG
std::cerr << "Failure attempting to run double precision kernel on device without DPFP support." << std::endl;
#endif
return clsparseInvalidDevice;
}
}
cl::Kernel kernel = KernelCache::get(control->queue, "scan",
"intra_block_inclusive_scan", params);
KernelWrap kWrapper(kernel);
kWrapper << postSumArray.data()
<< preSumArray.data()
<< operator_identity
<< numWorkGroupsK0
<< cl::Local(lds)
<< workPerThread;
cl::NDRange global ( kernel1_WgSize );
cl::NDRange local ( kernel1_WgSize );
cl_int status = kWrapper.run(control, global, local);
CLSPARSE_V(status, "Error: intra_block_inclusive_scan");
if (status != CL_SUCCESS)
{
return clsparseInvalidKernelExecution;
}
}
{
std::size_t lds = kernel0_WgSize * sizeof(T); //local mem size
std::string params = std::string()
+ " -DVALUE_TYPE=" + OclTypeTraits<T>::type
+ " -DWG_SIZE=" + std::to_string(kernel1_WgSize)
+ " -D" + ElementWiseOperatorTrait<OP>::operation;
if (sizeof(clsparseIdx_t) == 8)
{
std::string options = std::string()
+ " -DSIZE_TYPE=" + OclTypeTraits<cl_ulong>::type;
params.append(options);
}
else
{
std::string options = std::string()
+ " -DSIZE_TYPE=" + OclTypeTraits<cl_uint>::type;
params.append(options);
}
if(typeid(T) == typeid(cl_double))
{
params.append(" -DDOUBLE");
if (!control->dpfp_support)
{
#ifndef NDEBUG
std::cerr << "Failure attempting to run double precision kernel on device without DPFP support." << std::endl;
#endif
return clsparseInvalidDevice;
}
}
cl::Kernel kernel = KernelCache::get(control->queue, "scan",
"per_block_addition", params);
KernelWrap kWrapper(kernel);
kWrapper << output.data()
<< input.data()
<< postSumArray.data()
<< preSumArray1.data()
<< cl::Local(lds)
<< num_elements
<< (int)exclusive
<< operator_identity;
cl::NDRange global ( numElementsRUP );
cl::NDRange local ( kernel2_WgSize );
cl_int status = kWrapper.run(control, global, local);
CLSPARSE_V(status, "Error: per_block_addition");
if (status != CL_SUCCESS)
{
return clsparseInvalidKernelExecution;
}
}
return clsparseSuccess;
}
void CoverRefiner::Update()
{
ConnectivityMatrixType conn;
try
{
//conn.resize(MAX_NUMBER_OF_VERTICES, MAX_NUMBER_OF_VERTICES);
conn.resize(m_coverVertices->GetNumberOfPoints(), m_coverVertices->GetNumberOfPoints());
//conn.fill(0);
}
catch(...)
{
std::cout<<"Error allocating space for connectivity matrix in Refiner"<<std::endl;
throw;
}
//std::cout<<"Cover size: "<<localCover.size()<<std::endl;
InitializeConnectivityMatrix(conn);
//create storage for weights
//this will be synchronized with coverVertices
//------------------------------------------
//
// Step 1:
// for each vertex on hole boundary calculate s(vi) = average adjacent edge lengths, !!do not consider cover edges!!
//
//
//--------------------------------------------------------------
// This is handled by external call
// for(int i=0;i<sigmas.size();i++)
// std::cout<<sigmas[i]<<" "<<std::endl;
//------------------------------------------
//
// Step 1.5:
// relax all edges of the cover
//
//
//--------------------------------------------------
try
{
while( RelaxAllCoverEdges(conn) ) {};
}
catch(...)
{
std::cout<<"An exception happened"<<std::endl;
}
//------------------------------------------
//
// Step 2:
//
//
//2: for each (vi,vj,vk) of the cover
// get centroid vc
// s(vc) = (s(vi)+s(vj)+s(vk))/3
// (originally) check for all m=i,j,k sqrt(2)*||vc-vm||>max(s(vc),s(vm))
// check for all m=i,j,k 2*||vc-vm||^2 > max(s(vc)^2,s(vm)^2)
// replace (vi,vj,vk) with (vc,vj,vk), (vi,vc,vk), (vi,vj,vc)
// relax edges (vi,vj), (vi,vk), (vj,vk)
// split_created = true
// if !split_created end
//
//--------------------------------------------------
while ( true )
{
bool TriangleSplitted = false; //algorithm stops when no more splits are required
HoleCoverType new_cover;
// std::cout<<"Cover size: "<<m_coverFaces->size()<<std::endl;
//SaveIsolatedCover("1.vtk");
//for(HoleCoverType::iterator coverIt = m_coverFaces->begin(); coverIt!=m_coverFaces->end(); coverIt++)
for(HoleCoverType::iterator coverIt = m_coverFaces->begin(); coverIt!=m_coverFaces->end(); coverIt++)
{
//calculate centroid
const vtkIdType idVi = (*coverIt).id[0];
const vtkIdType idVj = (*coverIt).id[1];
const vtkIdType idVk = (*coverIt).id[2];
VectorType Vc;
double Svc;
// std::cout<<"Cover size: "<<m_coverFaces->size()<<std::endl;
// std::cout<<"Splitting verifying: "<<idVi<<" "<<idVj<<" "<<idVk<<std::endl;
if( IsTriangleSplitRequired(idVi, idVj, idVk, Vc, Svc) )
{ //create new triangles
//erase old triangle
// std::cout<<"Splitting confirmed: "<<idVi<<" "<<idVj<<" "<<idVk<<std::endl;
//m_coverFaces->erase(coverIt);
//add new vertex
const vtkIdType idVc = m_coverVertices->InsertNextPoint(Vc.data());
//replace (vi,vj,vk) with (vc,vj,vk), (vi,vc,vk), (vi,vj,vc)
TriangleCellType tri1; tri1.id[0]=idVc; tri1.id[1]=idVj; tri1.id[2]=idVk;
TriangleCellType tri2; tri2.id[0]=idVi; tri2.id[1]=idVc; tri2.id[2]=idVk;
TriangleCellType tri3; tri3.id[0]=idVi; tri3.id[1]=idVj; tri3.id[2]=idVc;
//relax edges (vi,vj), (vi,vk), (vj,vk)
// TriangleSplitted = true;
new_cover.push_back(tri1);
new_cover.push_back(tri2);
new_cover.push_back(tri3);
// std::cout<<"Inserting: "<<tri1.id[0]<<" "<<tri1.id[1]<<" "<<tri1.id[2]<<std::endl;
// std::cout<<"Inserting: "<<tri2.id[0]<<" "<<tri2.id[1]<<" "<<tri2.id[2]<<std::endl;
// std::cout<<"Inserting: "<<tri3.id[0]<<" "<<tri3.id[1]<<" "<<tri3.id[2]<<std::endl;
// CheckForDuplicateTriangles();
//Update matrices
m_sigmas.push_back(Svc);
//new point added - need resize
//conn.conservativeResize(coverVertices->GetNumberOfPoints(), coverVertices->GetNumberOfPoints());
//std::cout<<"conn matrix size: "<<conn.rows()<<", "<<conn.cols()<<std::endl;
// vtkIdType max_id1 = std::max( tri1.id[0], std::max( tri1.id[1], tri1.id[2]) );
// vtkIdType max_id2 = std::max( tri2.id[0], std::max( tri2.id[1], tri2.id[2]) );
// vtkIdType max_id3 = std::max( tri3.id[0], std::max( tri3.id[1], tri3.id[2]) );
// vtkIdType max_id4 = std::max( max_id1, std::max( max_id2, max_id3) );
//
// std::cout<<"max id to store: "<<max_id4<<std::endl;
// if(m_coverVertices->GetNumberOfPoints()>MAX_NUMBER_OF_VERTICES)
// {
try
{
conn.conservativeResize(m_coverVertices->GetNumberOfPoints(), m_coverVertices->GetNumberOfPoints());
}
catch(...)
{
std::cout<<"Error during conservativeResize of conn in refiner"<<std::endl;
throw;
}
// }
for(int i=0; i<3; i++)
{
conn.coeffRef(std::min(tri1.id[i], tri1.id[(i+1)%3]), std::max(tri1.id[i], tri1.id[(i+1)%3])) = 2;
conn.coeffRef(std::min(tri2.id[i], tri2.id[(i+1)%3]), std::max(tri2.id[i], tri2.id[(i+1)%3])) = 2;
conn.coeffRef(std::min(tri3.id[i], tri3.id[(i+1)%3]), std::max(tri3.id[i], tri3.id[(i+1)%3])) = 2;
}
// std::cout<<"Stored"<<std::endl;
// std::cout<<conn<<std::endl;
//relax edges (vi,vj), (vi,vk), (vj,vk)
// EdgeType edge; edge.v0 = idVi; edge.v1 = idVj;
// EdgeType candidateEdge;
//
// std::cout<<"relaxing after split"<<std::endl;
// if( FindConnectedVertices(&new_cover, edge, candidateEdge) )
// RelaxEdgeIfPossible(&new_cover, edge, candidateEdge, conn);
// std::cout<<"relaxed after split"<<std::endl;
TriangleSplitted = true;
// SaveIsolatedCover(localCover, coverVertices, "refined.vtk");
}
else
{
TriangleCellType tri1; tri1.id[0]=idVi; tri1.id[1]=idVj; tri1.id[2]=idVk;
// std::cout<<"Inserting: "<<tri1.id[0]<<" "<<tri1.id[1]<<" "<<tri1.id[2]<<std::endl;
new_cover.push_back(tri1);
}
}
// std::cout<<"Old faces: "<<m_coverFaces->size()<<std::endl;
// for(HoleCoverType::const_iterator iit1 = m_coverFaces->begin(); iit1!=m_coverFaces->end(); iit1++)
// std::cout<<(*iit1).id[0]<<" "<<(*iit1).id[1]<<" "<<(*iit1).id[2]<<std::endl;
//
// std::cout<<"New faces: "<<new_cover.size()<<std::endl;
// for(HoleCoverType::const_iterator iit1 = new_cover.begin(); iit1!=new_cover.end(); iit1++)
// std::cout<<(*iit1).id[0]<<" "<<(*iit1).id[1]<<" "<<(*iit1).id[2]<<std::endl;
m_coverFaces->clear();
//m_coverFaces->resize(new_cover.size());
for(HoleCoverType::const_iterator iit1 = new_cover.begin(); iit1!=new_cover.end(); iit1++)
{
TriangleCellType tri1; tri1.id[0]=(*iit1).id[0]; tri1.id[1]=(*iit1).id[1]; tri1.id[2]=(*iit1).id[2];
// std::cout<<"Saving :"<<tri1.id[0]<<" "<<tri1.id[1]<<" "<<tri1.id[2]<<std::endl;
m_coverFaces->push_back( tri1 );
}
//(*m_coverFaces) = new_cover;
// std::cout<<"After assignment: "<<m_coverFaces->size()<<std::endl;
// for(HoleCoverType::const_iterator iit1 = m_coverFaces->begin(); iit1!=m_coverFaces->end(); iit1++)
// std::cout<<(*iit1).id[0]<<" "<<(*iit1).id[1]<<" "<<(*iit1).id[2]<<std::endl;
//
//SaveIsolatedCover("2.vtk");
// std::cout<<"Cover size: "<<m_coverFaces->size()<<std::endl;
// std::cout<<"Sigmas size: "<<m_sigmas.size()<<std::endl;
//std::cout<<"Connectivity: "<<conn.rows()<<", "<<conn.cols()<<", "<<conn.size()<<std::endl;
//------------------------------------------
//
// Step 3: If no splits were performed - finish
//
if(!TriangleSplitted) break;
//------------------------------------------
//
// Step 4:
//
// Relax all cover edges
// std::cout<<"relaxing started"<<std::endl;
while( RelaxAllCoverEdges(conn) ) {};
// std::cout<<"After relaxing: "<<m_coverFaces->size()<<std::endl;
// for(HoleCoverType::const_iterator iit1 = m_coverFaces->begin(); iit1!=m_coverFaces->end(); iit1++)
// std::cout<<(*iit1).id[0]<<" "<<(*iit1).id[1]<<" "<<(*iit1).id[2]<<std::endl;
// std::cout<<"relaxing complete"<<std::endl;
}
CheckForDuplicateTriangles();
}
