hovren / kontiki Goto Github PK

Hello,

Is it possible to compute a spline for X,Y,Z and SO(3) from some X,Y,Z and rotation measurement at time t explicitly.
I essentially want to create a continuius representation from some data which I want to subsample.

Currently I use Scipy. Can I achieve something similar with Kontiki?
Can kontiki also compute the spline derivatives?

E.g.

 sx = interpolate.InterpolatedUnivariateSpline(time_data, xs, k=3)  # x(u) spline
 sy = interpolate.InterpolatedUnivariateSpline(time_data, ys, k=3)  # y(u) spline
 sz = interpolate.InterpolatedUnivariateSpline(time_data, zs, k=3)  # z(u) spline

 s_rot = RotationSpline(time_data, rotation_data)

Hello ~~ Thank you for your excellent work.

I am having some problems compiling the code.

When I from kontiki/python/ run sudo python3 setup.py install ,there is a error as follows

➜  python git:(master) ✗ sudo python3 setup.py install
Traceback (most recent call last):
  File "setup.py", line 125, in <module>
    with open(here / '../README.md') as f:
TypeError: invalid file: PosixPath('../README.md')

So I modify the setup.py and delete the code here / , here is the code after modifying

#here = Path(__file__).parents[0]
try:
    #with open('../README.md') as f:
    with open(here / '../README.md') as f:
        long_description = f.read()
except FileNotFoundError:
    long_description = "Omitting long_description since `README.md` is in a parent path."

# Version is defined by C++ library
#with open(here / '../cpplib/CMakeLists.txt') as f:
with open('../cpplib/CMakeLists.txt') as f:
    m = re.search(r'project\(Kontiki VERSION ([\w\.]+)\)', f.read())
    version = m.groups(0)[0]

However another error has occurred

➜  python git:(master) ✗ sudo python3 setup.py install
/usr/lib/python3.5/distutils/dist.py:261: UserWarning: Unknown distribution option: 'long_description_content_type'
  warnings.warn(msg)
/usr/lib/python3.5/distutils/dist.py:261: UserWarning: Unknown distribution option: 'python_requires'
  warnings.warn(msg)
running install
Checking .pth file support in /usr/local/lib/python3.5/dist-packages/
/usr/bin/python3 -E -c pass
TEST PASSED: /usr/local/lib/python3.5/dist-packages/ appears to support .pth files
running bdist_egg
running egg_info
writing requirements to kontiki.egg-info/requires.txt
writing top-level names to kontiki.egg-info/top_level.txt
writing kontiki.egg-info/PKG-INFO
writing dependency_links to kontiki.egg-info/dependency_links.txt
reading manifest file 'kontiki.egg-info/SOURCES.txt'
reading manifest template 'MANIFEST.in'
writing manifest file 'kontiki.egg-info/SOURCES.txt'
installing library code to build/bdist.linux-x86_64/egg
running install_lib
running build_py
running build_ext
CMAKE LIST DIR /home/ha/ws_github/kontiki
---------- Running CMake prepare ----------------------------------------
Traceback (most recent call last):
  File "setup.py", line 165, in <module>
    'Programming Language :: Python :: 3.6',
  File "/usr/lib/python3.5/distutils/core.py", line 148, in setup
    dist.run_commands()
  File "/usr/lib/python3.5/distutils/dist.py", line 955, in run_commands
    self.run_command(cmd)
  File "/usr/lib/python3.5/distutils/dist.py", line 974, in run_command
    cmd_obj.run()
  File "/usr/lib/python3/dist-packages/setuptools/command/install.py", line 67, in run
    self.do_egg_install()
  File "/usr/lib/python3/dist-packages/setuptools/command/install.py", line 109, in do_egg_install
    self.run_command('bdist_egg')
  File "/usr/lib/python3.5/distutils/cmd.py", line 313, in run_command
    self.distribution.run_command(command)
  File "/usr/lib/python3.5/distutils/dist.py", line 974, in run_command
    cmd_obj.run()
  File "/usr/lib/python3/dist-packages/setuptools/command/bdist_egg.py", line 161, in run
    cmd = self.call_command('install_lib', warn_dir=0)
  File "/usr/lib/python3/dist-packages/setuptools/command/bdist_egg.py", line 147, in call_command
    self.run_command(cmdname)
  File "/usr/lib/python3.5/distutils/cmd.py", line 313, in run_command
    self.distribution.run_command(command)
  File "/usr/lib/python3.5/distutils/dist.py", line 974, in run_command
    cmd_obj.run()
  File "/usr/lib/python3/dist-packages/setuptools/command/install_lib.py", line 23, in run
    self.build()
  File "/usr/lib/python3.5/distutils/command/install_lib.py", line 109, in build
    self.run_command('build_ext')
  File "/usr/lib/python3.5/distutils/cmd.py", line 313, in run_command
    self.distribution.run_command(command)
  File "/usr/lib/python3.5/distutils/dist.py", line 974, in run_command
    cmd_obj.run()
  File "setup.py", line 61, in run
    cwd=self.build_temp, env=env)
  File "/usr/lib/python3.5/subprocess.py", line 576, in check_call
    retcode = call(*popenargs, **kwargs)
  File "/usr/lib/python3.5/subprocess.py", line 557, in call
    with Popen(*popenargs, **kwargs) as p:
  File "/usr/lib/python3.5/subprocess.py", line 947, in __init__
    restore_signals, start_new_session)
  File "/usr/lib/python3.5/subprocess.py", line 1490, in _execute_child
    restore_signals, start_new_session, preexec_fn)
TypeError: Can't convert 'PosixPath' object to str implicitly

Could you give some suggestions ? Thank you a lot !

