ethz-asl / ethzasl_ptam

Modified version of Parallel Tracking and Mapping (PTAM)
http://wiki.ros.org/ethzasl_ptam
235 stars 184 forks source link

Calibration problem with fisheye lens #25

Open clod85 opened 11 years ago

clod85 commented 11 years ago

I need a little help. I've got a Point Grey camera FireFly MV;I have a high quality fisheye lens that provides a 190 degree field of view and a normal lens .

I did the calibration with both lenses and I got this results:

normal lens: [Cam_fx: 1.65485 Cam_fy: 2.20534 Cam_cx: 0.558802 Cam_cy: 0.502825 Cam_s: 0.742477]

fisheye lens: [Cam_fx: 0.489675 Cam_fy: 0.633751 Cam_cx: 0.583104 Cam_cy: 0.411363 Cam_s: 0]

PTAM works good when I mount the normal lens. I can verify that it correctly works because the tests suggested on the tutorials succeed.

When I run PTAM with the fish-eye lens it loses immediately the tracking and the map. It's quite impossible to try the test to verify how good is calibration. Anyway I try to shoot on the keyboard and its representation on the map is not flat. This is probably due to the distorsion factor that's 0 (!). I strongly doubt about calibration parameters. I've done several attempts but I always get bad results. I'd like to mention that I obtain low RMS values, like 0.4(never 0.3 as you suggest). Shutter speed = 5ms, the chessboard was on a flat surface and the pattern around was uniform; the room was enough enlightened.

I even try to use another toolbox to calibrate the camera and then modify the data to match with PTAM requirements. I didn't get good results neither because probably I modelled wrongly the distortion parameter. However I got very different values of focal length and image center.

Can I have your opinion about my calibration data? Are there any other reasons why PTAM not work with my fish-eye lens?

system123 commented 11 years ago

You need to realize that the Cam_s parameter is not a distortion parameter but rather a skew parameter. It has nothing to do with the distortion the lens adds but rather with the physical pixels on the imaging sensor. In order to use a fisheye camera you need to account for the distortion it adds, and rectify the images before feeding them into the PTAM system. Have a look at this paper to see how you can calibrate the fisheye to correct for distortion http://www.fieldrobotics.org/~cgeyer/OMNIVIS05/final/Ho.pdf

stephanweiss commented 11 years ago

The "s" is a bit misleading. Most pinhole models use "s" as the skew parameter Lloyd Hughes is describing. In the PTAM camera model (the ATAN or FoV model) "s" describes the distortion parameter to keep straight lines straight (from the paper "straight lines have to be straight").

A value of s=0 indicates no distortion. Did you accidentally check the "no dist" option during calibration? If you are working with a 180°+ FoV the "s" value should be close and above 1. So best thing to do is to redo the calib and check if all images/grids are correctly displayed.

The above being said, we experienced many issues with wide FoV lenses (larger than 150°). The calibration procedure is iffy and tedious. Also, PTAM still works with the epipolar constraints, so you want to make sure to only use max 179° of the FoV otherwise the reprojection function throws an error: set MaxFoV: 179 in the parameter yaml file.

I hope this helps

Best Stephan


