I want to use scipy.optimize package to solve a rigid body optimization problem (finding optimal translation and rotation of a object).
A way to parametrize this problem is through lie group theory and the lie algebra se(3) (often called twist coordinates). This means the state vector is composed of 6 parameters (i.e. pose = [rho_x, rho_y, rho_z, theta_0, theta_1, theta_3]) from which the SE(3) Transformation matrix (a composition of translation vector and rotation matrix) can be obtained through the so called 'wedge' or 'hat' operator and the matrix exponentiation. (I found this cheat-sheet to be an excellent overview)
Now my question: Can scipy be used to optimize a 3D pose that is parametrized by the twist coordinates? I read somewhere that such problems require 'on manifold optimization' methods. So can I use scipy's least_square() or minimize(method="BFGS") for this? or do these only work for flat euclidean spaces?
Alternatively it would also help me a lot if anyone knows of any example where scipy is used to solve a 3D pose optimization problem. I have been looking for examples for the last couple of days but without much success.
This question is a implementation specific part of a more general question I posted on the robotics stackexchange here.
Related
I'm looking for a code/package which given a starting point $x_o$ and ending point $x_1$ to compute equally spaced points on the geodesic between $x_0$ and $x_1$ given a function which returns the Riemannian metric tensor at any point. This does not seem to be a trivial task since you need to solve some form of a differential equation. Is anyone aware of any such materials?
You can compute minimal geodesics by solving the Eikonal equation using the Fast Marching Method, which is implemented in Python in Jason Furtney's scikit-fmm. It may be ok for Riemannian metrics with weak anisotropy. If not, it's at least a place to start.
Update 2019/01: Jean-Marie Mirebeau has a rather more complex package of code implementing his fast-marching method for solving the eikonal equation with Riemannian metrics. The code includes tools for computing geodesics. It's non-trivial to install and run, but I've had some success after persevering.
Hello people on the internet!
I have two 3D equations that I want to solve simultaneously. They are of the form F(x,y,z)=C and G(x,y,z)=0. The solution of these equations are supposed to describe curves (maybe even areas in some regions, I am not sure) and I want to obtain a discrete set of numerical solutions that "sample" these lines. I tried searching for a while, but the methods directed at solving I stumbled upon only aim to find a single solution.
I thought about using a Grid on 3d space and just check the equations, however that forces me to loosen the conditions a bit. But in case (or in regions where) the solution is a curve, the points are supposed to resemble a curve after all.
For better reference, my functions are of the form:
with random parameters c_i, d_i, k_i, phi_i.
For tips I would prefer native python, but I am open to any possible solution. Any ideas appreciated! :)
You're going to want to start by sampling those functions on a 3D grid containing the portion of the solution set you are interested.
Once you've identified which regions of the gird may contain potential solutions, you'll then use an iterative method to minimize the function (F(x)-C)^2 + (G(x))^2.
The key here is that you will do the iterative algorithm for each gird region you identified as "interesting." Each time, initializing the method with values laying inside the region of interest.
Note: Sorry for the poor notation.
I'm trying to build a very basic tool for a problem in mechanical engineering and need to do simple transformations of coordinates and vectors in euclidian space which include translations + rotations.
For example
a component part with a local coordinate system (cs) is moved in respect to a world cs. I need to compute the new position of the (origin of) the local cs and its orientation.
a vector is shifted and rotated in respect to a cs. Its new position has to be computed.
I'm aware of different concepts for doing these computations:
Euler angles
Quaternions
Homogeneous coordinates
From my POV the use of homogeneous coordinates would be the simplest approach because it is possible to compute translations and rotations in one step.
Questions:
What is the most common approach in programming to implement this kind of computations?
Is there a python library which can handle these transformations? I found some smaller libraries like transformations.py but I guess transformations like these are very very common and so I wonder if this isn't part of scipy or something like that.
After all i assume i'm searching for the wrong terms and would be glad if someone could provide a hint for further reading, code examples, libraries (especially for python).
Use numpy and linear algebra to do the transformations as matrix multiplications
Using a Microsoft Kinect, I am collecting depth data about an object. From these data, I create a "cloud" of points (point cloud), which, when plotted, allow me to view the object that I scanned using the Kinect.
However, I would like to be able to collect multiple point clouds from different "views" and align them. More specifically, I would like to use an algorithm such as Iterative Closest Point (ICP) to do so, transforming each point in my point cloud by calculating the rotation and translation between each cloud that I collect and the previously-collected cloud.
However, while I understand the process behind ICP, I do not understand how I would implement it in 3D. Perhaps it is my lack of mathematical experience or my lack of experience with frameworks such as OpenCV, but I cannot find a solution. I would like to avoid libraries such as the Point Cloud Library which does this sort of thing for me, since I would like to do it myself.
Any and all suggestions are appreciated (if there is a solution that involves OpenCV/python that I can work on, that would be even better!)
I am currently struggling with ICP myself. Here is what I have gathered so far:
ICP consists of three steps:
Given two point clouds A and B, find pairs of points between A and B that probably represent the same point in space. Often this is done simply by matching each point with its closest neighbor in the other cloud, but you can use additional features such as color, texture or surface normal to improve the matching. Optionally you can then discard the worst matches.
Given this list of correspondence pairs, find the optimal transformation from A to B
Apply this transformation to all points in A
repeat these three steps until you converge on an acceptable solution.
Step one is easy, although there are lots of ways to optimize its speed, since this is the major performance bottleneck of ICP; and to improve the accuracy, since this is the main source of errors. OpenCV can help you there with the FLANN library.
I assume your troubles are with step two, finding the best transformation given a list of correspondences.
One common approach works with Singular Value Decomposition (SVD). Here is a rough sketch of the algorithm. Searching for ICP & SVD will give a lot of further references.
Take the list of corresponding points A1..An and B1..Bn from step 1
calculate the centroid Ca of all points in A and the centroid Cb of all points in B
Calculate the 3x3 covariance matrix M
M = (A1 - Ca)* (B1 - Cb)T + ... + (An - Ca)* (Bn - Cb)T
Use SVD to calculate the 3x3 Matrices U and V for M
(OpenCV has a function to perform SVD)
Calculate R = U * VT.
This is your desired optimal rotation matrix.
Calculate the optimal translation as Cb - R*Ca
The optimal transformation is the combination of R and this translation
Please note that I have not yet implemented this algorithm myself, so I am only paraphrasing what I read.
A very good introduction to ICP, including accelerated variants, can be found in Rusinkievicz's old paper here.
A new ICP algorithm is now in OpenCV contrib (surface matching module). It also benefits from the variants of various types (including Rusinkievicz and more):
http://docs.opencv.org/3.0-beta/modules/surface_matching/doc/surface_matching.html
For MATLAB implementation:
http://www.mathworks.co.jp/matlabcentral/fileexchange/47152-icp-registration-using-efficient-variants-and-multi-resolution-scheme/content/icp_mod_point_plane_pyr.m
#tdirdal:
Ok then I may not be looking at the correct code.
I am talking about this package link:
The code starts with constructing a transformation matrix and then loads a *.ply which contains a mesh (faces and vertices). The rest of the code depends on the mesh that has been loaded.
I have a very simple problem and I would appreciate it if you could let me know how I can solve this using the ICP method. I have the following two point clouds. P2 is a subset of P39 and I would like to find P2 in P39. Please let me know how I can use your matlab package to solve this problem.
P2:
11.2706 -5.3392 1.1903
13.6194 -4.8500 2.6222
8.8809 -3.8407 1.1903
10.7704 -2.1800 2.6222
8.5570 -1.0346 1.1903
13.1808 -2.5632 1.1903
P39:
-1.9977 -4.1434 -1.6750
-4.3982 -3.5743 -3.1069
-6.8065 -3.0071 -1.6751
-9.2169 -2.4386 -3.1070
-11.6285 -1.8696 -1.6751
-16.4505 -0.7305 -1.6751
-14.0401 -1.3001 -3.1070
-18.8577 -0.1608 -3.1070
-25.9398 -0.8647 -3.1070
-30.1972 -4.6857 -3.1069
-28.2349 -2.5200 -3.1069
-29.5843 -0.2496 -1.6751
-31.1688 -2.0974 -3.1070
-21.2580 0.4093 -1.6751
-23.6450 0.9838 -3.1070
-26.0636 1.5073 -1.6751
-28.4357 1.9258 -3.1070
I'm trying to call upon the famous multilateration algorithm in order to pinpoint a radiation emission source given a set of arrival times for various detectors. I have the necessary data, but I'm still having trouble implementing this calculation; I am relatively new with Python.
I know that, if I were to do this by hand, I would use matrices and carry out elementary row operations in order to find my 3 unknowns (x,y,z), but I'm not sure how to code this. Is there a way to have Python implement ERO, or is there a better way to carry out my computation?
Depending on your needs, you could try:
NumPy if your interested in numerical solutions. As far as I remember, it could solve linear equations. Don't know how it deals with non-linear resolution.
SymPy for symbolic math. It solves symbolically linear equations ... according to their main page.
The two above are "generic" math packages. I doubt you will find (easily) any dedicated (and maintained) library for your specific need. Their was already a question on that topic here: Multilateration of GPS Coordinates