I have detected vehicles as a blob in OpenCV. Below is the blob.h file
class Blob {
public:
// member variables
std::vector<cv::Point> currentContour;
cv::Rect currentBoundingRect;
std::vector<cv::Point> centerPositions;
double dblCurrentDiagonalSize;
double dblCurrentAspectRatio;
bool blnCurrentMatchFoundOrNewBlob;
bool blnStillBeingTracked;
int intNumOfConsecutiveFramesWithoutAMatch;
cv::Point predictedNextPosition;
// function prototypes
Blob(std::vector<cv::Point> _contour);
void predictNextPosition(void);
};
What algorithm should I use to estimate the speed of the detected vehicle??
Thanks in Advance.
UPDATE
Here is the code I have tried to estimate the speed, but it doesn't put the text plus it crashes.
for (auto blob : blobs) {
if (blob.blnStillBeingTracked == true && blob.centerPositions.size() >= 2) {
int prevFrameIndex = (int)blob.centerPositions.size() - 2;
int currFrameIndex = (int)blob.centerPositions.size() - 1;
if (blob.centerPositions[prevFrameIndex].y > (intHorizontalLinePosition-50) && blob.centerPositions[currFrameIndex].y <= intHorizontalLinePosition) {
int distance = blob.centerPositions[currFrameIndex].y - blob.centerPositions[0].y;
int tickCount = cv::getTickCount();
int time = (tickCount - blob.firstTickCount)/cv::getTickFrequency();
int speed = distance/time;
double dblFontScale = blobs[currFrameIndex].dblCurrentDiagonalSize / 10.0;
int intFontThickness = (int)std::round(dblFontScale * 1.0);
std::cout<<"Speed: "<<speed<<std::endl;
cv::putText(img, std::to_string(speed), blobs[currFrameIndex].centerPositions.back(), CV_FONT_HERSHEY_SIMPLEX, dblFontScale, SCALAR_GREEN, intFontThickness);
}
}
}
In order to predict the vehicle's speed in a 3-dimensional space from a 2D image in the general case, you need to know the orientation of the vehicle (direction of travel) and distance from the camera.
If you know for example that the vehicle is travelling perpendicular to the direction the camera points (moving directly across the frame, not toward or away from the camera at all), you can use either
a) A known distance from the camera to the road and basic trigonometry, or
b) Markers of known distance
to calculate the velocity of the vehicle using several frames.
If you know the vehicle is travelling directly toward or directly away from the camera, you can use the change in width/height of the image outline to get a sense of the vehicle's speed. If you can also identify when the vehicle passes a landmark at a known distance from the camera, you can calculate the actual width/height of the vehicle and therefore accurately calculate the speed using that known width/height and rate of change of the size of the 2D projection of the vehicle.
Update
Given the additional information, it seems you can determine what Y position in the camera's 2D image corresponds to a particular distance down the road. If you measure two such points, you can count how long it takes for the lower bounds of currentBoundingRect to pass from the first point to the second point, e.g. in the diagram below to move from y=800 to y=200.
If it takes 2 seconds to move from y=800 to y=200, it also takes 2 seconds to move 100m - 50m = 50m, or 50m/2 seconds = 25m/second.
I'm trying to achieve Augmented Reality with SceneKit.
I got a intrinsic camera matrix and a extrinsic matrix by estimating pose of a marker, using ARuco (OpenCV augmented reality library).
And I set up the SCNCamera's projectionTransform with parameters of the intrinsic matrix (fovy, aspect, zNear, zFar).
Normally in OpenGL, world coordinate relative to camera coordinate is calculated with ModelView but in SceneKit, there is no things such as modelView.
So I calculated inverse matrix of the extrinsic matrix to get the camera coordinate relative to the world coordinate(the marker coordinate).
And I think I've got correct camera's position by the inverse matrix which contains rotation and translate matrix.
However I cannot get camera's rotation from that.
Do you have any ideas?
SceneKit has the same view matrixes that you've come across in OpenGL, they're just a little hidden until you start toying with shaders. A little too hidden IMO.
You seem to have most of this figured out. The projection matrix comes from your camera projectionTransform, and the view matrix comes from the inverse of your camera matrix SCNMatrix4Invert(cameraNode.transform). In my case everything was in world coordinates making my model matrix a simple identity matrix.
The code I ended up using to get the classic model-view-projection matrix was something like...
let projection = camera.projectionTransform()
let view = SCNMatrix4Invert(cameraNode.transform)
let model = SCNMatrix4Identity
let viewProjection = SCNMatrix4Mult(view, projection)
let modelViewProjection = SCNMatrix4Mult(model, viewProjection)
For some reason I found SCNMatrix4Mult(...) took arguments in a different order than I was expecting (eg; opposite to GLKMatrix4Multiply(...)).
I'm still not 100% on this, so would welcome edits/tips. Using this method I was unable to get the SceneKit MVP matrix (as passed to shader) to match up with that calculated by the code above... but it was close enough for what I needed.
#lock's answer looks good with a couple additions:
(1) access SCNNode worldTransform instead of transform in case the cameraNode is animated or parented:
let view = SCNMatrix4Invert(cameraNode.presentationNode.worldTransform)
(2) the code doesn't account for the view's aspect ratio. e.g., assuming a perspective projection, you'll want to do:
perspMatrix.m11 /= viewportAR; //if using Yfov -> adjust Y`
/* or, */
perspMatrix.m22 *= viewportAR; //if using Xfov -> adjust X`
Where, viewportAR = viewport.width / viewport.height
Another way to do it is to have one node with a rendered delegate in the scene, and retrieve SceneKit’s matrices from that delegate (they are passed as options):
FOUNDATION_EXTERN NSString * const SCNModelTransform;
FOUNDATION_EXTERN NSString * const SCNViewTransform;
FOUNDATION_EXTERN NSString * const SCNProjectionTransform;
FOUNDATION_EXTERN NSString * const SCNNormalTransform;
FOUNDATION_EXTERN NSString * const SCNModelViewTransform;
FOUNDATION_EXTERN NSString * const SCNModelViewProjectionTransform;
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
I'm currently working in an stereo vision project, in which I'm supposed to reconstruct 3D points from correspondences found in each camera view, and for that I'm using OpenCV 2.4.7 for C++.
I was able to correctly calibrate both cameras, compute fundamental matrix, compute re-projection matrix and also rectify images.
My problem lies on the final part of the project, which is compute 3D world coordinates from 2D point correspondences. I already tried using cv::triangulatePoints, but the results where points with coordinates (0, 0, 0), no matter what the input points were. I also tried the linear triangulation algorithm by Hartley & Strum, but that didn't give me good results either.
Could somebody give me a hint on what function I should use? Or maybe some tips on how to correctly implement the ones I've talked about. My biggest problem is to find good documentation on the internet, so that's why I decided to ask here.
Thank you!
I tried cv::triangulatePoints also and it calculates garbage. I was forced to implement a linear triangulation method manually, which returns the triangulated 3D point given a stereo pixel correspondence:
Mat triangulate_Linear_LS(Mat mat_P_l, Mat mat_P_r, Mat warped_back_l, Mat warped_back_r)
{
Mat A(4,3,CV_64FC1), b(4,1,CV_64FC1), X(3,1,CV_64FC1), X_homogeneous(4,1,CV_64FC1), W(1,1,CV_64FC1);
W.at<double>(0,0) = 1.0;
A.at<double>(0,0) = (warped_back_l.at<double>(0,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,0) - mat_P_l.at<double>(0,0);
A.at<double>(0,1) = (warped_back_l.at<double>(0,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,1) - mat_P_l.at<double>(0,1);
A.at<double>(0,2) = (warped_back_l.at<double>(0,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,2) - mat_P_l.at<double>(0,2);
A.at<double>(1,0) = (warped_back_l.at<double>(1,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,0) - mat_P_l.at<double>(1,0);
A.at<double>(1,1) = (warped_back_l.at<double>(1,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,1) - mat_P_l.at<double>(1,1);
A.at<double>(1,2) = (warped_back_l.at<double>(1,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,2) - mat_P_l.at<double>(1,2);
A.at<double>(2,0) = (warped_back_r.at<double>(0,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,0) - mat_P_r.at<double>(0,0);
A.at<double>(2,1) = (warped_back_r.at<double>(0,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,1) - mat_P_r.at<double>(0,1);
A.at<double>(2,2) = (warped_back_r.at<double>(0,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,2) - mat_P_r.at<double>(0,2);
A.at<double>(3,0) = (warped_back_r.at<double>(1,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,0) - mat_P_r.at<double>(1,0);
A.at<double>(3,1) = (warped_back_r.at<double>(1,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,1) - mat_P_r.at<double>(1,1);
A.at<double>(3,2) = (warped_back_r.at<double>(1,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,2) - mat_P_r.at<double>(1,2);
b.at<double>(0,0) = -((warped_back_l.at<double>(0,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,3) - mat_P_l.at<double>(0,3));
b.at<double>(1,0) = -((warped_back_l.at<double>(1,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,3) - mat_P_l.at<double>(1,3));
b.at<double>(2,0) = -((warped_back_r.at<double>(0,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,3) - mat_P_r.at<double>(0,3));
b.at<double>(3,0) = -((warped_back_r.at<double>(1,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,3) - mat_P_r.at<double>(1,3));
solve(A,b,X,DECOMP_SVD);
vconcat(X,W,X_homogeneous);
return X_homogeneous;
}
the input parameters are two 3x4 camera projection matrices and the left/right corresponding homogeneous pixel coordinates.
I am trying to find camera extrinsics from six chessboard images with Opencv. My output is:
<Extrinsic_Parameters type_id="opencv-matrix"><rows>8</rows><cols>6</cols><dt>d</dt><data>
1.7261576010447846e-01 3.1158880577193560e-01 1.2720406228471280e-02
-1.1592911113815259e+02 -2.2406582979927950e+02
8.1420941356557194e+02 3.9346701007260626e-01 6.9003564777197379e-01
-1.3469982321835601e+00 -8.6826153197023956e+01
1.6051013708505607e+02 7.2011353507821275e+02
-2.7069532546117758e-01 2.2148718738805429e-01
3.6171900096916804e-01 -5.2552433323678208e+01
-2.0518988227964823e+02 6.4574752691945832e+02
-1.0899083613916538e-01 2.7649799933587743e-01
1.4885762528126442e-03 -1.6853141436409484e+02
-1.3942460278197393e+02 6.7605597445537410e+02
-2.6583012230541703e-01 4.5957884020124173e-01
1.3218319619927716e+00 1.4402207754903725e+02
-2.3447541083582803e+02 6.3522213346840215e+02
4.3950055218555362e-01 3.1960642369670372e-01 1.6428303526609498e+00
3.6431577304606526e+02 -1.3627914048252993e+02
6.7111839770203858e+02 2.2222642699596459e-01 3.7120066692341575e-01
1.8680367378145426e+00 7.2471813700978785e+01
-1.4921069561111017e+02 7.8381396962300209e+02
-5.5897629846263175e-02 5.0568582309311438e-01
1.7593139413155239e+00 1.8515422152916943e+02
-1.7998599745081759e+02 6.3305731735881432e+02</data></Extrinsic_Parameters>
And:
<Distortion_Coefficients type_id="opencv-matrix"><rows>5</rows><cols>1</cols><dt>d</dt><data>
-3.0246684876115215e-01 1.5880077551448199e-01 0. 0.
-6.9087472680662912e-02</data></Distortion_Coefficients>
Now, how I calculate the real world coordinates of the point (u,v) on the screen?
With extrinsic and intrinsic parameters I find:
Pc = [R|t] * Pw
where Pc and Pw are cam and world coordinates, R is rotation matrix and t is translation vector