From: Lloyd Hughes [notifications@github.com] Sent: Tuesday, July 30, 2013 10:53 AM To: ethz-asl/ethzasl_ptam Subject: Re: [ethzasl_ptam] Calibration problem with fisheye lens (#25)

You need to realize that the Cam_s parameter is not a distortion parameter but rather a skew parameter. It has nothing to do with the distortion the lens adds but rather with the physical pixels on the imaging sensor. In order to use a fisheye camera you need to account for the distortion it adds, and rectify the images before feeding them into the PTAM system. Have a look at this paper to see how you can calibrate the fisheye to correct for distortion http://www.fieldrobotics.org/~cgeyer/OMNIVIS05/final/Ho.pdfhttp://www.fieldrobotics.org/%7Ecgeyer/OMNIVIS05/final/Ho.pdf

— Reply to this email directly or view it on GitHubhttps://github.com/ethz-asl/ethzasl_ptam/issues/25#issuecomment-21809413.

clod85 commented 11 years ago

Hi,

thank Lloyd Hughes for the precious article, it made my ideas more clear and It helped me understand what the problem is.

Before trying to rectify the images I've made several attempts to get new calibration parameters, following Stephan's suggestions. RMS = 5.28498 of reprojection error is the best I could get after many attempts. Those are the coefficents I got:

[Cam_fx: 0.40913 Cam_fy: 0.60955 Cam_cx: 0.50158 Cam_cy: 0.46875 Cam_s: 1.04794]

I think the error is too high in fact I can't neither initialize the map. My next step will be to try to rectify the images because I think a 90 degree lens dramatically limit PTAM perfomances. I thought that rectifing was optional because many calibration toolboxes work with unrectified images.

What kind of lens do you think is the best for PTAM performance? Thank you and I hope I'm not disturbing.

Claudio

stephanweiss commented 11 years ago

Claudio,

your RMS error indeed is too high. I do not have good experience with feeding the calibrator with rectified images. In your calibration attempts, did you check if all captured images are ok (there is a button "next" in the gui that scrolls through all images taken so you can check the grid overlay)?

For PTAM I would recommend a 120° up to 150°FoV lens. We made very good experiences with such type of lenses.

I hope this helps Stephan


From: clod85 [notifications@github.com] Sent: Thursday, August 01, 2013 5:51 AM To: ethz-asl/ethzasl_ptam Cc: stephanweiss Subject: Re: [ethzasl_ptam] Calibration problem with fisheye lens (#25)

Hi,

thank Lloyd Hughes for the precious article, it made my ideas more clear and It helped me understand what the problem is.

Before trying to rectify the images I've made several attempts to get new calibration parameters, following Stephan's suggestions. RMS = 5.28498 of reprojection error is the best I could get after many attempts. Those are the coefficents I got:

[Cam_fx: 0.40913 Cam_fy: 0.60955 Cam_cx: 0.50158 Cam_cy: 0.46875 Cam_s: 1.04794]

I think the error is too high in fact I can't neither initialize the map. My next step will be to try to rectify the images because I think a 90 degree lens dramatically limit PTAM perfomances. I thought that rectifing was optional because many calibration toolboxes work with unrectified images.

What kind of lens do you think is the best for PTAM performance? Actually my opinion is that a wide-lens would be perfect because its geometry could be described with the camera model used. Thank you and I hope I'm not disturbing.

Claudio

— Reply to this email directly or view it on GitHubhttps://github.com/ethz-asl/ethzasl_ptam/issues/25#issuecomment-21933961.

anuragajay commented 9 years ago

@stephanweiss I am using a camera with fisheye lens and 120 deg FOV. In PTAM window, I am not able to see my entire image. Its shows me a small area around a corner rather than entire image. What do you think could be the issue? Also, the sample example on the website works on my machine.

stephanweiss commented 9 years ago

Are you using the remote_ptam preview of the OpenGL preview that comes with the original PTAM code? If it is the latter, please make sure there is no graphic card issue. You may display the remote_ptam image, if this looks ok, it is most probably a graphics card issue.

Best, Stephan


From: anuragajay [notifications@github.com] Sent: Tuesday, August 18, 2015 1:30 PM To: ethz-asl/ethzasl_ptam Cc: Stephan Weiss Subject: Re: [ethzasl_ptam] Calibration problem with fisheye lens (#25)

@stephanweisshttps://github.com/stephanweiss I am using a camera with fisheye lens and 120 deg FOV. In PTAM window, I am not able to see my entire image. Its shows me a small area around a corner rather than entire image. What do you think could be the issue? Also, the sample example on the website works on my machine.

— Reply to this email directly or view it on GitHubhttps://github.com/ethz-asl/ethzasl_ptam/issues/25#issuecomment-132341513.

anuragajay commented 9 years ago

@stephanweiss Hi I am not using remote_ptam preview. I was just using the command "roslaunch ptam ptam.launch " and I did change the image topic in ptam launch file. But, with the sample Dataset(provided on website), PTAM is able to view entire image. I am also attaching screenshots so that you can confirm this. screenshot from 2015-08-18 16 17 37 screenshot from 2015-08-18 16 17 50

anuragajay commented 9 years ago

@stephanweiss I was feeding in colored image by mistake and that was creating problem. I have another problem. I am not using real world camera. I am taking pictures of simulation in gazebo by using a camera(in simulation) provided by a ROS package(hector quadrotor). Its intrinsics are:

height: 480 width: 752 distortion_model: plumb_bob D: [0.0, 0.0, 0.0, 0.0, 0.0] K: [217.0824735502627, 0.0, 376.5, 0.0, 217.0824735502627, 240.5, 0.0, 0.0, 1.0] R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] P: [217.0824735502627, 0.0, 376.5, -0.0, 0.0, 217.0824735502627, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0] binning_x: 0 binning_y: 0

But I don't know how to use these intrinsics to get cam_fx, cam_fy, cam_cx, cam_cy, cam_s. Also, it is clear that fx, fy,cx,cy in camera matrix K above is not same as cam_fx, cam_fy, cam_cx, cam_cy.

stephanweiss commented 9 years ago

The conversion from one camera model to another is not always trivial since all models are approximations of the true lens. You have different options:

a) simulate a checkerboard and use that for calibration in ptam's cameracalibrator

b) adapt the Atan camera model in PTAM to your camera model (i.e. re-write the undistort and distort functions in ATANCamera.cc/.h)

