bool Solver::postLessEqualZero(const LinearExprRepr& l)
{
if (!bValid)
return false;
if (l.vars.size()==0)
return l.free <= 0;
++stats.nbPosts;
if (l.vars.size()==1 and *l.coeffs.begin()==1)
{
++stats.nbColUpdates;
bValid = postLessEqual(l.vars.begin()->id, -l.free);
return bValid;
}
Idxs idxs(l.vars.size());
uint c = 0;
for (auto it = l.vars.begin(); it != l.vars.end(); ++it)
idxs[c++] = it->id;
Coeffs coeffs(l.vars.size());
c = 0;
for (auto it = l.coeffs.begin(); it != l.coeffs.end(); ++it)
coeffs[c++] = *it;
postLessEqual(idxs,coeffs,-l.free);
bPending = true;
return true;
}
boost::shared_ptr<std::vector<float> > test_feature(boost::shared_ptr<PointCloud> cloud,
const double radius, int max_nn, bool use_depth) {
boost::shared_ptr<std::vector<float>> stdevs
= boost::make_shared<std::vector<float>>(cloud->size());
std::vector<int> idxs(0);
std::vector<float> dists(0);
// 1m radius
//const double radius = 1.0;
GridSearch gs(*cloud);
// center
Eigen::Map<Eigen::Vector3f> center(cloud->sensor_origin_.data());
const Octree::Ptr ot = cloud->octree();
for(uint i = 0; i < cloud->size(); i++){
idxs.clear();
dists.clear();
//ot->radiusSearch(cloud->points[i], radius, idxs, dists);
//grid_nn_op(i, *cloud, idxs, 1, 50);
gs.radiusSearch((*cloud)[i], radius, idxs, dists, max_nn);
// calculate stdev of the distances?
// bad idea because you have a fixed radius
// Calculate distance from center of scan
Eigen::Map<const Eigen::Vector3f> query_point(&(cloud->points[i].x));
float sum = 0.0f;
float sum_sq = 0.0f;
for(int idx : idxs) {
const float * data = &(cloud->points[idx].x);
Eigen::Map<const Eigen::Vector3f> point(data);
float dist;
if(use_depth)
dist = (point-center).norm();
else
dist = (point-query_point).norm();
sum += dist;
sum_sq += dist*dist;
}
if(idxs.size() > 2)
(*stdevs)[i] = (sum_sq - (sum*sum)/(idxs.size()))/((idxs.size())-1);
else
(*stdevs)[i] = 0;
}
return stdevs;
}
osg::Geometry* osgpcl::SurfelFactoryI<PointT, NormalT, IntensityT>::buildGeometry(bool unique) {
typename pcl::PointCloud<NormalT>::ConstPtr normals = this->getInputCloud<NormalT>();
typename pcl::PointCloud<PointT>::ConstPtr xyz = this->getInputCloud<PointT>();
typename pcl::PointCloud<IntensityT>::ConstPtr iten = this->getInputCloud<IntensityT>();
if (xyz ==NULL) return NULL;
if (normals ==NULL) return NULL;
if (iten ==NULL) return NULL;
if (xyz->points.size() != normals->points.size()) return NULL;
if (xyz->points.size() != iten->points.size()) return NULL;
pcl::IndicesConstPtr indices = indices_;
{
bool rebuild_indices= false;
if (indices_ == NULL) rebuild_indices=true;
else if (indices_ ->size() != xyz->points.size() ) rebuild_indices=true;
if (rebuild_indices){
pcl::IndicesPtr idxs(new std::vector<int>);
idxs->reserve(xyz->points.size());
for(int i=0; i<xyz->points.size(); i++) idxs->push_back(i);
indices= idxs;
}
}
osg::Vec3Array* pts = new osg::Vec3Array;
osg::Vec3Array* npts = new osg::Vec3Array;
osg::Vec3Array* cpts = new osg::Vec3Array;
pts->reserve(indices->size());
npts->reserve(indices->size());
npts->reserve(indices->size());
for(int i=0; i<indices->size(); i++){
const int& idx = (*indices)[i];
const PointT& pt = xyz->points[idx];
const NormalT& npt = normals->points[idx];
const IntensityT& ipt = iten->points[idx];
pts->push_back(osg::Vec3(pt.x, pt.y, pt.z));
npts->push_back(osg::Vec3(npt.normal_x, npt.normal_y, npt.normal_z));
cpts->push_back(osg::Vec3(ipt.intensity, ipt.intensity, ipt.intensity));
}
osg::Geometry* geom = new osg::Geometry;
geom->setVertexArray( pts );
geom->addPrimitiveSet( new osg::DrawArrays( GL_POINTS, 0, pts->size() ) );
geom->setNormalArray(npts);
geom->setNormalBinding(osg::Geometry::BIND_PER_VERTEX);
geom->setColorArray(cpts);
geom->setColorBinding(osg::Geometry::BIND_PER_VERTEX);
geom->setStateSet(stateset_);
return geom;
}
Scalar EpetraMatrix<Scalar>::get(unsigned int m, unsigned int n) const
{
int n_entries = mat->NumGlobalEntries(m);
Hermes::vector<double> vals(n_entries);
Hermes::vector<int> idxs(n_entries);
mat->ExtractGlobalRowCopy(m, n_entries, n_entries, &vals[0], &idxs[0]);
for (int i = 0; i < n_entries; i++)
if (idxs[i] == (int)n)
return vals[i];
return 0.0;
}
PackShuffle(const Iterator& begin_iter,
const Iterator& end_iter,
unsigned int pack_size) : Tabular<Iterator, typename gaml::iterator_traits<Iterator>::tag_type>(begin_iter) {
unsigned int nb_blocks;
unsigned int i,j,k;
this->indices.resize(std::distance(begin_iter,end_iter));
std::vector<unsigned int> blocks;
std::vector<unsigned int> begins;
std::vector<unsigned int> ends;
std::vector<unsigned int> sizes;
std::vector<unsigned int> idxs(this->indices.size());
nb_blocks = idxs.size() / pack_size;
if(idxs.size() % pack_size)
nb_blocks++;
blocks.resize(nb_blocks);
begins.resize(nb_blocks);
ends.resize(nb_blocks);
sizes.resize(nb_blocks);
k = 0; for(auto& b : blocks) b = k++;
std::random_shuffle(blocks.begin(),blocks.end());
i = 0; for(auto& idx : idxs) idx = i++;
auto start = idxs.begin();
for(i = 0, k = 0; k < nb_blocks; ++k, i = j) {
j=i+pack_size;
if(j > idxs.size())
j = idxs.size();
begins[k] = i;
ends [k] = j;
sizes [k] = j-i;
std::random_shuffle(start+i,start+j);
}
auto out = this->indices.begin();
start = idxs.begin();
for(auto& b : blocks) {
std::copy(start + begins[b], start + ends[b], out);
out += sizes[b];
}
this->process_indices();
}
int nthSuperUglyNumber(int n, vector<int>& primes) {
int k = primes.size();
vector<int> idxs(k, 0), uglys(n, INT_MAX);
uglys[0] = 1;
for (int nth = 1; nth < n; ++nth) {
for (int idx = 0; idx < k; ++idx) {
uglys[nth] = min(
uglys[nth], primes[idx] * uglys[idxs[idx]]);
}
for (int idx = 0; idx < k; ++idx) {
idxs[idx] +=
(uglys[nth] == primes[idx] * uglys[idxs[idx]]);
}
}
return uglys.back();
}
int nthSuperUglyNumber(int n, vector<int>& primes) {
int cntPrimes = primes.size();
int minUglyNum,minIdx;
vector<int> idxs(cntPrimes,0);
vector<int> uglyNum;
uglyNum.push_back(1);
while(uglyNum.size() < n){
minUglyNum = INT_MAX,minIdx = 0;
for(int i = 0;i < cntPrimes;++i){
if(minUglyNum > primes[i]*uglyNum[idxs[i]]){
minUglyNum = primes[i]*uglyNum[idxs[i]];
minIdx = i;
}
}
if(uglyNum.empty()||minUglyNum>uglyNum.back())
uglyNum.push_back(minUglyNum);
++idxs[minIdx];
}
return uglyNum.back();
}
osgpcl::PointCloudGeometry* osgpcl::SurfelFactoryFF<PointT, NormalT, RadiusT>::buildGeometry(
bool unique_state) {
typename pcl::PointCloud<NormalT>::ConstPtr normals = this->getInputCloud<NormalT>();
typename pcl::PointCloud<PointT>::ConstPtr xyz = this->getInputCloud<PointT>();
typename pcl::PointCloud<RadiusT>::ConstPtr rads = this->getInputCloud<RadiusT>();
if (xyz ==NULL) return NULL;
if (normals ==NULL) return NULL;
if (rads ==NULL) return NULL;
if (xyz->points.size() != normals->points.size()) return NULL;
if (rads->points.size() != normals->points.size()) return NULL;
pcl::IndicesConstPtr indices = indices_;
{
bool rebuild_indices= false;
if (indices_ == NULL) rebuild_indices=true;
else if (indices_ ->size() != xyz->points.size() ) rebuild_indices=true;
if (rebuild_indices){
pcl::IndicesPtr idxs(new std::vector<int>);
idxs->reserve(xyz->points.size());
for(int i=0; i<xyz->points.size(); i++) idxs->push_back(i);
indices= idxs;
}
}
osg::Vec3Array* pts = new osg::Vec3Array;
osg::Vec3Array* npts = new osg::Vec3Array;
int fan_size = 1+ circle_cache.rows();
pts->reserve(indices->size()*fan_size );
npts->reserve(indices->size()*fan_size);
osg::Geometry* geom = new osg::Geometry;
for(int i=0, pstart=0; i<indices->size(); i++){
const int& idx = (*indices)[i];
const PointT& pt = xyz->points[idx];
const NormalT& npt = normals->points[idx];
pts->push_back(osg::Vec3(pt.x, pt.y, pt.z));
npts->push_back(osg::Vec3(npt.normal_x, npt.normal_y, npt.normal_z));
pcl::Normal nt;
Eigen::Matrix3f rot = Eigen::Matrix3f::Identity();
rot.row(2) << npt.getNormalVector3fMap().transpose();
Eigen::Vector3f ax2(1,0,0);
if ( npt.getNormalVector3fMap().dot(ax2 ) > 0.1) {
ax2 << 0,1,0;
if ( npt.getNormalVector3fMap().dot(ax2 ) > 0.1) {
ax2 << 0,0,1;
}
}
rot.row(1) << ax2.cross(npt.getNormalVector3fMap()).normalized().transpose();
rot.row(0) = ( ax2 - ax2.dot(npt.getNormalVector3fMap() )*npt.getNormalVector3fMap() ).normalized();
rot = rot*rads->points[idx].radius;
for(int j=0; j<circle_cache.rows(); j++){
Eigen::Vector3f apt = rot*circle_cache.row(j).transpose() + pt.getVector3fMap();
pts->push_back(osg::Vec3(apt[0],apt[1],apt[2]));
npts->push_back(osg::Vec3(npt.normal_x, npt.normal_y, npt.normal_z));
}
geom->addPrimitiveSet( new osg::DrawArrays( GL_TRIANGLE_FAN, pstart, fan_size ) );
pstart+= fan_size;
}
geom->setVertexArray( pts );
geom->setNormalArray(npts);
geom->setNormalBinding( osg::Geometry::BIND_PER_VERTEX );
geom->setStateSet(stateset_);
return geom;
}
osg::ref_ptr<osg::Node> MeshManager::get(size_t idx)
{
/* Not sure if this cache is a good idea since it shares the whole model
* tree. OSG can parent the same sub-tree to multiple points, which should
* be okay as long as the individual sub-trees don't need changing.
*/
auto iter = mModelCache.find(idx);
if(iter != mModelCache.end())
{
osg::ref_ptr<osg::Node> node;
if(iter->second.lock(node))
return node;
}
if(!mModelProgram)
{
mModelProgram = new osg::Program();
mModelProgram->addShader(osgDB::readShaderFile(osg::Shader::VERTEX, "shaders/object.vert"));
mModelProgram->addShader(osgDB::readShaderFile(osg::Shader::FRAGMENT, "shaders/object.frag"));
}
DFOSG::Mesh *mesh = DFOSG::MeshLoader::get().load(idx);
osg::ref_ptr<osg::Geode> geode(new osg::Geode());
for(auto iter = mesh->getPlanes().begin();iter != mesh->getPlanes().end();)
{
osg::ref_ptr<osg::Vec3Array> vtxs(new osg::Vec3Array());
osg::ref_ptr<osg::Vec3Array> nrms(new osg::Vec3Array());
osg::ref_ptr<osg::Vec3Array> binrms(new osg::Vec3Array());
osg::ref_ptr<osg::Vec2Array> texcrds(new osg::Vec2Array());
osg::ref_ptr<osg::Vec4ubArray> colors(new osg::Vec4ubArray());
osg::ref_ptr<osg::DrawElementsUShort> idxs(new osg::DrawElementsUShort(osg::PrimitiveSet::TRIANGLES));
uint16_t texid = iter->getTextureId();
osg::ref_ptr<osg::Texture> tex = TextureManager::get().getTexture(texid);
float width = tex->getTextureWidth();
float height = tex->getTextureHeight();
do {
const std::vector<DFOSG::MdlPlanePoint> &pts = iter->getPoints();
size_t last_total = vtxs->size();
vtxs->resize(last_total + pts.size());
nrms->resize(last_total + pts.size());
binrms->resize(last_total + pts.size());
texcrds->resize(last_total + pts.size());
colors->resize(last_total + pts.size());
idxs->resize((last_total + pts.size() - 2) * 3);
size_t j = last_total;
for(const DFOSG::MdlPlanePoint &pt : pts)
{
uint32_t vidx = pt.getIndex();
(*vtxs)[j].x() = mesh->getPoints()[vidx].x() / 256.0f;
(*vtxs)[j].y() = mesh->getPoints()[vidx].y() / 256.0f;
(*vtxs)[j].z() = mesh->getPoints()[vidx].z() / 256.0f;
(*nrms)[j].x() = iter->getNormal().x() / 256.0f;
(*nrms)[j].y() = iter->getNormal().y() / 256.0f;
(*nrms)[j].z() = iter->getNormal().z() / 256.0f;
(*binrms)[j].x() = iter->getBinormal().x() / 256.0f;
(*binrms)[j].y() = iter->getBinormal().y() / 256.0f;
(*binrms)[j].z() = iter->getBinormal().z() / 256.0f;
(*texcrds)[j].x() = pt.u() / width;
(*texcrds)[j].y() = pt.v() / height;
(*colors)[j] = osg::Vec4ub(255, 255, 255, 255);
if(j >= last_total+2)
{
(*idxs)[(j-2)*3 + 0] = last_total;
(*idxs)[(j-2)*3 + 1] = j-1;
(*idxs)[(j-2)*3 + 2] = j;
}
++j;
}
} while(++iter != mesh->getPlanes().end() && iter->getTextureId() == texid);
osg::ref_ptr<osg::VertexBufferObject> vbo(new osg::VertexBufferObject());
vtxs->setVertexBufferObject(vbo);
nrms->setVertexBufferObject(vbo);
texcrds->setVertexBufferObject(vbo);
colors->setVertexBufferObject(vbo);
colors->setNormalize(true);
osg::ref_ptr<osg::ElementBufferObject> ebo(new osg::ElementBufferObject());
idxs->setElementBufferObject(ebo);
osg::ref_ptr<osg::Geometry> geometry(new osg::Geometry);
geometry->setVertexArray(vtxs);
geometry->setNormalArray(nrms, osg::Array::BIND_PER_VERTEX);
geometry->setTexCoordArray(1, binrms, osg::Array::BIND_PER_VERTEX);
geometry->setTexCoordArray(0, texcrds, osg::Array::BIND_PER_VERTEX);
geometry->setColorArray(colors, osg::Array::BIND_PER_VERTEX);
geometry->setUseDisplayList(false);
geometry->setUseVertexBufferObjects(true);
geometry->addPrimitiveSet(idxs);
/* Cache the stateset used for this texture, so it can be reused for
* multiple models (should help OSG batch together objects with similar
* state).
*/
auto &stateiter = mStateSetCache[texid];
osg::ref_ptr<osg::StateSet> ss;
if(stateiter.lock(ss) && ss)
geometry->setStateSet(ss);
else
{
ss = geometry->getOrCreateStateSet();
ss->setAttributeAndModes(mModelProgram);
ss->addUniform(new osg::Uniform("diffuseTex", 0));
ss->setTextureAttribute(0, tex);
stateiter = ss;
}
geode->addDrawable(geometry);
}
mModelCache[idx] = osg::ref_ptr<osg::Node>(geode);
return geode;
}
