sure::Scalar sure::keypoints::calculateEntropyWithCrossproducts(const Octree& octree, Node* node, Scalar normalSamplingrate, Scalar radius, Scalar influenceRadius)
{
FixedPayload mainNormalIntegrate;
octree.integratePayload(node->fixed().getMeanPosition(), radius, mainNormalIntegrate);
Normal mainNormal = mainNormalIntegrate.calculateNormal();
if( mainNormal.isStable() )
{
sure::normal::CrossProductHistogram histogram;
histogram.setInfluenceRadius(influenceRadius);
NodeVector nodes = octree.getNodes(node->fixed().getMeanPosition(), radius, normalSamplingrate);
for(unsigned int i=0; i<nodes.size(); ++i)
{
Node* currNode = nodes[i];
NormalPayload* currPayload = static_cast<CrossProductPayload*>(currNode->opt());
if( currPayload->normal_.isStable())
{
histogram.insertCrossProduct(mainNormal.vector(), currPayload->normal_.vector());
}
}
return histogram.calculateEntropy();
}
return 0.0;
}
void TestOctree()
{
std::vector<vgl_point_3d<double> > Points;
Points.push_back(vgl_point_3d<double>(1.0, 0.0, 0.0));
Points.push_back(vgl_point_3d<double>(0.0, 0.0, 0.0));
Points.push_back(vgl_point_3d<double>(0.0, 1.0, 0.0));
std::vector<std::vector<unsigned int> > VertexLists;
std::vector<unsigned int> VertexList;
VertexList.push_back(0);
VertexList.push_back(1);
VertexList.push_back(2);
VertexLists.push_back(VertexList);
Octree Tree;
Tree.Build(Points, VertexLists);
OrientedPoint Intersection;
Ray R(vgl_point_3d<double>(0.5, 0.5, -1.0), vgl_vector_3d<double> (0.0, 0.0, 1.0));
bool intersect = Tree.IntersectRay(R, Intersection);
std::cout << "Intersect? " << intersect << std::endl;
if(intersect)
std::cout << "Intersection: " << Intersection << std::endl;
}
void test1()
{
Octree* mytree = new Octree();
std::cout << mytree->getOrigin().getX() << std::endl;
std::cout << mytree->getRadii().getX() << std::endl;
delete mytree;
}
void Octree::updateParents() {
Octree* node = parent;
while (node) {
node->subtree_colors.push_back(std::make_pair(color_id, getColor()));
node = node->getParent();
}
}
void Octree::testIntersectRayBoth( const Ray& ray, HitResult & hitRes )
{
stack<Octree*> trees;
Ray inverseRay = ray.inverse();
if (boundingBox.intersects(ray) || boundingBox.intersects(inverseRay))
trees.push(this);
while(!trees.empty())
{
Octree * t = trees.top(); trees.pop();
if (t->testIntersectHit(ray, hitRes))
{
return;
}
else
{
for (StdVector<Octree>::iterator child = t->children.begin(); child != t->children.end(); child++)
{
if (child->boundingBox.intersects(ray) || child->boundingBox.intersects(inverseRay))
trees.push(&(*child));
}
}
}
}
void Octree::newNode( int depth, double x, double y, double z )
{
double extent = boundingBox.xExtent / 2.0;
Vec3d center;
center.x() = boundingBox.center.x() + (extent * x);
center.y() = boundingBox.center.y() + (extent * y);
center.z() = boundingBox.center.z() + (extent * z);
BoundingBox bb(center, extent, extent, extent);
// Add child
children.push_back(Octree());
Octree * child = &children.back();
child->boundingBox = bb;
child->trianglePerNode = this->trianglePerNode;
// Collect triangles inside child's bounding box
for(StdVector<BaseTriangle*>::iterator it = this->triangleData.begin(); it != this->triangleData.end(); it++)
{
BaseTriangle* face = *it;
if( bb.containsTriangle(face->vec(0), face->vec(1), face->vec(2)) )
{
child->triangleData.push_back(face);
}
}
child->build(depth + 1); // build it
}
void GameEngine::Generate(Entity *node, Graphic::Object *obj)
{
Vector2f size(40, 40);
Octree *oct = new Octree(Box3f(Vector3f(0,0,0),Vector3f(50,50,50)));
Box3f box;
node->data = oct;
std::list<std::pair<Box3f,ISceneNode*> > listToInsert;
for (unsigned int i = 0; i < size[0]; ++i)
{
for (unsigned int j = 0; j < size[1]; ++j)
{
Graphic::Object *newObj = obj->Clone()->As<Graphic::Object>();
newObj->Matrix.Translation(i,j,0);
newObj->GetReelBox(box);
listToInsert.push_front(std::pair<Box3f,ISceneNode*>(box, newObj));
}
}
obj->Matrix.Translation(42,42,42);
obj->GetReelBox(box);
listToInsert.push_front(std::pair<Box3f,ISceneNode*>(box, obj));
oct->Insert(listToInsert, 16);
oct->DrawOutlines(true);
}
void slaveFunc () {
int nodeRank;
MPI_Comm_rank(MPI_COMM_WORLD, &nodeRank);
int treeSize;
MPI_Bcast (&treeSize, 1, MPI_INT, MASTER, MPI_COMM_WORLD);
char * treeBuffer = new char[treeSize];
MPI_Bcast (treeBuffer, treeSize, MPI_BYTE, MASTER, MPI_COMM_WORLD);
Octree t;
t.readSerializedData(treeBuffer, treeSize);
MPI_Status status;
int chunkSize;
MPI_Recv (&chunkSize, 1, MPI_INT, MASTER, HEADER, MPI_COMM_WORLD, &status);
cout << "chunk size" << chunkSize << endl;
RayPixel * chunk = new RayPixel[chunkSize];
MPI_Recv (chunk, chunkSize * sizeof(RayPixel), MPI_BYTE, MASTER, RAY_ARRAY, MPI_COMM_WORLD, &status);
TracePixel * traceChunk = new TracePixel[chunkSize];
for (int i = 0; i < chunkSize; i++) {
traceChunk[i] = traceRay(chunk[i], t);
}
cout << "done tracing client pack" << endl;
MPI_Request request;
MPI_Isend (&chunkSize, 1, MPI_INT, MASTER, HEADER, MPI_COMM_WORLD, &request);
MPI_Isend (traceChunk, chunkSize * sizeof(TracePixel), MPI_BYTE, MASTER, NODE_ARRAY, MPI_COMM_WORLD, &request);
}
void Octree::intersectRayBoth( const Ray& ray, IndexSet & tris )
{
stack<Octree*> trees;
Ray inverseRay(ray.inverse());
if (boundingBox.intersects(ray) || boundingBox.intersects(inverseRay))
trees.push(this);
else
return;
while(!trees.empty())
{
Octree * t = trees.top(); trees.pop();
if (!t->intersectHit(tris))
{
for (StdVector<Octree>::iterator child = t->children.begin(); child != t->children.end(); child++)
{
if (child->boundingBox.intersects(ray) || child->boundingBox.intersects(inverseRay))
{
trees.push(&(*child));
}
}
}
}
}
int main( int args, char* argv[] ) {
Octree* tree = new Octree();
int counts[3];
tree->countNodes(counts);
std::cout << " Internal " << counts[0] << "\tPseudo " << counts[1] << "\tLeaf " << counts[2] << "\n";
delete tree;
}
void test3()
{
Vec3<double> temp = Vec3<double>(0.0, 0.0, 0.0);
Vec3<double> origin = Vec3<double>(100.0, 100.0, 100.0);
Vec3<double> radii = Vec3<double>(10.0, 10.0, 10.0);
Octree * mytree = new Octree(origin, radii);
std::cout << mytree->getOctant(temp) << std::endl;
delete mytree;
}
// This function finds the first
// non-leaf octant for a data point
Octree<T> *findBestChild( const glm::vec3 &pos ){
int octant = 0;
Octree<T> *ret = this;
do {
octant = ret->getOctantFromPoint( pos );
ret = ret->children[ octant ];
} while( !ret->isLeafNode() );
return ret;
}
OctreePalette::OctreePalette(const std::vector<RGBAPixel>& colors)
: colors(colors) {
// add each color to the octree, assign a palette index and update parents
for (size_t i = 0; i < colors.size(); i++) {
RGBAPixel color = colors[i];
Octree* node = Octree::findOrCreateNode(&octree, color);
node->setColor(color);
node->setColorID(i);
node->updateParents();
}
}
sure::Scalar sure::keypoints::calculateCornerness(const Octree& octree, Node* node, Scalar radius)
{
Octree::NodeVector vec;
vec = octree.getNodes(node, octree.getUnitSize(radius));
Vector3 mean(Vector3::Zero());
Scalar weight(0.0);
for(unsigned int i=0; i<vec.size(); ++i)
{
Node* currNode = vec[i];
sure::payload::EntropyPayload* payload = static_cast<EntropyPayload*>(currNode->opt());
if( payload->entropy_ > 0.0 )
{
mean += (payload->entropy_ * currNode->fixed().getMeanPosition());
// mean[0] += (payload->entropy_ * currNode->fixed().getMeanPosition()[0]);
// mean[1] += (payload->entropy_ * currNode->fixed().getMeanPosition()[1]);
// mean[2] += (payload->entropy_ * currNode->fixed().getMeanPosition()[2]);
weight += payload->entropy_;
}
}
if( weight > 0.0 )
{
mean /= weight;
}
else
{
return 0.f;
}
Matrix3 covariance(Matrix3::Zero());
for(unsigned int i=0; i<vec.size(); ++i)
{
Node* currNode = vec[i];
EntropyPayload* payload = static_cast<EntropyPayload*>(currNode->opt());
if( payload->entropy_ > 0.0 )
{
// Vector3 d;
// d[0] = mean[0] - currNode->fixed().getMeanPosition()[0];
// d[1] = mean[1] - currNode->fixed().getMeanPosition()[1];
// d[2] = mean[2] - currNode->fixed().getMeanPosition()[2];
covariance += payload->entropy_ * ( (mean - currNode->fixed().getMeanPosition()) * ((mean - currNode->fixed().getMeanPosition()).transpose()) );
// covariance += payload->entropy_ * ( d * d.transpose() );
}
}
covariance /= weight;
Vector3 eigenValues;
pcl::eigen33(covariance, eigenValues);
return (Scalar) (eigenValues[0] / eigenValues[2]);
}
void Drawable::RemoveFromOctree()
{
if (octant_)
{
Octree* octree = octant_->GetRoot();
if (updateQueued_)
octree->CancelUpdate(this);
// Perform subclass specific deinitialization if necessary
OnRemoveFromOctree();
octant_->RemoveDrawable(this);
}
}
void Gizmo3D::Show()
{
if (scene_.Null())
return;
gizmo_->SetEnabled(true);
Octree* octree = scene_->GetComponent<Octree>();
if (!octree)
return;
octree->AddManualDrawable(gizmo_);
}
unsigned sure::keypoints::extractKeypoints(Octree& octree, Scalar samplingrate, Scalar searchRadius, Scalar featureRadius, std::vector<Feature>& features, std::vector<Node*>& keypointNodes)
{
unsigned samplingDepth = octree.getDepth(samplingrate);
unsigned keypoints(0);
for(unsigned int i=0; i<octree[samplingDepth].size(); ++i)
{
Node* currNode = octree[samplingDepth][i];
EntropyPayload* payload = static_cast<EntropyPayload*>(currNode->opt());
if( payload->flag_ != POSSIBLE )
{
continue;
}
NodeVector neighbors = octree.getNodes(currNode, octree.getUnitSize(searchRadius));
for(unsigned int j=0; j<neighbors.size(); ++j)
{
Node* currNeighbor = neighbors[j];
EntropyPayload* neighborPayload = static_cast<EntropyPayload*>(currNeighbor->opt());
if( neighborPayload->flag_ == IS_MAXIMUM )
{
payload->flag_ = SUPPRESSED;
break;
}
else if( neighborPayload->flag_ == POSSIBLE )
{
if( payload->entropy_ < neighborPayload->entropy_ )
{
payload->flag_ = SUPPRESSED;
break;
}
}
else
{
continue;
}
}
if( payload->flag_ == POSSIBLE )
{
payload->flag_ = IS_MAXIMUM;
sure::feature::Feature f;
f.radius() = featureRadius;
f.position() = currNode->fixed().getMeanPosition();
features.push_back(f);
keypointNodes.push_back(currNode);
keypoints++;
}
}
return keypoints;
}
void GameEngine::KeyboardEvent(System::Event &event)
{
_camera->KeyboardEvent(event);
if (event.type == System::Event::KeyPressed)
{
if (event.key.code == System::Key::Space)
{
Octree *oct = _entity->data->As<Octree>();
if (oct->DrawOutlines() == false)
oct->DrawOutlines(true);
else
oct->DrawOutlines(false);
}
}
}
void _notifier_add(VisibilityNotifier *p_notifier, const AABB &p_rect) {
ERR_FAIL_COND(notifiers.has(p_notifier));
notifiers[p_notifier].aabb = p_rect;
notifiers[p_notifier].id = octree.create(p_notifier, p_rect);
changed = true;
}
void addSphere()
{
if(spheres.size()>=MAX) return ;
GLfloat r, x, y, z, vx, vy, vz;
int signo;
r = (rand()/ (RAND_MAX + 1.0))/2.75 + 0.3;
signo = pow(-1.0, (rand()%2)+1); //Generates 1 or 2
x = signo*(rand()%12)/(rand()%12 + 1.0);
y = (rand()%15)/(rand()%15+1.0) + 10.0;
signo = pow(-1.0, (rand()%2)+1);
z = signo*(rand()%12)/(rand()%12 + 1.0);
signo = pow(-1.0, (rand()%2)+1);
vx = signo*(rand()%2+1);
signo = pow(-1.0, (rand()%2)+1);
vy = signo*(rand()%2+1);
signo = pow(-1.0, (rand()%2)+1);
vz = signo*(rand()%2+1);
GLint tex=rand()%8;
Vector3 pos(x,y,z), vel(vx,vy,vz);
//cout << "r: "<< r << " pos: " << pos << " vel: " << vel << " tex: "<< texNames[tex] << endl;
Sphere *s = new Sphere(r,pos,vel,texts[tex], quad);
spheres.push_back(s);
octree.add(s);
}
Octree* Octree::findOrCreateNode(Octree* octree, RGBAPixel color) {
assert(octree != nullptr);
uint8_t red = rgba_red(color);
uint8_t green = rgba_green(color);
uint8_t blue = rgba_blue(color);
uint8_t alpha = rgba_alpha(color);
Octree* node = octree;
for (int i = 7; i >= 8 - OCTREE_COLOR_BITS; i--) {
int index = (nth_bit(red, i) << 3) | (nth_bit(green, i) << 2) | nth_bit(blue, i) << 1 | nth_bit(alpha, i);
node = node->getChildren(index);
assert(node != nullptr);
}
return node;
}
sure::Scalar sure::keypoints::calculateEntropyWithCrossproductsPairwise(const Octree& octree, Node* node, Scalar normalSamplingrate, Scalar radius, Scalar influenceRadius)
{
sure::normal::CrossProductHistogram histogram;
histogram.setInfluenceRadius(influenceRadius);
NodeVector nodes = octree.getNodes(node->fixed().getMeanPosition(), radius, normalSamplingrate);
for(unsigned int i=0; i<nodes.size(); ++i)
{
Node* firstNode = nodes[i];
NormalPayload* firstPayload = static_cast<NormalPayload*>(firstNode->opt());
const Normal& firstNormal = firstPayload->normal_;
if( !firstNormal.isStable() )
{
continue;
}
for(unsigned int j=i+1; j<nodes.size(); ++j)
{
Node* secondNode = nodes[j];
CrossProductPayload* secondPayload = static_cast<CrossProductPayload*>(secondNode->opt());
const Normal& secondNormal = secondPayload->normal_;
if( secondNormal.isStable() )
{
histogram.insertCrossProduct(firstNormal.vector(), secondNormal.vector());
}
}
return histogram.calculateEntropy();
}
return 0.0;
}
void _notifier_remove(VisibilityNotifier* p_notifier) {
Map<VisibilityNotifier*,NotifierData>::Element *E=notifiers.find(p_notifier);
ERR_FAIL_COND(!E);
octree.erase(E->get().id);
notifiers.erase(p_notifier);
List<Camera*> removed;
for (Map<Camera*,CameraData>::Element*F=cameras.front();F;F=F->next()) {
Map<VisibilityNotifier*,uint64_t>::Element*G=F->get().notifiers.find(p_notifier);
if (G) {
F->get().notifiers.erase(G);
removed.push_back(F->key());
}
}
while(!removed.empty()) {
p_notifier->_exit_camera(removed.front()->get());
removed.pop_front();
}
changed=true;
}
int
main (int argc, char** argv)
{
OBJReader reader;
PointCloud cloud;
Octree octree (10);
reader.read (argv[1], cloud);
std::cout << "size:" << cloud.size () << std::endl;
octree.add (cloud);
std::cout << "leafs:" << octree.getLeafCount () << std::endl;
return 0;
}
Octree ct_to_octree(const char* path, int resolution, int z_offset = 84){
Octree octree;
octree.resolution = resolution;
const char* filepath = "./textures/ct.raw";
short int buffer;
FILE* file = fopen(filepath, "rb");
fseek(file, 0L, SEEK_END);
int size = ftell(file) / sizeof(buffer);
fseek(file, 0L, SEEK_SET);
printf("size:%d\n", size);
int x = 0;
int y = 0;
int z = z_offset;
for (int i = 0; i < size; i += 1){
fread((void*)(&buffer), sizeof(buffer), 1, file);
if (buffer > 300 && y < 340){
float value = log(float(buffer)) / 8.0;
float color[4] = {value, value, value, 1.0};
octree.set(x,y,z,color);
octree.set(x,y, z + 1,color);
}
x += 1;
if (x > 511){
x = 0;
y += 1;
}
if (y > 511){
y = 0;
z += 2;
}
//printf("buffer: %d\n", buffer);
}
fclose(file);
return octree;
}
void _update(uint64_t p_frame) {
if (p_frame == last_frame)
return;
last_frame = p_frame;
if (!changed)
return;
for (Map<Camera *, CameraData>::Element *E = cameras.front(); E; E = E->next()) {
pass++;
Camera *c = E->key();
Vector<Plane> planes = c->get_frustum();
int culled = octree.cull_convex(planes, cull.ptrw(), cull.size());
VisibilityNotifier **ptr = cull.ptrw();
List<VisibilityNotifier *> added;
List<VisibilityNotifier *> removed;
for (int i = 0; i < culled; i++) {
//notifiers in frustum
Map<VisibilityNotifier *, uint64_t>::Element *H = E->get().notifiers.find(ptr[i]);
if (!H) {
E->get().notifiers.insert(ptr[i], pass);
added.push_back(ptr[i]);
} else {
H->get() = pass;
}
}
for (Map<VisibilityNotifier *, uint64_t>::Element *F = E->get().notifiers.front(); F; F = F->next()) {
if (F->get() != pass)
removed.push_back(F->key());
}
while (!added.empty()) {
added.front()->get()->_enter_camera(E->key());
added.pop_front();
}
while (!removed.empty()) {
E->get().notifiers.erase(removed.front()->get());
removed.front()->get()->_exit_camera(E->key());
removed.pop_front();
}
}
changed = false;
}
TracePixel traceRay(RayPixel ray, Octree& tree){
vector<int> leaves;
leaves.reserve(512);
tree.trace(leaves,ray.r,false);
float sum = 0;
for (int k = 0; k < leaves.size(); k++) {
sum += tree.nodes[leaves[k]].val;
}
return TracePixel(ray.x, ray.y, sum);
}
void TestOctree(const std::string &Filename)
{
vtkSmartPointer<vtkXMLPolyDataReader> reader = vtkSmartPointer<vtkXMLPolyDataReader>::New();
reader->SetFileName(Filename.c_str());
reader->Update();
vtkSmartPointer<vtkPolyData> polydata = reader->GetOutput();
Octree Tree;
Tree.Build(polydata);
OrientedPoint Intersection;
//Ray R(vgl_point_3d<double>(0.5, 0.5, -1.0), vgl_vector_3d<double> (0.0, 0.0, 1.0));
Ray R(vgl_point_3d<double>(10.0, 0.0, 0.0), vgl_vector_3d<double> (-1.0, 0.0, 0.0));
bool intersect = Tree.IntersectRay(R, Intersection);
std::cout << "Intersect? " << intersect << std::endl;
if(intersect)
std::cout << "Intersection: " << Intersection << std::endl;
}
void _notifier_update(VisibilityNotifier* p_notifier,const AABB& p_rect) {
Map<VisibilityNotifier*,NotifierData>::Element *E=notifiers.find(p_notifier);
ERR_FAIL_COND(!E);
if (E->get().aabb==p_rect)
return;
E->get().aabb=p_rect;
octree.move(E->get().id,E->get().aabb);
changed=true;
}
void sure::keypoints::allocateEntropyPayload(Octree& octree, Scalar samplingrate, sure::memory::FixedSizeAllocatorWithDirectAccess<EntropyPayload>& allocator)
{
unsigned depth = octree.getDepth(samplingrate);
allocator.resizeIfSmaller(octree[depth].size());
for(unsigned int i=0; i<octree[depth].size(); ++i)
{
Node* node = octree[depth][i];
EntropyPayload* payload = allocator.allocate(i);
node->setOptionalPayload(static_cast<sure::payload::Payload*>(payload));
}
}