c) undistort/rectify the images using the camera model you have and use them in ptam with a Atan camera model for rectified images. fx, fy, cx, cy are actually cam_fx, cam_fy, cam_cx, cam_cy normalized by the image resolution.

Hope this helps.


From: anuragajay [notifications@github.com] Sent: Tuesday, August 18, 2015 11:37 PM To: ethz-asl/ethzasl_ptam Cc: Stephan Weiss Subject: Re: [ethzasl_ptam] Calibration problem with fisheye lens (#25)

@stephanweisshttps://github.com/stephanweiss I was feeding in colored image by mistake and that was creating problem. I have another problem. I am not using real world camera. I am taking pictures of simulation in gazebo by using a camera(in simulation) provided by a ROS package(hector quadrotor). Its intrinsics are:

height: 480 width: 752 distortion_model: plumb_bob D: [0.0, 0.0, 0.0, 0.0, 0.0] K: [217.0824735502627, 0.0, 376.5, 0.0, 217.0824735502627, 240.5, 0.0, 0.0, 1.0] R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] P: [217.0824735502627, 0.0, 376.5, -0.0, 0.0, 217.0824735502627, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0] binning_x: 0 binning_y: 0

But I don't know how to use these intrinsics to get cam_fx, cam_fy, cam_cx, cam_cy, cam_s. Also, it is clear that fx, fy,cx,cy in camera matrix K above is not same as cam_fx, cam_fy, cam_cx, cam_cy.

— Reply to this email directly or view it on GitHubhttps://github.com/ethz-asl/ethzasl_ptam/issues/25#issuecomment-132462572.

anuragajay commented 9 years ago

Thanks

anuragajay commented 9 years ago

@stephanweiss I simulated the checkboard but for some reason during calibration, the RMS error for me remains around 0.9. Any suggestion how to improve that.

When I run PTAM using above calibration, camera pose appears to be more or less same. This should be a result of bad calibration, right?

stephanweiss commented 9 years ago

what type of lens do you simulate? What is the PTAM behavior?


