pt2pl objective gets stuck sometimes about probreg HOT 18 CLOSED

That's a really good pointer, I think I'm making an error in transforming my normals between frames, which might cause the oscillation, or wrongly estimated quality of the fit. I'll check this in detail and report back.

from probreg.

I have adapted the constant sigma and it terminates successfully. The problem reappears for a few other pairs, but I'll continue my parameter tweaking and should be able to get rid of those as well. Thanks!

from probreg.

HI,

Thank you for the data.
I tried it in my environment and got the following results.
I also attach the code.

result:  [-0.13227301  0.38758146 -0.1735933 ] 1.0 [ 0.16110352  0.02992692 -0.10307387] # xyz[m], scale, rpy[deg]

import numpy as np
import transformations as trans
from probreg import filterreg
from probreg import callbacks
import open3d as o3

sdata = o3.io.read_triangle_mesh("pt2pl-no-converge/frame00018.obj")
tdata = o3.io.read_triangle_mesh("pt2pl-no-converge/frame00019.obj")

def estimate_normals(pcd, params):
    pcd.estimate_normals(search_param=params)
    pcd.orient_normals_to_align_with_direction()

source = o3.geometry.PointCloud()
source.points = o3.utility.Vector3dVector(sdata.vertices)
target = o3.geometry.PointCloud()
target.points = o3.utility.Vector3dVector(tdata.vertices)
estimate_normals(target, o3.geometry.KDTreeSearchParamHybrid(radius=0.5, max_nn=50))

cbs = [callbacks.Open3dVisualizerCallback(source, target)]
objective_type = 'pt2pl'
tf_param, _, _ = filterreg.registration_filterreg(source, target,
                                                  target_normals=np.asarray(target.normals),
                                                  objective_type=objective_type,
                                                  sigma2=0.1,
                                                  tol=0.001,
                                                  update_sigma2=False,
                                                  min_sigma2=0.1,
                                                  callbacks=cbs)
rot = trans.identity_matrix()
rot[:3, :3] = tf_param.rot
print("result: ", np.rad2deg(trans.euler_from_matrix(rot)),
      tf_param.scale, tf_param.t)

The target data seem pretty close to the source, so how accurate does it need to be?

from probreg.

I also get a good result rather immediately, but the algorithm seems not to converge although the transformation does not change anymore (at least visually). Still I run into the maximum iterations and this pair (and a few others) stall my overall throughput significantly.

from probreg.

I must add that I use the normals that are provided in the OBJ file. Are there debugging options that might help me identify the cause of this?

from probreg.

I added the debug output as mentioned in #35 and I can see that the maxiter is reached although the result looks good after very few iterations. Some kind of oscillation seems to happen looking at the Criteria: output:

...
Iteration: 668, Criteria: 65.53958129882812
Iteration: 669, Criteria: 65.46092224121094
Iteration: 670, Criteria: 65.53949737548828
Iteration: 671, Criteria: 65.46096801757812
Iteration: 672, Criteria: 65.53949737548828
Iteration: 673, Criteria: 65.46092987060547
Iteration: 674, Criteria: 65.53953552246094
Iteration: 675, Criteria: 65.46095275878906
Iteration: 676, Criteria: 65.53953552246094
Iteration: 677, Criteria: 65.46094512939453
Iteration: 678, Criteria: 65.53958892822266
Iteration: 679, Criteria: 65.46092987060547
Iteration: 680, Criteria: 65.53949737548828
Iteration: 681, Criteria: 65.46092224121094
...

However I don't know how to interpret this further. I get the same output when changing the tolerance to 0.01 for example.

from probreg.

I think they are many iterations in the result.
I think the result is already close to the point of convergence, since gradient-based optimization causes oscillations close to the point of convergence.
About 30~100 iterations is reasonable.

from probreg.

True, however I would expect the termination criterion of the algorithm to capture that the solution is already sufficiently good, instead of oscillating close to the optimum because the initial solution is "too good". I'll try to figure out the termination criterion from the source code and see if I can make this robust somehow.

from probreg.

