i am looking for a way to collect a set of voxels. A voxel is a 3D cell that can be full/empty/unknown and is built upon a point cloud (for data reduction). The voxels collection once built is never modified (destroyed and rebuilt each round), but different kind of access are required (neighborood, iterative on all, direct).
The voxel space is very very sparse, out of order of 1.000.000 possible voxels in space only at most 1000 are used.
So i decided to use a (unordered since using c++)hashmap to collect them (an octree is an overkill i think) with the voxel ID as a key. Now i need a function to convert in both way a 3D point to voxel ID and the ID to the voxel 3D point centroid.
What i find hard is a very fast way to do it, i'd like to have they key as a single int value like:
unsigned int VoxelsMap::pointToVoxelId(const Vector3f & point){
unsigned int id = 0;
int x = (int)floor(roundpoint[0]);
int y = (int)floor(roundpoint[1]);
int z = (int)floor(roundpoint[2]);
id = A-BIJECTIVE-FUNCTION(x, y, z);
return id;
}
but for the bijective function i cant come up with anything very fast (as for the previous casts etc that i dont like for a function that must be used so often (200FPS x ~1000 x 3) ).
So:
is the hashmap a good datastructure (what worries me is the neighborood search)
what can be a function for A-BIJECTIVE-FUNCTION or for the whole function
Thanks.
#include <iostream>
using namespace std;
int main()
{
int x = 2.1474e+009;
int y = -2097152;
int z = -2048;
int rx = x;
int ry = y << 10;
int rz = z << 20;
int hashed = rx + ry + rz;
x = rx;
y = ry >> 10;
z = rz >> 20;
cout << hashed << endl;
cout << x << " " << y << " " << z << endl;
return 0;
}
This hash/unhash method should be the fastest.
Note that I only use 30 bits out of 32 bits of the integer. This allows a maxmimum world size of 4.2950e+009 x 4194304 x 4096. If you want to extend the world limits, you will have to use more/bigger integers.
Hope this can help.
Do you want to collect them space-wise in a way that links every adjacent voxel? If this is what you want, then you could use the Hoshen-Kopelman algorithm in 3D. Writing the code for that should take like a day or two, and you're done. The exmaple in the link is for 2D; expanding that to 3D isn't an issue at all.
Hope this helps.
Why not using a more elaborate key for your hashmap? Instead of a simple int, you could build a tuple with your x,y,z coordinates or implement you own struct. The later option would require implementing operator==() and a hash function. Some information about a good hash function can be found here.
Related
Problem: Generate a extrapolated local path which provides path points ahead of the max FOV.
Situation: Having a car move round an unknown looped track of varying shape using a field of view which is limited so can only provide reliably 3 points ahead of the car and the car's current position. Note for more information the tack is defined by cone gates and the information provided about the locations of said gates is 2D (x,y).
Background: I have successfully generated a vector of mid points between gates however wish to generate an extrapolated path for the motion control algorithm to use. the format of this path needs to be a sequence of PathPoint (s) which contain (x,y velocity, gravity). note that gravity is just used to cap the maximum acceleration and is not important to the situation nor is velocity as the post is only concerned about generating respective (x,y) co-ordinates.
Attempted Solution Methodology: To fit two cubic functions for X positions and Y positions using the set of four points i.e f(x) and g(y). These functions should then be provided as the desired (f(x),g(y)) positions as we vary the look ahead distance to supply 20 path points.
Question: I do not believe this method to be correct both in theory and in implementation can anyone think of an easy/simple methodology to achieve the out come of having position in x axis and position in y axis to be the functions from the argument of overall distance from the car?
double PathPlanningClass::squared(double arg)
{
return arg*arg;
}
double PathPlanningClass::cubed(double arg)
{
return arg*arg*arg;
}
//https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html
void PathPlanningClass::Coeffs()
{
Eigen::Matrix4f Aone;
Eigen::Vector4f bone;
Aone << _x, squared(_x), cubed(_x), _midpoints[0].getX(), squared(_midpoints[0].getX()), cubed(_midpoints[0].getX()), _midpoints[1].getX(), squared(_midpoints[1].getX()), cubed(_midpoints[1].getX()), _midpoints[_midpoints.size()-1].getX(), squared(_midpoints[_midpoints.size()-1].getX()), cubed(_midpoints[_midpoints.size()-1].getX());
bone << _y, _midpoints[0].getY(), _midpoints[1].getY(), _midpoints[_midpoints.size()-1].getY();
Eigen::Vector4f x = Aone.colPivHouseholderQr().solve(bone);
_Ax = x(1);
_Bx = x(2);
_Cx = x(3);
_Dx = x(4);
Eigen::Matrix4f Atwo;
Eigen::Vector4f btwo;
Atwo << _y, squared(_y), cubed(_y), _midpoints[0].getY(), squared(_midpoints[0].getY()), cubed(_midpoints[0].getY()), _midpoints[1].getY(), squared(_midpoints[1].getY()), cubed(_midpoints[1].getY()), _midpoints[_midpoints.size()-1].getY(), squared(_midpoints[_midpoints.size()-1].getY()), cubed(_midpoints[_midpoints.size()-1].getY());
btwo << _x, _midpoints[0].getX(), _midpoints[1].getX(), _midpoints[_midpoints.size()-1].getX();
Eigen::Vector4f y = Aone.colPivHouseholderQr().solve(bone);
_Ay = y(1);
_By = y(2);
_Cy = y(3);
_Dx = y(4);
return;
}
void PathPlanningClass::extrapolate()
{
// number of desired points
int numOfpoints = 20;
// distance to be extrapolated from car's location
double distance = 10;
// the argument for g(y) and f(x)
double arg = distance/numOfpoints;
for (int i = 0 ; i < numOfpoints; i++)
{
double farg = _Ax + _Bx*arg*i + _Cx*squared(arg*i) + _Dx*cubed(arg*i);
double garg = _Ay + _By*arg*i + _Cy*squared(arg*i) + _Dy*cubed(arg*i);
PathPoint newPoint(farg, garg, velocity(_x, _y, _yaw), 9.8);
_path.push_back(newPoint);
}
return;
}
Suppose I have a Mat of indices (locations) called B, We can say that this Mat has dimensions of 1 x 100 and We suppose to have another Mat, called A, full of data of the same dimensions of B.
Now, I would access to the data of A with B. Usually I would create a for loop and I would take for each elements of B, the right elements of A. For the most fussy of the site, this is the code that I would write:
for(int i=0; i < B.cols; i++){
int index = B.at<int>(0, i);
std::cout<<A.at<int>(0, index)<<std:endl;
}
Ok, now that I showed you what I could do, I ask you if there is a way to access the matrix A, always using the B indices, in a more intelligent and fast way. As someone could do in python thanks to the numpy.take() function.
This operation is called remapping. In OpenCV, you can use function cv::remap for this purpose.
Below I present the very basic example of how remap algorithm works; please note that I don't handle border conditions in this example, but cv::remap does - it allows you to use mirroring, clamping, etc. to specify what happens if the indices exceed the dimensions of the image. I also don't show how interpolation is done; check the cv::remap documentation that I've linked to above.
If you are going to use remapping you will probably have to convert indices to floating point; you will also have to introduce another array of indices that should be trivial (all equal to 0) if your image is one-dimensional. If this starts to represent a problem because of performance, I'd suggest you implement the 1-D remap equivalent yourself. But benchmark first before optimizing, of course.
For all the details, check the documentation, which covers everything you need to know to use te algorithm.
cv::Mat<float> remap_example(cv::Mat<float> image,
cv::Mat<float> positions_x,
cv::Mat<float> positions_y)
{
// sizes of positions arrays must be the same
int size_x = positions_x.cols;
int size_y = positions_x.rows;
auto out = cv::Mat<float>(size_y, size_x);
for(int y = 0; y < size_y; ++y)
for(int x = 0; x < size_x; ++x)
{
float ps_x = positions_x(x, y);
float ps_y = positions_y(x, y);
// use interpolation to determine intensity at image(ps_x, ps_y),
// at this point also handle border conditions
// float interpolated = bilinear_interpolation(image, ps_x, ps_y);
out(x, y) = interpolated;
}
return out;
}
One fast way is to use pointer for both A (data) and B (indexes).
const int* pA = A.ptr<int>(0);
const int* pIndexB = B.ptr<int>(0);
int sum = 0;
for(int i = 0; i < Bi.cols; ++i)
{
sum += pA[*pIndexB++];
}
Note: Be carefull with pixel type, in this case (as you write in your code) is int!
Note2: Using cout for each point access put the optimization useless!
Note3: In this article Satya compare four methods for pixel access and fastest seems "foreach": https://www.learnopencv.com/parallel-pixel-access-in-opencv-using-foreach/
I'm trying to use VLFeat's kmeans implementation in C but I'm having a really hard time understanding how it works.
Note: I am using the C API in a C++ program, so any code posted by me here is C++. Additionally, I am using the Eigean header library, so that's where those Matrix data types are coming from.
Things unclear to from the example and API are:
What format does the data have to be in? The kmeans library functions appear to require a one-dimensional array, which could be taken from the backing of a matrix. However, does this matrix need to be column major or row major? That is, how does the function know to differentiate between dimensions of data and different data vectors?
How do I actually access the cluster center info? I ran a test where I declared I wanted 5 clusters, but using their example code from the link above, I only return 1.
Code:
int numData = 1000;
int dims = 10;
// Use float data and the L1 distance for clustering
VlKMeans * kmeans = vl_kmeans_new (VL_TYPE_FLOAT, VlDistanceL1) ;
// Use Lloyd algorithm
vl_kmeans_set_algorithm (kmeans, VlKMeansLloyd) ;
// Initialize the cluster centers by randomly sampling the data
Matrix<float, 1000,10, RowMajor> data = buildData(numData, dims);
vl_kmeans_init_centers_with_rand_data (kmeans, data.data(), dims, numData, 5);
// Run at most 100 iterations of cluster refinement using Lloyd algorithm
vl_kmeans_set_max_num_iterations (kmeans, 100) ;
vl_kmeans_refine_centers (kmeans, &data, numData) ;
// Obtain the energy of the solution
energy = vl_kmeans_get_energy(kmeans) ;
// Obtain the cluster centers
centers = (double*)vl_kmeans_get_centers(kmeans);
cout << *centers << endl;
Example Output: centers = 0.0376879 (a scalar)
How do I get all centers? I tried using an array to store centers, but it won't accept the type.
I also tried the following, assuming that perhaps I was just accessing the center info wrong:
cout << centers[0]<< endl;
cout << centers[1]<< endl;
cout << centers[2]<< endl;
cout << centers[3]<< endl;
cout << centers[4]<< endl;
cout << centers[5]<< endl;
cout << centers[6]<< endl;
cout << centers[7]<< endl;
cout << centers[8]<< endl;
But I should only have none-zero values for indices 0-4 (given 5 cluster centers). I actually expected exceptions to be thrown for higher indices. If this is the right approach, could someone please explain to me what these other values (indices 5-8) are from?
I'm sure there are other confusing pieces as well, but I haven't even addressed them yet as I've been stuck on these two pretty important pieces (I mean what is kmeans if you can't cluster properly to start).
Thank you in advance for your help!
What format does the data have to be in?
The manual says:
All algorithms support float or double data and can use the l1 or the l2 distance for clustering.
You specify that when you create your kmeans handle, e.g:
VlKMeans *kmeans = vl_kmeans_new(VL_TYPE_FLOAT, VlDistanceL2);
does this matrix need to be column major or row major?
It must be in row major, i.e: data + dimension * i is the i-th center.
How do I actually access the cluster center info?
With vl_kmeans_get_centers. For example if you work with float-s:
/* no need to cast here since get centers returns a `void *` */
const float *centers = vl_kmeans_get_centers(kmeans);
(see this answer regarding the cast)
The total size (in bytes) of this array is sizeof(float) * dimension * numCenters. If you want to print out the centers you can do:
int i, j;
for (i = 0; i < numCenters; i++) {
printf("center # %d:\n", i);
for (j = 0; j < dimension; j++) {
printf(" coord[%d] = %f\n", j, centers[dimension * i + j]);
}
}
This one has me stumped. I have three classes - a large class called Level that contains, among other things, a multidimensional vector of pointers to Plant instances and a multidimensional vector of pointers to Mob instances; both vectors are meant to classify the mobs and plants according to their position, so that I can search for each in a smaller vector based on their approximate location, rather than cycling through all existing plants/mobs in order to find the one nearest to a given point.
The vectors are as follows, with the smallest std::vectors representing a square area of 128 pixels on each side. Mobs and plants are classed by dividing their X and Y coords by 128 and adding them to the proper sector (I am careful that the resulting values are in fact integers).
std::vector< std::vector< std::vector<Plant*> > >* m_PlantSectors
std::vector< std::vector< std::vector<Mob*> > >* m_AnimalSectors
The mobs sometimes need to find plants. Here is where the problem arises: when the mobs query the multidimensional vector, searching for plants in their approximate area (if the mob's coords/128 are, say, [1,2] it searches m_PlantSectors[2][1]), they sometimes find plants that don't exist.
Not only that, but these plants have impossible positions, on the order of 1.9777e+33 or 3.75853e-39 (for instance). When I try to change the color of the chosen plant to red in order to find it visually, I find that none of the plants on the screen (the only plants are ones I've hand-placed) have changed color.
I've marked all the plants with an integer ID; there are 36 plants, with IDs from 1-36, but the plants my mobs find have IDs like 63 or 429 - ones that can't possibly exist, since no such number of plants was created (there is a single plant creation function that consistently reports how many plants exist, so no plants are being accidentally created). The mobs all run off to the upper-left part of the screen after the imaginary plants, and die of starvation.
So somehow, I an creating ghost plants. So far, I've tried two separate approaches for allowing Mob instances to look for Plant instances. The first looks like this:
float TargetDist = 256 * 256;
Plant* Candidate = 0;
Plant* ForageTarget = 0;
int xSect = m_X / 128;
int ySect = m_Y / 128;
std::vector<Plant*> ThisSect = pLevel->CheckPSector(xSect, ySect);
for (int i = 0; i < ThisSect.size(); ++i)
{
cout << "Searching in Sector (" << ySect << ", " << xSect << ")\n";
Candidate = ThisSect[i];
cout << "Candidate at: " << Candidate->GetX() << ", " << Candidate->GetY() << "\n";
Candidate->Mark();
//Calculate distance
float xDist = Candidate->GetX() - m_X;
float yDist = Candidate->GetY() - m_Y;
float tDist = sqrt(xDist * xDist + yDist * yDist);
if (tDist <= TargetDist)
{
ForageTarget = Candidate;
TargetDist = tDist;
}
}
Where CheckPSector() looks like this:
std::vector<Plant*> Level::CheckPSector(int x, int y)
{
return m_PlantSectors[y][x];
}
The second thing I tried was this:
float TargetDist = 256 * 256;
Plant* Candidate = 0;
Plant* ForageTarget = 0;
int xSect = m_X / 128;
int ySect = m_Y / 128;
std::vector< std::vector< std::vector<Plant*> > >* Sectors = pLevel->AccessPlantSectors();
for (int i = 0; i < (*Sectors)[ySect][xSect].size(); ++i)
{
cout << "Searching in Sector (" << ySect << ", " << xSect << ")\n";
Candidate = (*Sectors)[ySect][xSect][i];
cout << "Candidate at: " << Candidate->GetX() << ", " << Candidate->GetY() << "\n";
Candidate->Mark();
//Calculate distance
float xDist = Candidate->GetX() - m_X;
float yDist = Candidate->GetY() - m_Y;
float tDist = sqrt(xDist * xDist + yDist * yDist);
if (tDist <= TargetDist)
{
ForageTarget = Candidate;
TargetDist = tDist;
}
}
Using this:
std::vector< std::vector< std::vector<Plant*> > >* Level::AccessPlantSectors()
{
return &m_PlantSectors;
}
Both of these, however, result in the animals finding imaginary plants and running off into the void.
I don't want to copy potentially large multidimensional vectors into Mob instances on a regular basis, because there will be many such instances at any one time, and I'd like the program to run somewhat smoothly. Even so, I just tried to do the whole thing by copying the entire vector instead of just the relevant one, and I get the same result: imaginary plants.
I've never quite had a problem like this before; what could be happening here?
EDIT: Perhaps I should mention that getting the chosen plants to self-report their own position and ID fails equally, turning up absurd results, so it isn't just the functions in Plant I use to access private members. Meanwhile, querying all existing plants does not reveal any with the information the ghost plants self-report.
If m_PlantSectors is defined as:
std::vector< std::vector< std::vector<Plant*> > >* m_PlantSectors
Then Level::AccessPlantSectors() should return m_PlantSectors, not &m_PlantSectors because you already have a pointer.
Likewise, Level::CheckPSector(int x, int y) should return (*m_PlantSectors)[y][x] because you need to deference the pointer before invoking the [] operator.
As you've written them, Level::CheckPSector(int x, int y) returns random memory and I'm surprised that Level::AccessPlantSectors() compiles.
I have a 3D array
double values[30][30][30];
I have a loop where I assign values to the array;
Something like:
for(int z = 0;z<30; z++)
for (int y = 0;y<30; y++)
for (int x = 0;x<30; x++)
values[z][y][x] = intensity;
end
So this is how I am filling the array. The problem is that I want to create column in addition to intensity to store another variable. For instance, the second to last line should be something like
values[z][y][x] = intensity | distance;
I hope you get the idea. My knowledge is limited and I couldn't come up with a solution. Thanks for your suggestions.
This is really dependant on your datatypes. The easiest solution is using a struct:
struct data {
float intensity; // or replace 'float' with whatever datatype you need
float distance;
};
Use this struct instead of the datatype you're using now for the array, then later on set the values:
values[z][y][x].intensity = intensity;
values[z][y][x].distance = distance;
If you're using small values only (e.g. char for each value only) you could as well use bitwise operators to store everything in an integer:
values[z][y][x] = intensity << 8 | distance;
intensity = values[z][y][x] >> 8;
distance = values[z][y][x] & 255;
But I wouldn't advise you to do so unless you're really savy with that value ranges (e.g. for saving bitmap/texture stuff).