From: anuragajay [notifications@github.com] Sent: Thursday, August 20, 2015 4:14 AM To: ethz-asl/ethzasl_ptam Cc: Stephan Weiss Subject: Re: [ethzasl_ptam] Calibration problem with fisheye lens (#25)

@stephanweisshttps://github.com/stephanweiss I simulated the checkboard but for some reason during calibration, the RMS error for me remains around 0.9. Any suggestion how to improve that.

When I run PTAM using above calibration, camera pose appears to be more or less same. This should be a result of bad calibration, right?

— Reply to this email directly or view it on GitHubhttps://github.com/ethz-asl/ethzasl_ptam/issues/25#issuecomment-132977561.

anuragajay commented 9 years ago

@stephanweiss I don't know exactly the type of lens simulated. It doesn't exactly say that but the lens is an idealized lens without any distortion. Therefore I am uploading an image to give you a better idea. screenshot from 2015-08-20 17 27 58 Regarding calibration, I found a package hector_quadrotor_ptam which incorporates PTAM with hector quadrotor(simulation I am using) and I used their parameters. If I use their parameters, PTAM performs much better(though not perfect) given motion is very slow. I am again uploading 2 images to give you an idea. screenshot from 2015-08-20 17 26 35 screenshot from 2015-08-20 17 26 22

The color display on bottom left is image by my camera and the monochromatic image in middle is the image seen by PTAM(I basically convert RGB image to a monochromatic image).

anuragajay commented 9 years ago

@stephanweiss Just to confirm, vslam/pose_world gives camera pose wrt world and vslam/pose gives world pose wrt camera.

antonays commented 7 years ago

I have the same problem, I am using a raspberry pi with raspi camera, on which an external fish eye lens is attached. I need the lens to be able to apply lsd-slam algorithm which requires a wide lens for good results. (it works with the normal lens of the camera but is not able to track very well)

When using the fish eye lens i am not able to calibrate using the ptam tool.

Without the the additional lens, the calibrator works fine and gives (probably) the correct values. When attaching the lens the calibration tool works but is not able to recognize the chessboard pattern, so i am not able to take even a single snapshot, not to mention get a calibration.

Worth to note that it is possible to calibrate using the opencv calibrator (the one that comes with ROS) in both cases; with the lens and without, but this is not good for me because i need FoV camera parameters and not opencv. If i knew how to transform between the two i could bypass the problem but i don't have this information.

I've read the paper "Using Geometric Constraints for Fisheye Camera Calibration" that was suggested here but still cannot find a solution. When discussing Rectification of the image before the calibrator, does that mean to crop out the vignette that is a result of the fish eye?

Thank you @stephanweiss @system123 @clod85

sbarkby commented 7 years ago

@antonays

Hey Antonays, similar to you I'm using PTAM's camera calibrator as I need my fisheye lens (full 180 deg field of view) calibrated according to the FOV model and not openCV's model. Spent a day trying to get the calibrator to recognise the calibration pattern so I could grab images. Managed to do it in the end by increasing the blur sigma to 3.0 and corner patch pixel size to 45, done by adding the following lines to calibrator_settings.cfg:

CameraCalibrator.BlurSigma=3.0 CameraCalibrator.CornerPatchPixelSize=45

Once the calibrator could detect the chess board I managed to capture 5 images and run the optimiser but it seemed to prematurely exit, with an rms error listed as -nan.

After a little digging it turns out that halfway through the optimisation the calibrator decides the camera model is invalid (possibly because I'm using a full 180deg fisheye and the distortion is more extreme than other lenses, or because the fisheye projection is not completely contained within my 640 by 480 image).

If you get to this point and suffer the same symptoms I would recommend disabling the "Camera.Invalid()" check in vector CalibImage::Project(ATANCamera &Camera). After I did this I successfully got a calibration with rms error ~0.36

I would also alter the default calibration params so that the initial distortion coefficient is closer to 1 since you're using a fisheye e.g.

const Vector ATANCamera::mvDefaultParams = makeVector(0.5, 0.66, 0.5, 0.5, 1.0);

Hope this helps

antonays commented 7 years ago

@sbarkby Thank you for the reply.

Unfortunately i am still not able to calibrate even with the tips your provided. I am not even able to get to the point that the calibrator says that the model is invalid, It is just not able to pick up the pattern and i am not able to grab any image using the tool.

Just to make sure, when you were referring to the changes in the config file, it was the file src/ethzasl_ptam/ptam/PtamFixParams.yaml right? then the options that you mentioned inside this file are: Calibrator_BlurSigma: 3.0 CameraCalibrator_CornerPatchSize: 45

Still no luck for me...

I am starting to think that the problem is inherent with the camera i am using and there is no way to solve it. I am using a Raspberry pi with a Raspberry pi camera module. It is a rolling shutter so this fact might give problems. Moreover, the lens i am using is an external fish eye that temporarily attached on top of the camera. The lens gives very strong distortion, i get a very blurry image that the calibration tool is not able to process at all, it does not even try to match the pattern. I am thinking that it might be because the image is very blurry. Trying to calibrate without the lens, everything works fine and i am able to calibrate with an RMS of less than 0.5 as required. I have an additional wide lens, 0.67 of 180 degrees, with it, i am experiencing some calibration problems but am able to calibrate with RMS less than 1. Can't get lower RMS.

This is what i get with no wide lens - crisp image, easy to calibrate nowide

This is what i get with the 0.67 lens, worst image, possible to calibrate, bad results image_narrow

This is what i get with the fish eye lens - impossible to calibrate, blur is very noticeable even with naked eye image_wide

sbarkby commented 7 years ago

@antonays

Ah, I'm using the GPL release of the original version (https://github.com/Oxford-PTAM/PTAM-GPL) not the one that was modified for ROS. In this version the calibration settings are kept in calibrator_settings.cfg. Worth noting it took me half a day of hacking to create a build system and remove the bit rot from this version (as the library dependencies have moved on since its release)

Below is a screenshot of the fisheye lens I used. screenshot_2017-03-15_17-12-41

Yes I would agree the likely problem is that the image is just too blurry, you definitely don't need as much Calibrator_BlurSigma as I used. The rolling shutter will not give problems during calibration, only potentially during PTAM tracking if you are moving the camera very fast.

antonays commented 7 years ago

Thank you @sbarkby. I think i now understood that my problem is with the focal length, so i will try to play with it and update here.

Thanx!