Scale (arc length) estimation during curves

[dannyway03]

hi Johannes,

I read in CostFunctorCeresRollPitchYaw class the following ///@brief here we do not optimize the arclength so the arclength has to be determined from outside /// @todo add variant with arc length as a variable double arc_length_fixed_

and I'm having hard time in understanding how the arc_length can be obtained from the optimization rather than external sources. Can you provide more details that I can use as reference for the implementation?

That would be great! Many thanks. -d

[johannes-graeter]

Hi Danny,

it is so awesome that you read my code in such detail and pay attention to the nifty bits :) Thanks for asking, this is something that amazed me at first and took some time to understand. First I found that with my first setup, a calibrated multi camera setup. When you drive around a corner each camera follow a different motion, so I thought from the different radii of these motions and the known calibrated pose between them, the arc length becomes temporarily determined and you could estimate it (you can see that actually in the error landscape of the optimization problem). However when deriving it mathematically I found to my own surprise, that this is also valid for a mono setup, if you know the pose of the camera to your vehicle center of motion (except some degenerate cases). I derived that mathematically in my dissertation ( https://books.google.de/books?hl=de&lr=https://books.google.de/books?hl=de&lr=&id=cZW8DwAAQBAJ&oi=fnd&pg=PR3&dq=johannes+gr%C3%A4ter&ots=ZP5q9CM7ei&sig=XeJGO74TB6HWYUC9lS4xIuu3EzU&redir_esc=y#v=onepage&q=johannes%20gr%C3%A4ter&f=false&id=cZW8DwAAQBAJ&oi=fnd&pg=PR3&dq=johannes+gr%C3%A4ter&ots=ZP5q9CM7ei&sig=XeJGO74TB6HWYUC9lS4xIuu3EzU&redir_esc=y#v=onepage&q=johannes%20gr%C3%A4ter&f=false , page 65-70) which is in German but hopefully the math, graphs and sketches speaks a universal language as well. I am happy to answer further questions as well since I deem it a cool topic :) Would be awesome if you would like to implement that and I would be more than happy to support!

Happy reading! Best Johannes

[dannyway03]

Great Johannes! Thanks for the reference. Although German is not really my superpower, I will try to understand the concept, and hopefully be able to put it in code. Any chance you already have some reference implementation i can build on top? I'll keep you posted if any update or further question! Cheers, -d

[johannes-graeter]

Hi, my silence was a way too convenient way of saying " no there isn't one" ... However it is quite simple really. The motion model already supports the arc length as a variable input so you only have to add it as a variable to the ceres cost functors and feed it in (make sure not to copy it along the way but pass it as pointer....)

[bereze]

Hi Johannes, The implementation so far is used to optimize the relative rotation, and translation part is derived from the motion model with optimized rotation and travelled distance, right? I also have read your dissertation although having difficulties in German. As I understand, the derivation is based on the fact that the camera is in front of the vehicle? If there are 6 DoF transform (like lateral displacement) between the camera and the car body, is the derivation still valid?

Thanks.