I just started to use PCL library to deal with the classic ICP problem, and to begin with, I tested it out the ICP function by calculating the transformation between two pair of 3D points, with each being n = 4. Yet, weirdly enough, despite specifying the covergence criteria (code as shown below), it seems that the library just does not return the right values (reprojection as shown below in the image).
Im wondering whether this could be the unit problem (but I've changed it to mm so the numbers are larger), or the number of pointclouds is just too small. I'm relatively new to this library, would be grateful if y'all can provide some advices 😃.
icp.convergence_criteria_->setTranslationThreshold(0.01);
icp.convergence_criteria_->setAbsoluteMSE(0.01);
this is the reproject 3D points image, blues are the desired result, while red it the calculated ones.
Related
I apologize for any formatting mistakes, first time here.
I'm currently working on program in Java as a personal project that simulates and allows for the design of a lens system with surfaces generally defined using the equations covered here (Wikipedia). In this case the "order" of the surface referring to the greatest axrx value. Although possible, I'm pretty sure the order rarely is above 12.
Single solution:
Multiple solutions:
The linked images show two possible cases for a complex aspherical lens defined using this particular equation:
Assuming a "ray" comes from somewhere below the frame upwards as seen in the examples, how would I calculate the first point of collision between that ray and the lens surface? Specifically in three dimensions, as the examples above are only two dimensions as limited by Desmos. Being a lens, the resulting surface in three dimensions is possesses rotational symmetry where the 2D examples have reflection symmetry.
Edit removes unneeded sentence.
I have successfully calculated Rotation, Translation with the intrinsic camera matrix of two cameras.
I also got rectified images from the left and right cameras. Now, I wonder how I calculate the 3D coordinate of a point, just one point in an image. Here, please see the green points. I have a look at the equation, but it requires baseline which I don't know how to calculate. Could you show me the process of calculating the 3d coordinate of the green point with the given information (R, T, and intrinsic matrix)?
FYI
1. I also have a Fundamental matrix and Essential matrix, just in case we need them.
2. Original image size is 960 x 720. Rectified ones are 925 x 669
3. The green point from the left image: (562, 185), from the right image: (542, 185)
The term "baseline" usually just means translation. Since you already have your rotation, translation and intrinsics matrices (let's not them R, T and K). you can triangulate and don't need either the Fundamental or Essential matrices (they could be used to extract R, T etc but you already have them). You don't really need your images to be rectified either, since it doesn't change the triangulation process that much. There are many ways to triangulate, each with their pros and cons, and many libraries that implement them. So, all I can do here is give you and overview of the problem and potential solutions, as well as pointers to resources that you can either use as their are or as a source of inspiration to write your own code.
Formalization and solution outlines. Let's formalize what we are after here. You have a 3d point X, with two observations x_1Â and x_2 respectively in the left and right images. If you backproject them, you obtain two rays:
ray_1=K^{1}x_1
rat_2=R*K^{-1}x_2+T //I'm assuming that [R|T] is the pose of the second camera expressed in the referential of the first camera
Ideally, you'd want those two rays to meet at point X. Since in practice we always have some noise (discretization noise, rounding errors and so on) the two rays wont meet at X, so the best answer would be a point Q such that
Q=argmin_X {d(X,ray_1)^2+d(X,ray_2)^2}
where d(.) denotes the Euclidian distance between a line and a point. You can solve this problem as a regular least squares problem, or you can just take the geometric approach (called midpoint) of considering the line segment l that is perpendicular to both ray_1Â and ray_2, and take its middle as your solution. Another quick and dirty way is to use the DLT. Basically, you re-write the constrains (i.e. X should be as close as possible to both rays) as a linear system AX=0 and solve it with SVD.
Usually, the geometric (midpoint) method is the less precise. The DLT based one, while not the most stable numerically, usually produces acceptable results.
Ressources that present in depth formalization
Hartley-Zisserman's book of course! Chapter 12. A simple DLT-based method, which is the one used in opencv (both in the calibration and sfm modules) is explained on page 312. It is very easy to implement, it shouldn't take more that 10 minutes in any language.
Szeliski'st book. It has an intersting discussion on triangulation in the chapter on SFM, but is not as straight-forward or in depth as Hartley-Zisserman's.
Code. You can use the triangulation methods from opencv, either from the calib3d module, or from the contribs/sfm module. Both use the DLT, but the code from the SFM module is more easily understandable (the calib3d code has a lot of old-school C code which is not very pleasant to read). There is also another lib, called openGV, which has a few interesting methods for triangulation.
cv::triangulatePoints
cv::sfm::triangulatePoints
OpenGV
The openGV git repo doesn't seem very active, and I'm not a big fan of the design of the library, but if I remember correctly (feel free to tell me otherwise) it offers methods other that the DLT for triangulations.
Naturally, those are all written in C++, but if you use other languages, finding wrappers or similar libraries wont be difficult (with python you still have opencv wrappers, and MATLAB has a bundle module, etc.).
I'm maintaining software which uses PCL. I'm myself not much experienced in PCL, I've only tried some examples and tried to understand the official PCL-Ducumentation (which is unfortunately mainly sparse, doxygen-generated text). My impression is, only a PCL contributors have real change to use the library efficiently.
One feature I have to fix in the software is aligning two clouds. The clouds are two objects, which should be stacked together with a layer in-between (The actual task is to calculate the volume of the layer ).
I hope the picture explains the task well. The objects are scanned both from the sides to be stacked (one from above and the other from below). On both clouds the user selects manually two points. Then, as I hope there should be a mean in PCL to align two clouds providing the two clouds and the coordinates of the points. The alignment is required only in X-Y Plane.
Unfortunately I can't find out which function should I use for this, partly because the PCL documentation is IHMO really humble, partly because of lack of experience.
My desperate idea was to stack the clouds using P1 as the origin of both and then rotate the second cloud manually using the calculated angle between P11,P21 and P12,P22. This works, but since the task appears to me very common, I'd expect PCL to provide a dedicated function for that.
Could you point me to a proper API-function, code-snippet, example, similar project or a good book helping to understand PCL API and usage?
Many thanks!
I think this problem does not need PCL. It is simple enough to form the correct linear equation and solve it.
If you want to use PCL without worrying about the maths too much (though, if the above is a mystery to you, then studying some computational geometry would be very useful), here is my suggestion.
Most PCL operations work on 3D point clouds. I understand from your question that you only have 2D point clouds OR you don't care about the 3rd dimension. In this case if I were you I would represent the points as a 3D point cloud and set the z dimension to zero.
You will only need two point clouds with 3 points as that is how many points you are feeding to the transformation estimation algorithm. The first 2 points in the clouds will be the points chosen by the user. The third one will be any point that you have chosen that you know is the same in both clouds. You need this third one otherwise the transform is still ambiguous if it is a general transform that is being computed. You can calculate however such a point as you know 2 points already and you know that all the points are on a common plane (as you have projected them by losing the z values). Just don't choose it co-linear with the other two points. For example, halfway between the two points and 2cm in the perpendicular direction (ensuring to go in the correct direction).
Then you can use the estimateRigidTransformation functions to find the transform.
http://docs.pointclouds.org/1.7.0/classpcl_1_1registration_1_1_transformation_estimation_s_v_d.html
This function is also good for over-determined problems (it is the workhorse of the ICP algorithm in PCL) but as long as you have enough points to determine the transform it should work.
I am quite new to this, and iv'e heard that i need to get my inversed projection matrix and so on to create a ray from a 2D point to a 3D world point, however since im using OpenglES and there are not as many methods as there would be regulary to help me with this. (And i simply don't know how to do it) im using a trigenomeric formula for this insted.
For each time i iterate one step down the negative Z-axis i multiply the Y-position on the screen (-1 to 1) with
(-z / (cot(myAngle / 2))
And the X position likewise but with a koefficent equally to the aspect ratio.
myAngle is the frustum perspective angle.
This works really good for me and i get very accurate values, so what i wonder is: Why should i use the inverse of the projection matrix and multiply it with some stuff instead of using this?
Most of the time you have a matrix lying around for your OpenGl camera. Using an inverse matrix is simple when you already have a camera matrix on hand. It is also (oh so very slightly at computer speeds) faster to do a matrix multiply. And in cases where you are doing a bajillion of these calculations per frame, it can matter.
Here is some good info on getting started on a camera class if you are interested:
Camera Class
And some matrix resources
Depending on what you are working on, I wouldn't worry too much about the 'best way to do it.' You just want to make sure you understand what your code is doing then keep improving it.
For use in a rigid body simulation, I want to compute the mass and inertia tensor (moment of inertia), given a triangle mesh representing the boundary of the (not necessarily convex) object, and assuming constant density in the interior.
Assuming your trimesh is closed (whether convex or not) there is a way!
As dmckee points out, the general approach is building tetrahedrons from each surface triangle, then applying the obvious math to total up the mass and moment contributions from each tet. The trick comes in when the surface of the body has concavities that make internal pockets when viewed from whatever your reference point is.
So, to get started, pick some reference point (the origin in model coordinates will work fine), it doesn't even need to be inside of the body. For every triangle, connect the three points of that triangle to the reference point to form a tetrahedron. Here's the trick: use the triangle's surface normal to figure out if the triangle is facing towards or away from the reference point (which you can find by looking at the sign of the dot product of the normal and a vector pointing at the centroid of the triangle). If the triangle is facing away from the reference point, treat its mass and moment normally, but if it is facing towards the reference point (suggesting that there is open space between the reference point and the solid body), negate your results for that tet.
Effectively what this does is over-count chunks of volume and then correct once those areas are shown to be not part of the solid body. If a body has lots of blubbery flanges and grotesque folds (got that image?), a particular piece of volume may be over-counted by a hefty factor, but it will be subtracted off just enough times to cancel it out if your mesh is closed. Working this way you can even handle internal bubbles of space in your objects (assuming the normals are set correctly). On top of that, each triangle can be handled independently so you can parallelize at will. Enjoy!
Afterthought: You might wonder what happens when that dot product gives you a value at or near zero. This only happens when the triangle face is parallel (its normal is perpendicular) do the direction to the reference point -- which only happens for degenerate tets with small or zero area anyway. That is to say, the decision to add or subtract a tet's contribution is only questionable when the tet wasn't going to contribute anything anyway.
Decompose your object into a set of tetrahedrons around the selected interior point. (That is solids using each triangular face element and the chosen center.)
You should be able to look up the volume of each element. The moment of inertia should also be available.
It gets to be rather more trouble if the surface is non-convex.
I seem to have miss-remembered by nomenclature and skew is not the adjective I wanted. I mean non-regular.
This is covered in the book "Game Physics, Second Edition" by D. Eberly. The chapter 2.5.5 and sample code is available online. (Just found it, haven't tried it out yet.)
Also note that the polyhedron doesn't have to be convex for the formulas to work, it just has to be simple.
I'd take a look at vtkMassProperties. This is a fairly robust algorithm for computing this, given a surface enclosing a volume.
If your polydedron is complicated, consider using Monte Carlo integration, which is often used for multidimensional integrals. You will need an enclosing hypercube, and you will need to be able to test whether a given point is inside or outside the polyhedron. And you will need to be patient, as Monte Carlo integration is slow.
Start as usual at Wikipedia, and then follow the external links pages for further reading.
(For those unfamiliar with Monte Carlo integration, here's how to compute a mass. Pick a point in the containing hypercube. Add to the point_total counter. Is it in the polyhedron? If yes, add to the point_internal counter. Do this lots (see the convergence and error bound estimates.) Then
mass_polyhedron/mass_hypercube \approx points_internal/points_total.
For a moment of inertia, you weight each count by the square of the distance of the point to the reference axis.
The tricky part is testing whether a point is inside or outside your polyhedron. I'm sure that there are computational geometry algorithms for that.