Hi, thank you for sharing this code! I find that this is the only one open sourced project that try to do SfM with the aid of imu, does it just like visult-inertial odometry that reconstruct the scene with real-world scale? Or you just use the gyroscope to predict the rotation of next image?

Hi, and thank you for this code. I would like to use it in c++ without any Python. is this possible?

What is the input data required?

Thanks!

Hello hovren

I add a new sensor model (LiDAR model) to the Kontiki, and I also create a new measurement type of the LiDAR model(LiDARMeasurement).

The R3 spline trajectory is initialized by solving a linear problem(Ax=b) and the SO3 spline trajectory is initialized by the optimation method. I have checked out the initial result and it's correct enough for the next step.

So I add AccelerometerMeasurement, GyroscopeMeasurement, LiDARMeasurement to the estimator. However, the optimization result is not right, especially in the front part of the trajectory, which is very unstable as shown in this picture.

I have observed several things.

1. The optimization direction was wrong at the beginning. The terminal output during optimization is as follows. Note the first few iterations, the error is increasing

   iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
      0  3.082471e+10    0.00e+00    2.11e+09   0.00e+00   0.00e+00  1.00e+04        0    5.50e-02    6.00e-02
      1  5.499182e+11   -5.19e+11    0.00e+00   3.12e+01  -1.71e+01  5.00e+03        1    4.30e-02    1.03e-01
      2  6.604305e+11   -6.30e+11    0.00e+00   2.55e+01  -2.07e+01  1.25e+03        1    1.53e-02    1.18e-01
      3  6.878899e+10   -3.80e+10    0.00e+00   1.93e+01  -1.25e+00  1.56e+02        1    1.77e-02    1.36e-01
      4  1.021614e+10    2.06e+10    1.18e+09   1.30e+01   6.87e-01  1.65e+02        1    7.07e-02    2.07e-01
      5  3.401698e+09    6.81e+09    4.83e+08   6.37e+00   6.82e-01  1.73e+02        1    7.33e-02    2.80e-01
     ......
     28  8.597815e+08    6.39e+06    6.11e+07   6.52e-02   4.47e-01  9.30e-02        1    6.60e-02    1.52e+00
     29  8.494701e+08    1.03e+07    6.07e+07   9.56e-02   4.42e-01  9.29e-02        1    7.56e-02    1.60e+00
     30  8.439439e+08    5.53e+06    5.95e+07   6.33e-02   3.87e-01  9.18e-02        1    6.93e-02    1.67e+00
   
   Solver Summary (v 1.14.0-eigen-(3.3.7)-lapack-suitesparse-(4.4.6)-cxsparse-(3.1.4)-eigensparse-openmp-no_tbb)
   
                                        Original                  Reduced
   Parameter blocks                          372                      364
   Parameters                               1296                     1274
   Effective parameters                     1112                     1092
   Residual blocks                          9904                     9904
   Residuals                               24144                    24144
   
   Minimizer                        TRUST_REGION
   
   Sparse linear algebra library    SUITE_SPARSE
   Trust region strategy     LEVENBERG_MARQUARDT
   
                                           Given                     Used
   Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
   Threads                                     8                        8
   Linear solver ordering              AUTOMATIC                      364
   
   Cost:
   Initial                          3.082471e+10
   Final                            8.439439e+08
   Change                           2.998077e+10
   
   Minimizer iterations                       31
   Successful steps                           21
   Unsuccessful steps                         10
   
   Time (in seconds):
   Preprocessor                         0.004960
   
     Residual only evaluation           0.139199 (30)
     Jacobian & residual evaluation     1.119277 (21)
     Linear solver                      0.345781 (30)
   Minimizer                            1.662274
   
   Postprocessor                        0.000659
   Total                                1.667893
   
   Termination:                   NO_CONVERGENCE (Maximum number of iterations reached. Number of iterations: 30.)
   

2. Given the adjacent control points in the quaternion spline, the inner product of the first few konts is less than zero.
   The inner product:

   control point idx: 0; q1^T q2 = -0.956805
   control point idx: 1; q1^T q2 = -0.999958
   control point idx: 3; q1^T q2 = -0.455493
   control point idx: 4; q1^T q2 = -0.84764
   

   The first few control points:

    control point q0 = -0.653931 -0.364804 -0.263813  0.608026
    control point q1 =  0.722436  0.239671  0.010041 -0.648493
    control point q2 = -0.720838  -0.24549 -0.016903  0.647952
    control point q3 = -0.729814  0.407561   0.49898  0.228658
    control point q4 =   -0.1708 -0.588182  -0.76801  0.187164
    control point q5 =  0.517802  0.181479  0.770429 -0.324632
    control point q6 =   0.575074  -0.721305 -0.0431002  -0.383603
   

I am wondering how to keep the inner product of the adjacent control points large than zero during the optimation. Could you give me some suggestions? Thank you a lot !

Hi Hannes

I want ask you about your thoughts on treating SE(3) / Sim(3) as a single object vs treating them as separate parts.

Libraries like sophus and manif do provide support for SE3, and LieTorch even provides support for Sim3. But the expressions involved in their jacobian become increasingly complex and sometimes do not have closed forms.

I was looking online for a discussion on the relative merit of these two approaches, and found your paper, Trajectory Representation and Landmark Projection for Continous Time Structure from Motion (https://arxiv.org/abs/1805.02543). Here you have a comparison in the case of spline interpolation. I want to ask whether you think the same would hold true in a pose-graph optimization setup, or some SLAM setup.

Hi,

This is great work.
I am trying to run Kontiki with IMU measurements from inbuilt GoPro IMU.

Do you have any example of how to create SfM data from the mp4 GoPro Video?
What software did you use? Did you use COLMAP?

Thanks,