I am attempting to find a simple way in SceneKit to calculate the depth of a pixels in SceneKit and LiDAR data from
sceneView.session.currentFrame?.smoothedSceneDepth?.depthMap
Ideally I don't want to use metal shaders. I would prefer find a points in my currentFrame and their corresponding depth map, to get the depth of a points in SceneKit (ideally in world coordinates, not just local to that frustum at that point in time).
Fast performance isn't necessary as it won't be calculated at capture.
I am aware of the Apple project at link, however this is far too complex for my needs.
As a starting point, my code works like this:
guard let depthData = frame.sceneDepth else { return }
let camera = frame.camera
let depthPixelBuffer = depthData.depthMap
let depthHeight = CVPixelBufferGetHeight(depthPixelBuffer)
let depthWidth = CVPixelBufferGetWidth(depthPixelBuffer)
let resizeScale = CGFloat(depthWidth) / CGFloat(CVPixelBufferGetWidth(frame.capturedImage))
let resizedColorImage = frame.capturedImage.toCGImage(scale: resizeScale);
guard let colorData = resizedColorImage.pixelData() else {
fatalError()
}
var intrinsics = camera.intrinsics;
let referenceDimensions = camera.imageResolution;
let ratio = Float(referenceDimensions.width) / Float(depthWidth)
intrinsics.columns.0[0] /= ratio
intrinsics.columns.1[1] /= ratio
intrinsics.columns.2[0] /= ratio
intrinsics.columns.2[1] /= ratio
var points: [SCNVector3] = []
let depthValues = depthPixelBuffer.depthValues()
for vv in 0..<depthHeight {
for uu in 0..<depthWidth {
let z = -depthValues[uu + vv * depthWidth]
let x = Float32(uu) / Float32(depthWidth) * 2.0 - 1.0;
let y = 1.0 - Float32(vv) / Float32(depthHeight) * 2.0;
points.append(SCNVector3(x, y, z))
}
}
The resulting point cloud looks ok, but is severely bent on the Z-axis. I realize this code is also not adjusting for screen orientation either.
Cupertino kindly got back to me with this response on the forums at developer.apple.com:
The unprojection calculation itself is going to be identical, regardless of whether it is done CPU side or GPU side.
CPU side, the calculation would look something like this:
/// Returns a world space position given a point in the camera image, the eye space depth (sampled/read from the corresponding point in the depth image), the inverse camera intrinsics, and the inverse view matrix.
func worldPoint(cameraPoint: SIMD2<Float>, eyeDepth: Float, cameraIntrinsicsInversed: simd_float3x3, viewMatrixInversed: simd_float4x4) -> SIMD3<Float> {
let localPoint = cameraIntrinsicsInversed * simd_float3(cameraPoint, 1) * -eyeDepth
let worldPoint = viewMatrixInversed * simd_float4(localPoint, 1);
return (worldPoint / worldPoint.w)[SIMD3(0,1,2)];
}
Implemented, this looks like
for vv in 0..<depthHeight {
for uu in 0..<depthWidth {
let z = -depthValues[uu + vv * depthWidth]
let viewMatInverted = (sceneView.session.currentFrame?.camera.viewMatrix(for: UIApplication.shared.statusBarOrientation))!.inverse
let worldPoint = worldPoint(cameraPoint: SIMD2(Float(uu), Float(vv)), eyeDepth: z, cameraIntrinsicsInversed: intrinsics.inverse, viewMatrixInversed: viewMatInverted * rotateToARCamera )
points.append(SCNVector3(worldPoint))
}
}
The point cloud is pretty messy, needs confidence worked out, and there are gaps vertically where Int rounding has occurred, but it's a solid start. Missing functions come from the link to the Apple demo project in the question above.
I tested my code in Playground, but as the discussion points out, that Playground is debug configuration, once I put all those code in real app running, they don't make a big difference. Don't know about this debug/release thing before.
Swift performance related question, I need to loop through the pixel offset of images, first I attempted it in this way.
func p1() -> [[Int]]{
var offsets = [[Int]]()
for row in 0..<height {
var rowOffset = [Int]()
for col in 0..<width {
let offset = width * row + col
rowOffset.append(offset)
}
offsets.append(rowOffset)
}
return offsets
}
But it is very slow, I searched and found some code snippet loop through offset this way:
func p2() -> [[Int]]{
return (0..<height).map{ row in
(0..<width).map { col in
let offset = width * row + col
return offset
}
}
}
So I tested if I use function p1 and p2 to loop through height = 128 and width = 128 image , p1 is 18 times slower than p2, why p1 is so slow compared with p2 ? also I'm wondering is there any other faster approach for this task?
The most obvious reason why the map approach is faster is because map allocates the array capacity up front (since it knows how many elements will be in the resulting array). You can do this too in your code by calling ary.reserveCapacity(n) on your arrays, e.g.
func p1() -> [[Int]]{
var offsets = [[Int]]()
offsets.reserveCapacity(height) // NEW LINE
for row in 0..<height {
var rowOffset = [Int]()
rowOffset.reserveCapacity(width) // NEW LINE
for col in 0..<width {
let offset = width * row + col
rowOffset.append(offset)
}
offsets.append(rowOffset)
}
return offsets
}
I have a block that is passing data in that I'd like to convert to an array of array of floats -- e.g. [[0.1,0.2,0.3, 1.0], [0.3, 0.4, 0.5, 1.0], [0.5, 0.6, 0.7, 1.0]]. This data is passed to me in the form of data:UnsafeMutablePointer<UnsafeMutableRawPointer> (The inner arrays are RGBA values)
fwiw -- the block parameters are from SCNParticleEventBlock
How can I dereference data into a [[Float]]? Once I have the array containing the inner arrays, I can reference the inner array (colorArray) data with:
let rgba: UnsafeMutablePointer<Float> = UnsafeMutablePointer(mutating: colorArray)
let count = 4
for i in 0..<count {
print((rgba+i).pointee)
}
fwiw -- this is Apple's example Objective-C code for referencing the data (from SCNParticleSystem handle(_:forProperties:handler:) )
[system handleEvent:SCNParticleEventBirth
forProperties:#[SCNParticlePropertyColor]
withBlock:^(void **data, size_t *dataStride, uint32_t *indices , NSInteger count) {
for (NSInteger i = 0; i < count; ++i) {
float *color = (float *)((char *)data[0] + dataStride[0] * i);
if (rand() & 0x1) { // Switch the green and red color components.
color[0] = color[1];
color[1] = 0;
}
}
}];
You can actually subscript the typed UnsafeMutablePointer without having to create an UnsafeMutableBufferPointer, as in:
let colorsPointer:UnsafeMutableRawPointer = data[0] + dataStride[0] * i
let rgbaBuffer = colorsPointer.bindMemory(to: Float.self, capacity: dataStride[0])
if(arc4random_uniform(2) == 1) {
rgbaBuffer[0] = rgbaBuffer[1]
rgbaBuffer[1] = 0
}
Were you ever able to get your solution to work? It appears only a handful of SCNParticleProperties can be used within an SCNParticleEventBlock block.
Based on this answer, I've written the particle system handler function in swift as:
ps.handle(SCNParticleEvent.birth, forProperties [SCNParticleSystem.ParticleProperty.color]) {
(data:UnsafeMutablePointer<UnsafeMutableRawPointer>, dataStride:UnsafeMutablePointer<Int>, indicies:UnsafeMutablePointer<UInt32>?, count:Int) in
for i in 0..<count {
// get an UnsafeMutableRawPointer to the i-th rgba element in the data
let colorsPointer:UnsafeMutableRawPointer = data[0] + dataStride[0] * i
// convert the UnsafeMutableRawPointer to a typed pointer by binding it to a type:
let floatPtr = colorsPointer.bindMemory(to: Float.self, capacity: dataStride[0])
// convert that to a an UnsafeMutableBufferPointer
var rgbaBuffer = UnsafeMutableBufferPointer(start: floatPtr, count: dataStride[0])
// At this point, I could convert the buffer to an Array, but doing so copies the data into the array and any changes made in the array are not reflected in the original data. UnsafeMutableBufferPointer are subscriptable, nice.
//var rgbaArray = Array(rgbaBuffer)
// about half the time, mess with the red and green components
if(arc4random_uniform(2) == 1) {
rgbaBuffer[0] = rgbaBuffer[1]
rgbaBuffer[1] = 0
}
}
}
I'm really not certain if this is the most direct way to go about this and seems rather cumbersome compared to the objective-C code (see above question). I'm certainly open to other solutions and/or comments on this solution.
I saw the below algorithm works to check if a point is in a given polygon from this link:
int pnpoly(int nvert, float *vertx, float *verty, float testx, float testy)
{
int i, j, c = 0;
for (i = 0, j = nvert-1; i < nvert; j = i++) {
if ( ((verty[i]>testy) != (verty[j]>testy)) &&
(testx < (vertx[j]-vertx[i]) * (testy-verty[i]) / (verty[j]-verty[i]) + vertx[i]) )
c = !c;
}
return c;
}
I tried this algorithm and it actually works just perfect. But sadly I cannot understand it well after spending some time trying to get the idea of it.
So if someone is able to understand this algorithm, please explain it to me a little.
Thank you.
The algorithm is ray-casting to the right. Each iteration of the loop, the test point is checked against one of the polygon's edges. The first line of the if-test succeeds if the point's y-coord is within the edge's scope. The second line checks whether the test point is to the left of the line (I think - I haven't got any scrap paper to hand to check). If that is true the line drawn rightwards from the test point crosses that edge.
By repeatedly inverting the value of c, the algorithm counts how many times the rightward line crosses the polygon. If it crosses an odd number of times, then the point is inside; if an even number, the point is outside.
I would have concerns with a) the accuracy of floating-point arithmetic, and b) the effects of having a horizontal edge, or a test point with the same y-coord as a vertex, though.
Edit 1/30/2022: I wrote this answer 9 years ago when I was in college. People in the chat conversation are indicating it's not accurate. You should probably look elsewhere. 🤷♂️
Chowlett is correct in every way, shape, and form.
The algorithm assumes that if your point is on the line of the polygon, then that is outside - for some cases, this is false. Changing the two '>' operators to '>=' and changing '<' to '<=' will fix that.
bool PointInPolygon(Point point, Polygon polygon) {
vector<Point> points = polygon.getPoints();
int i, j, nvert = points.size();
bool c = false;
for(i = 0, j = nvert - 1; i < nvert; j = i++) {
if( ( (points[i].y >= point.y ) != (points[j].y >= point.y) ) &&
(point.x <= (points[j].x - points[i].x) * (point.y - points[i].y) / (points[j].y - points[i].y) + points[i].x)
)
c = !c;
}
return c;
}
I changed the original code to make it a little more readable (also this uses Eigen). The algorithm is identical.
// This uses the ray-casting algorithm to decide whether the point is inside
// the given polygon. See https://en.wikipedia.org/wiki/Point_in_polygon#Ray_casting_algorithm
bool pnpoly(const Eigen::MatrixX2d &poly, float x, float y)
{
// If we never cross any lines we're inside.
bool inside = false;
// Loop through all the edges.
for (int i = 0; i < poly.rows(); ++i)
{
// i is the index of the first vertex, j is the next one.
// The original code uses a too-clever trick for this.
int j = (i + 1) % poly.rows();
// The vertices of the edge we are checking.
double xp0 = poly(i, 0);
double yp0 = poly(i, 1);
double xp1 = poly(j, 0);
double yp1 = poly(j, 1);
// Check whether the edge intersects a line from (-inf,y) to (x,y).
// First check if the line crosses the horizontal line at y in either direction.
if ((yp0 <= y) && (yp1 > y) || (yp1 <= y) && (yp0 > y))
{
// If so, get the point where it crosses that line. This is a simple solution
// to a linear equation. Note that we can't get a division by zero here -
// if yp1 == yp0 then the above if will be false.
double cross = (xp1 - xp0) * (y - yp0) / (yp1 - yp0) + xp0;
// Finally check if it crosses to the left of our test point. You could equally
// do right and it should give the same result.
if (cross < x)
inside = !inside;
}
}
return inside;
}
To expand on the "too-clever trick". We want to iterate over all adjacent vertices, like this (imagine there are 4 vertices):
i
j
0
1
1
2
2
3
3
0
My code above does it the simple obvious way - j = (i + 1) % num_vertices. However this uses integer division which is much much slower than all other operations. So if this is performance critical (e.g. in an AAA game) you want to avoid it.
The original code changes the order of iteration a bit:
i
j
0
3
1
0
2
1
3
2
This is still totally valid since we're still iterating over every vertex pair and it doesn't really matter whether you go clockwise or anticlockwise, or where you start. However now it lets us avoid the integer division. In easy-to-understand form:
int i = 0;
int j = num_vertices - 1; // 3
while (i < num_vertices) { // 4
{body}
j = i;
++i;
}
Or in very terse C style:
for (int i = 0, j = num_vertices - 1; i < num_vertices; j = i++) {
{body}
}
This might be as detailed as it might get for explaining the ray-tracing algorithm in actual code. It might not be optimized but that must always come after a complete grasp of the system.
//method to check if a Coordinate is located in a polygon
public boolean checkIsInPolygon(ArrayList<Coordinate> poly){
//this method uses the ray tracing algorithm to determine if the point is in the polygon
int nPoints=poly.size();
int j=-999;
int i=-999;
boolean locatedInPolygon=false;
for(i=0;i<(nPoints);i++){
//repeat loop for all sets of points
if(i==(nPoints-1)){
//if i is the last vertex, let j be the first vertex
j= 0;
}else{
//for all-else, let j=(i+1)th vertex
j=i+1;
}
float vertY_i= (float)poly.get(i).getY();
float vertX_i= (float)poly.get(i).getX();
float vertY_j= (float)poly.get(j).getY();
float vertX_j= (float)poly.get(j).getX();
float testX = (float)this.getX();
float testY = (float)this.getY();
// following statement checks if testPoint.Y is below Y-coord of i-th vertex
boolean belowLowY=vertY_i>testY;
// following statement checks if testPoint.Y is below Y-coord of i+1-th vertex
boolean belowHighY=vertY_j>testY;
/* following statement is true if testPoint.Y satisfies either (only one is possible)
-->(i).Y < testPoint.Y < (i+1).Y OR
-->(i).Y > testPoint.Y > (i+1).Y
(Note)
Both of the conditions indicate that a point is located within the edges of the Y-th coordinate
of the (i)-th and the (i+1)- th vertices of the polygon. If neither of the above
conditions is satisfied, then it is assured that a semi-infinite horizontal line draw
to the right from the testpoint will NOT cross the line that connects vertices i and i+1
of the polygon
*/
boolean withinYsEdges= belowLowY != belowHighY;
if( withinYsEdges){
// this is the slope of the line that connects vertices i and i+1 of the polygon
float slopeOfLine = ( vertX_j-vertX_i )/ (vertY_j-vertY_i) ;
// this looks up the x-coord of a point lying on the above line, given its y-coord
float pointOnLine = ( slopeOfLine* (testY - vertY_i) )+vertX_i;
//checks to see if x-coord of testPoint is smaller than the point on the line with the same y-coord
boolean isLeftToLine= testX < pointOnLine;
if(isLeftToLine){
//this statement changes true to false (and vice-versa)
locatedInPolygon= !locatedInPolygon;
}//end if (isLeftToLine)
}//end if (withinYsEdges
}
return locatedInPolygon;
}
Just one word about optimization: It isn't true that the shortest (and/or the tersest) code is the fastest implemented. It is a much faster process to read and store an element from an array and use it (possibly) many times within the execution of the block of code than to access the array each time it is required. This is especially significant if the array is extremely large. In my opinion, by storing each term of an array in a well-named variable, it is also easier to assess its purpose and thus form a much more readable code. Just my two cents...
The algorithm is stripped down to the most necessary elements. After it was developed and tested all unnecessary stuff has been removed. As result you can't undertand it easily but it does the job and also in very good performance.
I took the liberty to translate it to ActionScript-3:
// not optimized yet (nvert could be left out)
public static function pnpoly(nvert: int, vertx: Array, verty: Array, x: Number, y: Number): Boolean
{
var i: int, j: int;
var c: Boolean = false;
for (i = 0, j = nvert - 1; i < nvert; j = i++)
{
if (((verty[i] > y) != (verty[j] > y)) && (x < (vertx[j] - vertx[i]) * (y - verty[i]) / (verty[j] - verty[i]) + vertx[i]))
c = !c;
}
return c;
}
This algorithm works in any closed polygon as long as the polygon's sides don't cross. Triangle, pentagon, square, even a very curvy piecewise-linear rubber band that doesn't cross itself.
1) Define your polygon as a directed group of vectors. By this it is meant that every side of the polygon is described by a vector that goes from vertex an to vertex an+1. The vectors are so directed so that the head of one touches the tail of the next until the last vector touches the tail of the first.
2) Select the point to test inside or outside of the polygon.
3) For each vector Vn along the perimeter of the polygon find vector Dn that starts on the test point and ends at the tail of Vn. Calculate the vector Cn defined as DnXVn/DN*VN (X indicates cross product; * indicates dot product). Call the magnitude of Cn by the name Mn.
4) Add all Mn and call this quantity K.
5) If K is zero, the point is outside the polygon.
6) If K is not zero, the point is inside the polygon.
Theoretically, a point lying ON the edge of the polygon will produce an undefined result.
The geometrical meaning of K is the total angle that the flea sitting on our test point "saw" the ant walking at the edge of the polygon walk to the left minus the angle walked to the right. In a closed circuit, the ant ends where it started.
Outside of the polygon, regardless of location, the answer is zero.
Inside of the polygon, regardless of location, the answer is "one time around the point".
This method check whether the ray from the point (testx, testy) to O (0,0) cut the sides of the polygon or not .
There's a well-known conclusion here: if a ray from 1 point and cut the sides of a polygon for a odd time, that point will belong to the polygon, otherwise that point will be outside the polygon.
To expand on #chowlette's answer where the second line checks if the point is to the left of the line,
No derivation is given but this is what I worked out:
First it helps to imagine 2 basic cases:
the point is left of the line . / or
the point is right of the line / .
If our point were to shoot a ray out horizontally where would it strike the line segment. Is our point to the left or right of it? Inside or out? We know its y coordinate because it's by definition the same as the point. What would the x coordinate be?
Take your traditional line formula y = mx + b. m is the rise over the run. Here, instead we are trying to find the x coordinate of the point on that line segment that has the same height (y) as our point.
So we solve for x: x = (y - b)/m. m is rise over run, so this becomes run over rise or (yj - yi)/(xj - xi) becomes (xj - xi)/(yj - yi). b is the offset from origin. If we assume yi as the base for our coordinate system, b becomes yi. Our point testy is our input, subtracting yi turns the whole formula into an offset from yi.
We now have (xj - xi)/(yj - yi) or 1/m times y or (testy - yi): (xj - xi)(testy - yi)/(yj - yi) but testx isn't based to yi so we add it back in order to compare the two ( or zero testx as well )
I think the basic idea is to calculate vectors from the point, one per edge of the polygon. If vector crosses one edge, then the point is within the polygon. By concave polygons if it crosses an odd number of edges it is inside as well (disclaimer: although not sure if it works for all concave polygons).
This is the algorithm I use, but I added a bit of preprocessing trickery to speed it up. My polygons have ~1000 edges and they don't change, but I need to look up whether the cursor is inside one on every mouse move.
I basically split the height of the bounding rectangle to equal length intervals and for each of these intervals I compile the list of edges that lie within/intersect with it.
When I need to look up a point, I can calculate - in O(1) time - which interval it is in and then I only need to test those edges that are in the interval's list.
I used 256 intervals and this reduced the number of edges I need to test to 2-10 instead of ~1000.
Here's a php implementation of this:
<?php
class Point2D {
public $x;
public $y;
function __construct($x, $y) {
$this->x = $x;
$this->y = $y;
}
function x() {
return $this->x;
}
function y() {
return $this->y;
}
}
class Point {
protected $vertices;
function __construct($vertices) {
$this->vertices = $vertices;
}
//Determines if the specified point is within the polygon.
function pointInPolygon($point) {
/* #var $point Point2D */
$poly_vertices = $this->vertices;
$num_of_vertices = count($poly_vertices);
$edge_error = 1.192092896e-07;
$r = false;
for ($i = 0, $j = $num_of_vertices - 1; $i < $num_of_vertices; $j = $i++) {
/* #var $current_vertex_i Point2D */
/* #var $current_vertex_j Point2D */
$current_vertex_i = $poly_vertices[$i];
$current_vertex_j = $poly_vertices[$j];
if (abs($current_vertex_i->y - $current_vertex_j->y) <= $edge_error && abs($current_vertex_j->y - $point->y) <= $edge_error && ($current_vertex_i->x >= $point->x) != ($current_vertex_j->x >= $point->x)) {
return true;
}
if ($current_vertex_i->y > $point->y != $current_vertex_j->y > $point->y) {
$c = ($current_vertex_j->x - $current_vertex_i->x) * ($point->y - $current_vertex_i->y) / ($current_vertex_j->y - $current_vertex_i->y) + $current_vertex_i->x;
if (abs($point->x - $c) <= $edge_error) {
return true;
}
if ($point->x < $c) {
$r = !$r;
}
}
}
return $r;
}
Test Run:
<?php
$vertices = array();
array_push($vertices, new Point2D(120, 40));
array_push($vertices, new Point2D(260, 40));
array_push($vertices, new Point2D(45, 170));
array_push($vertices, new Point2D(335, 170));
array_push($vertices, new Point2D(120, 300));
array_push($vertices, new Point2D(260, 300));
$Point = new Point($vertices);
$point_to_find = new Point2D(190, 170);
$isPointInPolygon = $Point->pointInPolygon($point_to_find);
echo $isPointInPolygon;
var_dump($isPointInPolygon);
I modified the code to check whether the point is in a polygon, including the point is on an edge.
bool point_in_polygon_check_edge(const vec<double, 2>& v, vec<double, 2> polygon[], int point_count, double edge_error = 1.192092896e-07f)
{
const static int x = 0;
const static int y = 1;
int i, j;
bool r = false;
for (i = 0, j = point_count - 1; i < point_count; j = i++)
{
const vec<double, 2>& pi = polygon[i);
const vec<double, 2>& pj = polygon[j];
if (fabs(pi[y] - pj[y]) <= edge_error && fabs(pj[y] - v[y]) <= edge_error && (pi[x] >= v[x]) != (pj[x] >= v[x]))
{
return true;
}
if ((pi[y] > v[y]) != (pj[y] > v[y]))
{
double c = (pj[x] - pi[x]) * (v[y] - pi[y]) / (pj[y] - pi[y]) + pi[x];
if (fabs(v[x] - c) <= edge_error)
{
return true;
}
if (v[x] < c)
{
r = !r;
}
}
}
return r;
}
I want to divide the Google map display into 200 parts , I have this code
bounds = map.getBounds();
southWest = bounds.getSouthWest();
northEast = bounds.getNorthEast();
tileWidth = (northEast.lng() - southWest.lng()) / 10;
tileHeight = (northEast.lat() - southWest.lat()) / 20;
for (x=0; x < 20 ; x++)
{
for (y=0; y < 10 ; y++)
{
var x1 = southWest.lat()+ (tileHeight * x);
var y1 = southWest.lng()+ (tileWidth * y);
var x2 = x1 + tileHeight;
var y2 = y1 + tileWidth;
var tempCell = new GLatLngBounds(new GLatLng(x1, y1), new GLatLng(x2, y2));
}
}
I just cant figure out what is wrong with it...
Any Idea ??
I tried the code you posted - it seems to work just fine. The problem is probably elsewhere in your code. Can you post more details?
It is worthwhile to note, however, that this code will fail in spectacular fashion if the bounds include the international date line. Let us know if this is the problem.
I can't help but notice you use tempCell to hold the result, but what is done after that? do you ever refer to those bounded regions again?