I am currently using a Project Tango tablet for robotic obstacle avoidance. I want to create a matrix of z-values as they would appear on the Tango screen, so that I can use OpenCV to process the matrix. When I say z-values, I mean the distance each point is from the Tango. However, I don't know how to extract the z-values from the TangoXyzIjData and organize the values into a matrix. This is the code I have so far:
public void action(TangoPoseData poseData, TangoXyzIjData depthData) {
byte[] buffer = new byte[depthData.xyzCount * 3 * 4];
FileInputStream fileStream = new FileInputStream(
depthData.xyzParcelFileDescriptor.getFileDescriptor());
try {
fileStream.read(buffer, depthData.xyzParcelFileDescriptorOffset, buffer.length);
fileStream.close();
} catch (IOException e) {
e.printStackTrace();
}
Mat m = new Mat(depthData.ijRows, depthData.ijCols, CvType.CV_8UC1);
m.put(0, 0, buffer);
}
Does anyone know how to do this? I would really appreciate help.
The short answer is it can't be done, at least not simply. The XYZij struct in the Tango API does not work completely yet. There is no "ij" data. Your retrieval of buffer will work as you have it coded. The contents are a set of X, Y, Z values for measured depth points, roughly 10000+ each callback. Each X, Y, and Z value is of type float, so not CV_8UC1. The problem is that the points are not ordered in any way, so they do not correspond to an "image" or xy raster. They are a random list of depth points. There are ways to get them into some xy order, but it is not straightforward. I have done both of these:
render them to an image, with the depth encoded as color, and pull out the image as pixels
use the model/view/perspective from OpenGL and multiply out the locations of each point and then figure out their screen space location (like OpenGL would during rendering). Sort the points by their xy screen space. Instead of the calculated screen-space depth just keep the Z value from the original buffer.
or
wait until (if) the XYZij struct is fixed so that it returns ij values.
I too wish to use Tango for object avoidance for robotics. I've had some success by simplifying the use case to be only interested in the distance of any object located at the center view of the Tango device.
In Java:
private Double centerCoordinateMax = 0.020;
private TangoXyzIjData xyzIjData;
final FloatBuffer xyz = xyzIjData.xyz;
double cumulativeZ = 0.0;
int numberOfPoints = 0;
for (int i = 0; i < xyzIjData.xyzCount; i += 3) {
float x = xyz.get(i);
float y = xyz.get(i + 1);
if (Math.abs(x) < centerCoordinateMax &&
Math.abs(y) < centerCoordinateMax) {
float z = xyz.get(i + 2);
cumulativeZ += z;
numberOfPoints++;
}
}
Double distanceInMeters;
if (numberOfPoints > 0) {
distanceInMeters = cumulativeZ / numberOfPoints;
} else {
distanceInMeters = null;
}
Said simply this code is taking the average distance of a small square located at the origin of x and y axes.
centerCoordinateMax = 0.020 was determined to work based on observation and testing. The square typically contains 50 points in ideal conditions and fewer when held close to the floor.
I've tested this using version 2 of my tango-caminada application and the depth measuring seems quite accurate. Standing 1/2 meter from a doorway I slid towards the open door and the distance changed form 0.5 meters to 2.5 meters which is the wall at the end of the hallway.
Simulating a robot being navigated I moved the device towards a trash can in the path until 0.5 meters separation and then rotated left until the distance was more than 0.5 meters and proceeded forward. An oversimplified simulation, but the basis for object avoidance using Tango depth perception.
You can do this by using camera intrinsics to convert XY coordinates to normalized values -- see this post - Google Tango: Aligning Depth and Color Frames - it's talking about texture coordinates but it's exactly the same problem
Once normalized, move to screen space x[1280,720] and then the Z coordinate can be used to generate a pixel value for openCV to chew on. You'll need to decide how to color pixels that don't correspond to depth points on your own, and advisedly, before you use the depth information to further colorize pixels.
The main thing is to remember that the raw coordinates returned are already using the basis vectors you want, i.e. you do not want the pose attitude or location
So, here is my situation. I have created a object detection program which is based on color object detection. My program detects the color red and it works perfectly. But here is the problems i am facing:-
Whenever there are more than one red object in the surrounding, my program detects them and it cannot really track one object at that time(i.e it tracks other red objects of various sizes in the background. It shows me the error that "too much noise in the background". As you can see in the "threshold image" attached, it detects the round object (which is my tracking object) and my cap which is red in color. I want my program to detect only my tracking object("which is a round shaped coke cap"). How can i achieve that? Please help me out. I have my engineering design contest in few days and i have to demo my program infront of my lecturers. My program should only be able to detect and track the object which i want. Thanks
My code for the objectdetection program is a little long. So, i am hereby explaining the code as follows- I captured a frame from the webcam frame-converted it to HSV- used HSV Inrange filter to filter out the other colors but red- applied morphological operations on the filtered image. This all goes in my main function
I am using a frame resolution of 1280*720 for my webcam frame. It kind of slows down my program but it was a trade off which i had to do for performing gesture controlled operations. Anyways here is my drawobjectfunction and trackfilteredobjectfunction.
int H_MIN = 0;
int H_MAX = 256;
int S_MIN = 0;
int S_MAX = 256;
int V_MIN = 0;
int V_MAX = 256;
//default capture width and height
const int FRAME_WIDTH = 1280;
const int FRAME_HEIGHT = 720;
//max number of objects to be detected in frame
const int MAX_NUM_OBJECTS=50;
//minimum and maximum object area
const int MIN_OBJECT_AREA = 20*20;
const int MAX_OBJECT_AREA = FRAME_HEIGHT*FRAME_WIDTH/1.5;
void drawObject(int x, int y,Mat &frame){
circle(frame,Point(x,y),20,Scalar(0,255,0),2);
if(y-25>0)
line(frame,Point(x,y),Point(x,y-25),Scalar(0,255,0),2);
else line(frame,Point(x,y),Point(x,0),Scalar(0,255,0),2);
if(y+25<FRAME_HEIGHT)
line(frame,Point(x,y),Point(x,y+25),Scalar(0,255,0),2);
else line(frame,Point(x,y),Point(x,FRAME_HEIGHT),Scalar(0,255,0),2);
if(x-25>0)
line(frame,Point(x,y),Point(x-25,y),Scalar(0,255,0),2);
else line(frame,Point(x,y),Point(0,y),Scalar(0,255,0),2);
if(x+25<FRAME_WIDTH)
line(frame,Point(x,y),Point(x+25,y),Scalar(0,255,0),2);
else line(frame,Point(x,y),Point(FRAME_WIDTH,y),Scalar(0,255,0),2);
putText(frame,intToString(x)+","+intToString(y),Point(x,y+30),1,1,Scalar(0,255,0),2);
}
void trackFilteredObject(int &x, int &y, Mat threshold, Mat &cameraFeed){
Mat temp;
threshold.copyTo(temp);
//these two vectors needed for output of findContours
vector< vector<Point> > contours;
vector<Vec4i> hierarchy;
//find contours of filtered image using openCV findContours function
findContours(temp,contours,hierarchy,CV_RETR_CCOMP,CV_CHAIN_APPROX_SIMPLE );
//use moments method to find our filtered object
double refArea = 0;
bool objectFound = false;
if (hierarchy.size() > 0) {
int numObjects = hierarchy.size();
//if number of objects greater than MAX_NUM_OBJECTS we have a noisy filter
if(numObjects<MAX_NUM_OBJECTS){
for (int index = 0; index >= 0; index = hierarchy[index][0]) {
Moments moment = moments((cv::Mat)contours[index]);
double area = moment.m00;
//if the area is less than 20 px by 20px then it is probably just noise
//if the area is the same as the 3/2 of the image size, probably just a bad filter
//we only want the object with the largest area so we safe a reference area each
//iteration and compare it to the area in the next iteration.
if(area>MIN_OBJECT_AREA && area<MAX_OBJECT_AREA && area>refArea){
x = moment.m10/area;
y = moment.m01/area;
objectFound = true;
refArea = area;
}else objectFound = false;
}
//let user know you found an object
if(objectFound ==true){
putText(cameraFeed,"Tracking Object",Point(0,50),2,1,Scalar(0,255,0),2);
//draw object location on screen
drawObject(x,y,cameraFeed);}
}else putText(cameraFeed,"TOO MUCH NOISE! ADJUST FILTER",Point(0,50),1,2,Scalar(0,0,255),2);
}
}
Here is the link of the image; as you can see it also detects the red hat in the background along with the red cap of the coke bottle.
My observations:- Here is what i think, to achieve my desired goal of not detecting objects of unknown sizes of red color. I think i have to edit the value of maximum object area which i declared in the above program as (const int MAX_OBJECT_AREA = FRAME_HEIGHT*FRAME_WIDTH/1.5;). I think i have to change this value, that might eliminate the detection of bigger continous red pictures. But also, there is another problem some objects are not completely red in color and they have patches of red and other colors. So, if the detected area is within the range specfied in my program then my program detects those red patches too. What i mean to say is i was wearing a tshirt which has mixed colors and when i tested my program by wearing that tshirt, my program was able to detect the red color out of the other colors. Now, how do i solve this issue?
I think you can try out the following procedure:
obtain a circular kernel having roughly the same area as your object of interest. You can do it like: Mat kernel = getStructuringElement(MORPH_ELLIPSE, Size(d, d));
where d is the diameter of the disk.
perform normalized-cross-correlation or convolution of the filtered regions image with this kernel (I think normalized-cross-correlation would be better. And add an empty boarder around the kernel).
the peak of the resulting image should give you the location of the circular region in your filtered image (if you are using normalized-cross-correlation, you'll have to add the shift).
To speed things up, you can perform this at a reduced resolution.
You can filter out non-circular shapes by detecting circles in your thresholded image. OpenCV provides a built-on method to detect circles using Hough transform, more info here. You can take advantage of this function to retain only circles that have a radius in a given range.
Another possibility is to implement connected component labeling (CCL) into your demo program.
I believe that it was removed at some point in verions 2.x of OpenCV, but a basic implementation of the two-pass version is straightforward from the Wikipedia page.
CCL will assign a unique ID for each object after thresholding. You then have to implement matching between the objects at frame (T-1) and objects in frame (T) (for example based on some nearest distance criterion) and possibly trajectory filtering or smoothing, but this would definitely give you some extra-points.
I'm making an app that (among other things) displays a simplified compass image that rotates according to the device's rotation. The problem is that simply doing this:
float heading = -1.0f * M_PI * trueHeading / 180.0f; //trueHeading is always between 0 and 359, never 360
self.compassNeedle.transform = CGAffineTransformMakeRotation(heading);
inside CLLocationManager's didUpdateHeading method makes the animation ugly and choppy.
I have already used Instruments to find out whether its simply my app not being able to render at more than 30-48 fps, but that's not the case.
How can I smooth out the image view's rotation so that it's more like Apple's own Compass app?
Instead of using the current instant value, try using the average of the last N values for the true heading. The value may be jumping around a lot in a single instant but settle down "in the average".
Assuming you have a member variable storedReadings which is an NSMutableArray:
-(void)addReading(float):newReading
{
[storedReadings addObject:[NSNumber numberWithFloat:newReading]];
while([storedReadings count] > MAX_READINGS)
{
[storedReadings removeObjectAtIndex:0];
}
}
then when you need the average value (timer update?)
-(float)calcReading
{
float result = 0.0f;
if([storedReadings count] > 0)
{
foreach(NSNumber* reading in storedReadings)
{
result += [reading floatValue];
}
result /= [storedReadings count];
}
return result;
}
You get to pick MAX_READINGS a priori.
NEXT LEVEL(S) UP
If the readings are not jumping around so much but the animation is still choppy, you probably need to do something like a "smooth" rotation. At any given time, you have the current angle you are displaying, theta (store this in your class, start it out at 0). You also have your target angle, call it target. This is the value you get from the smoothed calcReading function. The error is defined as the difference between the two:
error = target-theta;
Set up a timer callback with a period of something like 0.05 seconds (20x per second). What you want to do is adjust theta so that the error is driven towards 0. You can do this in a couple of ways:
thetaNext += kProp * (target - theta); //This is proportional feedback.
thetaNext += kStep * sign(target-theta); // This moves theta a fixed amount each update. sign(x) = +1 if x >= 0 and -1 if x < 0.
The first solution will cause the rotation to change sharply the further it is from the target. It will also probably oscillate a little bit as it swings past the "zero" point. Bigger values of kProp will yield faster response but also more oscillation. Some tuning will be required.
The second solution will be much easier to control...it just "ticks" the compass needle around each time. You can set kStep to something like 1/4 degree, which gives you a "speed" of rotation of about (1/4 deg/update) * (20 updates/seconds) = 5 degrees per second. This is a bit slow, but you can see the math and change kStep to suit your needs. Note that you may to "band" the "error" value so that no action is taken if the error < kStep (or something like that). This prevents your compass from shifting when the angle is really close to the target. You can change kStep when the error is small so that it "slides" into the ending position (i.e. kStep is smaller when the error is small).
For dealing with Angle Issues (wrap around), I "normalize" the angle so it is always within -Pi/Pi. I don't guarantee this is the perfect way to do it, but it seems to get the job done:
// Takes an angle greater than +/- M_PI and converts it back
// to +/- M_PI. Useful in Box2D where angles continuously
// increase/decrease.
static inline float32 AdjustAngle(float32 angleRads)
{
if(angleRads > M_PI)
{
while(angleRads > M_PI)
{
angleRads -= 2*M_PI;
}
}
else if(angleRads < -M_PI)
{
while(angleRads < -M_PI)
{
angleRads += 2*M_PI;
}
}
return angleRads;
}
By doing it this way, -pi is the angle you reach from going in either direction as you continue to rotate left/right. That is to say, there is not a discontinuity in the number going from say 0 to 359 degrees.
SO PUTTING THIS ALL TOGETHER
static inline float Sign(float value)
{
if(value >= 0)
return 1.0f;
return -1.0f;
}
//#define ROTATION_OPTION_1
//#define ROTATION_OPTION_2
#define ROTATION_OPTION_3
-(void)updateArrow
{
// Calculate the angle to the player
CGPoint toPlayer = ccpSub(self.player.position,self.arrow.position);
// Calculate the angle of this...Note there are some inversions
// and the actual image is rotated 90 degrees so I had to offset it
// a bit.
float angleToPlayerRads = -atan2f(toPlayer.y, toPlayer.x);
angleToPlayerRads = AdjustAngle(angleToPlayerRads);
// This is the angle we "wish" the arrow would be pointing.
float targetAngle = CC_RADIANS_TO_DEGREES(angleToPlayerRads)+90;
float errorAngle = targetAngle-self.arrow.rotation;
CCLOG(#"Error Angle = %f",errorAngle);
#ifdef ROTATION_OPTION_1
// In this option, we just set the angle of the rotated sprite directly.
self.arrow.rotation = CC_RADIANS_TO_DEGREES(angleToPlayerRads)+90;
#endif
#ifdef ROTATION_OPTION_2
// In this option, we apply proportional feedback to the angle
// difference.
const float kProp = 0.05f;
self.arrow.rotation += kProp * (errorAngle);
#endif
#ifdef ROTATION_OPTION_3
// The step to take each update in degrees.
const float kStep = 4.0f;
// NOTE: Without the "if(fabs(...)) check, the angle
// can "dither" around the zero point when it is very close.
if(fabs(errorAngle) > kStep)
{
self.arrow.rotation += Sign(errorAngle)*kStep;
}
#endif
}
I put this code into a demo program I had written for Cocos2d. It shows a character (big box) being chased by some monsters (smaller boxes) and has an arrow in the center that always points towards the character. The updateArrow call is made on a timer tick (the update(dt) function) regularly. The player's position on the screen is set by the user tapping on the screen and the angle is based on the vector from the arrow to the player. In the function, I show all three options for setting the angle of the arrow:
Option 1
Just set it based on where the player is (i.e. just set it).
Option 2
Use proportional feedback to adjust the arrow's angle each time step.
Option 3
Step the angle of the arrow each timestep a little bit if the error angle is more than the step size.
Here is a picture showing roughly what it looks like:
And, all the code is available here on github. Just look in the HelloWorldLayer.m file.
Was this helpful?
I'm using open frameworks and opencv to track blobs on a webcam. I'm getting the x value of the blob centroid and tracking it. The problem is, it jumps around allot, I'm wondering if there is a better way to compute the average position over a certain number of frames and use that number it's all being computed in the draw() function.
void testApp::draw(){
ofVec2f centroid = contourFinder.blobs[0].centroid;
int width = ofGetWidth();
float pct = (float)centroid.x / (float)width;
float totFrame = fingerMovie.getTotalNumFrames ();
float gotFrame = totFrame * pct;
}
you should create a loop for N frames, sum all coordinates you get, then divide by N.
I am not experienced with ofx but there must be a function to get next frame.
After loop ends, move camera to the average coordinate and re-initialize the loop.
I'm recently playing with Away3D Library and have a problem in finding Face center in Away3D. Why Away3DLite has a face.center feature while Away3D doesn't have it ? and what is the alternative solution for this ?
If you want to find the center of a face, it's simply the average position of all the vertices making up that face:
function getFaceCenter(f : Face) : Vector3D
{
var vert : Vertex;
var ret : Vector3D = new Vector3D;
for each (vert in f.vertices) {
ret.x += vert.x;
ret.y += vert.y;
ret.z += vert.z;
}
ret.x /= f.vertices.length;
ret.y /= f.vertices.length;
ret.z /= f.vertices.length;
return ret;
}
The above is a very simple function to calculate an average, although on a 3D vector instead of a simple scalar number. That average is the center of all the vertices in the face.
If you need to do this a lot, optimize the method by preventing it from allocating a vector (by passing in a vector to which the return values should be written) and create a temporary variable for the vertex list length instead of dereferencing it through two object references like min (f and vertices), which is unnecessarily heavy.