I have a slam algorithm that outputs at around 30Hz, an implementation of ORBSLAM2.


I am reading this into a renderer that expects 60+ Hz.

Because my sample speed is low, I am getting 'shuddering' in the display, as the renderer adds linear 'steps' between the samples.

For example, I am seeing a result like:

time   sample    result

1         20          20
2         n/a         20
3         n/a         20
4         22          22
5         n/a         22
6         n/a         22
7         24          24
8         n/a         24
9         n/a         24

What i need to do, is predict the next sample, and fill in the gaps, so to speak, so that I end up with something like:

time   sample    result

1         20          20
2         n/a         20.66
3         n/a         21.33
4         22          22
5         n/a         22.66
6         n/a         23.66
7         24          24
8         n/a         24.33
9         n/a         25.66

I need to predict 6DOF, for which i have translation xyz, and a quaternion xyzw. But if I can find a way to predict even one axis, for a start, that would be great.

I have the data outputting as xyz and xyzw, at around 30Hz. I also have an xsens IMU, which i am using to pass in an initial rotation value.

Can i use a predictive filter for this purpose? Is a kalman suitable?

I am looking at:


and a Bayes filter:


But am a little out of my depth.

Thank you, please ask if I have not made sense!

  • $\begingroup$ What type of SLAM are you using? Also, what are the states you need to "fill in"? Are you trying to predict pose, landmarks, point clouds? $\endgroup$ – Ralff Apr 3 '17 at 1:15
  • $\begingroup$ Hi, thanks for your reply. I have added more info. I need to predict camera pose as translation xyz and quaternion rotation. It is an implementation of ORBSLAM2. $\endgroup$ – anti Apr 3 '17 at 8:55
  • $\begingroup$ ... But for now, if I could get even a single value predicting as i want, i can extrapolate from there! $\endgroup$ – anti Apr 3 '17 at 11:06

In this situation, you probably shouldn't use a Kalman filter for "filling" in the gaps. Typically, you use a Kalman filter (or Bayes filter etc...) to fuse information from different sources.

In your case, you have are using SLAM for localization. The SLAM algorithm is taking in data and estimating the pose of the camera as well as a sparse 3D reconstruction of the environment. Internally, this probably already performs some type of prediction/update for tracking etc...

The problem you are having is that the algorithm is only running at 30Hz, and you need 60Hz of pose data if I understand correctly. If I were you, I would take the states (position and velocity) at each time step and propagate them forward (at 60Hz).

To propagate them forward, I mean integrate your pose using your previous state estimates. For example, if your states were x position, y position, x velocity, and y velocity, then you can assume your velocity is constant and propagate your position using that velocity.

$$x_{k+1} = x_k + v_{x,k} \Delta t$$

$$y_{k+1} = y_k + v_{y,k} \Delta t$$

Note that you will need to write out the equations for your system. In the above equations, the $k$ is the previous time step, and $\Delta t$ is the change in time between the previous time step and the current time step. Once you receive another measurement from the SLAM algorithm, use that measurement instead of the propagated state, then repeat. You can keep the intermediate propagated states, but you don't want to perform an update because your propagated states are not based on sensor measurements. So, basically in between the measurements you get from ORBSLAM, you will have these intermediate measurements.

Edit: Keep in mind that the question you asked is about how to fill in the gaps given low frequency data. I am assuming you do not have extra sensors such as IMU, GPS, encoders, etc... Regardless, if additional sensors are available, this still doesn't address how to obtain data at a higher frequency (although it is likely the IMU will operate above 50Hz). I am suggesting an approach to fill in missing data without adding complexity to your system. Assuming constant velocity is reasonable if the motion isn't highly dynamic. If you do have additional sensors you want to incorporate, I think you would want to reconsider your overall approach because you can use IMU and encoders to improve the feature tracking and reconstruction.