You can see the normals by pressing 'n' in Open3D's viewer.

from probreg.

Hi @neka-nat. I checked my code again and I run into the same problem, even if I do not update any normals for visualization in between at all. I removed the intermediate callback altogether and run the algorithm "offline", still I run into the maximum number of iterations with oscillating behavior around the "correct" solution. If time permits could you try to use the normals that I provided in the OBJ files above to see if you can reproduce the behavior?

from probreg.

Hi,
Have you tried using estimate_normals?
If the normals in the obj file and the normals in the estimate_normals produce the same result, then that may not be the problem.

from probreg.

I already estimated normals for my datasets by using the implicit neighborhood information of the points coming from the LiDAR scan. Computing normals again unnecessarily, which will require a spatial neighborhood search, is prohibitive in my scenario because I need to register the scans with very high performance in (close to) real time. I attached a screenshot showing the two scans above with my estimated normal information, I think they look fine and should not be the cause of the problem.

from probreg.

I see.
Can I get your code?
I'll try to run it in my environment.

from probreg.

I stripped everything and built a minimal example for you to test. That minimal example actually worked fine here in my setup. As it turns out, there was a bug in my code that led to the source point cloud normal array being passed to the registration function instead of the target normals. I assume when the point set sizes didn't match nasty things happened because indices went out of bounds etc. Sorry for the noise and thank you very much for your assistance! Closing.

from probreg.

Okay. I was being a bit fast. Although my code made a wrong assumption about the order of source and target, the point position to normal assignment was, after all, correct. Changing the order just made some cases work and others fail instead. I attached a minimal example that reproduces the error with the two OBJ files I uploaded above. Thank you again for looking into this!

Note that I use the libigl python bindings to load the OBJs with normal information. If you don't have this or another library at hand to do it I can write up a quick parser if it helps.

import os
import time
import copy

import numpy as np
import open3d as o3d
import igl

from probreg import filterreg

import logging
log = logging.getLogger('probreg')
log.setLevel(logging.DEBUG)

frame_vertices = []
frame_normals = []

for filename in ['frame00019.obj', 'frame00018.obj']:
    [v, _, n, _, _, _] = igl.read_obj(filename)
    frame_vertices.append(o3d.utility.Vector3dVector(v))
    frame_normals.append(o3d.utility.Vector3dVector(n))

print('read frames: ' + str(len(frame_vertices)))

test_source = o3d.geometry.PointCloud()
test_source.points = frame_vertices[0]
test_source.normals = frame_normals[0]

test_target = o3d.geometry.PointCloud()
test_target.points = frame_vertices[1]
test_target.normals = frame_normals[1]

mstepresult = filterreg.registration_filterreg(test_source, test_target,
                                               target_normals=test_target.normals,
                                               maxiter=1000,
                                               sigma2=0.1,
                                               tol=0.001,
                                               objective_type='pt2pl',
                                               callbacks=[])

from probreg.

I ran the code you gave me.
The data is pretty close to each other, so I think it's necessary to set the parameters to match the close data.
In the original paper, the accuracy is about 0.1 to 2 degrees.
The code above shows about 0.5 degrees.
In my code, I stabilized the result by setting min_sigma2=0.1.

from probreg.

Could you elaborate a bit on this? Are you implying that I should use an adaptive sigma to make the algorithm converge when I have a good initial solution? Are the values that I'm seeing in the debug log counted in degrees? How would I adapt my sigma given the observation that the my example oscillates with around 0.5 degrees?

To me it seems as if the step size of the twist optimization is too large when close to the optimum and the optimization should somehow account for this, e.g. by adding a damping or inertia term of some kind.

Thank you!

from probreg.

I investigated the code further.
It looks like it would be better to tune the sigma2 rather than min_sigma2.
The following parameters converge faster.

tf_param, _, _ = filterreg.registration_filterreg(test_source, test_target,
                                               target_normals=test_target.normals,
                                               maxiter=1000,
                                               sigma2=1.0,
                                               tol=0.001,
                                               objective_type='pt2pl',
                                               callbacks=[])

from probreg.