| improve this answer | |
  • $\begingroup$ Thanks! Can you please explain what you mean by propagate them forward (at 60Hz) ? I need to avoid the 'stepping' as above, so i cannot just duplicate the same value until i get a new updated value. $\endgroup$ – anti Apr 3 '17 at 16:39
  • 1
    $\begingroup$ @anti, the 'k' you see is a second subscript on the velocity variable. You want (new x value) = (current x value) + (current velocity) * (time difference). NOTE: To be most correct you should also propagate for the covariance matrices for the save time difference. If you do this then you can simply apply the measurement when it arrives and it will 'just work'. If you don't want to do that, or it's too confusing, you need to remember all the variables from the last measurement step (i.g. ignore any variables from the intermediate propagation) $\endgroup$ – ryan0270 Apr 3 '17 at 20:42
  • 1
    $\begingroup$ Sorry, that sentence was horrible (fingers not typing what I told them). It should read "propagate the covariance matrices for the same time difference". I'm not sure what source you're using for your Kalman filter, but using en.wikipedia.org/wiki/Kalman_filter as a reference I'm talking about the second equation under the Details::Predict section (the first equation is just the x update equation you already have now, but in a fancied up form) $\endgroup$ – ryan0270 Apr 3 '17 at 21:09
  • 1
    $\begingroup$ No, I mean when you get a new measurement you need to throw away all the intermediate steps before applying the new measurement. This means using all the old variable values you had immediately after the last measurement. $\endgroup$ – ryan0270 Apr 3 '17 at 21:18
  • 1
    $\begingroup$ Please see my additional edits. I still do not recommend a Kalman filter for this application. You do not need to propagate covariance since you would just be filling in the gaps. You are not modifying the 30Hz solution. $\endgroup$ – Ralff Apr 3 '17 at 21:37

Short answer, yes, if you use your IMU to fill in the gaps.

You can use additional Bayesian filtering to fill in the pose gaps as long as you fuse in your IMU data. The IMU (assuming it has reasonable covariance and frequency) will be closer to the ground truth between the gaps than the forward propogation cBEiN recommends. cBEiN gives the solution for constant velocity between frames, but if you can't assume your robot has zero acceleration or angular acceleration (e.g. it's vibrating) between frames, you have all the tools you need at your fingertips in your trusty IMU. There's not so much drift creating a full state estimate in just 33ms. Integrating the IMU data to create poses between fused states (that is, state estimates made using both IMU and visual data) can fill in the time between your visual frames. An additional benefit is that it will account for sharp accelerations that are too blurry on camera to properly estimate your pose. These are situations which would often result in loss of localization, but by feeding the fused estimate back into your visual SLAM when its covariance is better than the visual odometry's covariance, it ought to put you back on track and prevent some of the major distortions that require additional loop closures to correct. Lastly, if you're using a depth camera, many of these cameras suffer from depth distortion during lateral movements or rotations, which can throw off your localization. If you notice these effects, it's again a great opportunity to fuse your IMU.

If you're using ROS, this is a great use-case for the robot_localization package.

| improve this answer | |
  • $\begingroup$ Hi, and thank you for your reply. I am on windows, without ROS, sadly. (Our renderer is windows only). So I could potentially use: bayesclasses.sourceforge.net/Bayes++.html here? I am a bit lost on how to proceed! $\endgroup$ – anti Apr 3 '17 at 20:41
  • $\begingroup$ Have you considered using ROS on the robot and simply feeding the data in to Windows? While it's a bad plan to use ROS exclusively on Windows, I've been able to have simple ROS nodes on Windows and send data back and forth between the robot and Windows base station. $\endgroup$ – Nate Gardner Apr 3 '17 at 20:43
  • $\begingroup$ we are too tied into a windows pipeline at this point, and it has to stay that way for now! $\endgroup$ – anti Apr 3 '17 at 20:46
  • $\begingroup$ It's been a long time since I've looked at Bayes++, but, without knowing much about your robot, I suggest you think about the linearity of your system to decide whether KF, EKF, or UKF meets your needs. There's also always particle filtering if you have multimodal data, but I'm pretty sure ORBSLAM has a unimodal/normal pose estimate. You should create a 15-dimensional state estimate and fuse the data according to covariance of both your visual odometry and your IMU. The covariance exponentiates each time you integrate. Also, do you have a pre-fused IMU? That can help a lot in this situation $\endgroup$ – Nate Gardner Apr 3 '17 at 20:47
  • 1
    $\begingroup$ This is old but really relevant here: youtube.com/watch?v=C7JQ7Rpwn2k If you have a pre-fusion package like this for the IMU, you can then simply run the direct state estimates out of the fused datastream into your filter, along with the pose coming off your visual odometry. $\endgroup$ – Nate Gardner Apr 3 '17 at 20:48

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.