pixelToBearing failed?

[rising-turtle]

Hi all:

The function pixelToBearing failed when running rovio with the calibrated camera. The output is shown below. While the e.dot(e) is decreasing, the magnitude of du and the value of y_bar are increasing heavily, resulting in diverge solutions.

What's the possible reason that lead to this ? Is it because that the camera intrinsic parameter or distortion parameter is not correct?

Any suggestion is greatly appreciated, thanks!

Set Camera Matrix to: 190.978 0 254.932 0 190.973 256.897 0 0 1 Set distortion parameters (Equidist) to: k1(0.00348239), k2(0.000715035), k3(-0.00205324), k4(0.000202937) e.dot(e) = 0.369963 > tolerance: 1e-10 du = -1.71043, -1.42448 ybar = -2.96414, -2.46859 e.dot(e) = 0.0994242 > tolerance: 1e-10 du = -3.96508, -3.30218 ybar = -6.92922, -5.77077 e.dot(e) = 0.0318826 > tolerance: 1e-10 du = -12.0547, -10.0394 ybar = -18.9839, -15.8101 e.dot(e) = 0.0129381 > tolerance: 1e-10 du = -58.4364, -48.6669 ybar = -77.4203, -64.477 e.dot(e) = 0.00739563 > tolerance: 1e-10 du = -742.798, -618.615 ybar = -820.219, -683.092 e.dot(e) = 0.00606772 > tolerance: 1e-10 du = -75801.3, -63128.6 ybar = -76621.5, -63811.7 e.dot(e) = 0.00593853 > tolerance: 1e-10 du = -6.54664e+08, -5.45216e+08 ybar = -6.54741e+08, -5.4528e+08 e.dot(e) = 0.00593714 > tolerance: 1e-10 du = 3.65339e+14, 3.0426e+14 ybar = 3.65338e+14, 3.0426e+14 e.dot(e) = 10.1509 > tolerance: 1e-10 du = -nan, -nan ybar = -nan, -nan du = -nan -nan test_camera: failed to pixelToBearing,

[rising-turtle]

The input point c is (15.5, 57.5), to call pixelToBearing(c, vec).

[bloesch]

I have not experience any such failure before. Does it show this behavior for every point or only for (15.5, 57.5)? You could maybe try adding some damping (decrease the du, or added some diagonal damping to JTJ)?

[rising-turtle]

Hi bloesch: Thanks for reply. Not every point nor only one point cannot converge. It seems many points diverge, try (15.5, 57.5) or (25.5, 67.5). Below I extract this piece of code so you can test by yourself. Try different input 'a' to check out.

best,

/ test camera model, especially distortion mapping /

include

include

include

include <eigen3/Eigen/Dense>

using namespace std;

class Cam { public: Cam() { fx = 190.978; cx = 254.932; fy = 190.973; cy = 256.897; k1 = 0.00348239; k2 = 0.000715035; k3 = - 0.00205324; k4 = 0.000202937; }; void distortEquidist(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const;

test_cam_model_2 (copy).txt double fx; double fy; double cx; double cy; double k1; double k2; double k3; double k4; };

void test_cam_model(); bool pixelToBearing(Cam& cam, const Eigen::Vector2d& c,Eigen::Vector3d& vec);

int main(int argc, char* argv[]) { test_cam_model(); return 0; }

void test_cam_model() { Eigen::Vector2d a(25.5, 67.5); // (419, 103); // 15.5, 57.5 Eigen::Vector3d b; Cam cam; cout <<"input a = "<<endl<<a<<endl; if(pixelToBearing(cam, a, b)) { cout <<"succeed to converge! a: "<<a<<" b: "<<b<<endl; }else { cout <<"failed to converge!"<<endl; } return ; } bool pixelToBearing(Cam& cam, const Eigen::Vector2d& c,Eigen::Vector3d& vec) { Eigen::Vector2d y; double fx = cam.fx; double fy = cam.fy; double cx = cam.cx; double cy = cam.cy; y(0) = (static_cast(c.x()) - cx) / fx; y(1) = (static_cast(c.y()) - cy) / fy; // Undistort by optimizing const int max_iter = 100; const double tolerance = 1e-10; // 1e-10 Eigen::Vector2d ybar = y; // current guess (undistorted) Eigen::Matrix2d J; Eigen::Vector2d y_tmp; // current guess (distorted) Eigen::Vector2d e; Eigen::Vector2d du; bool success = false; for (int i = 0; i < max_iter; i++) { cam.distortEquidist(ybar, y_tmp, J); e = y - y_tmp; du = (J.transpose() J).inverse() J.transpose() e; ybar += du; if (e.dot(e) <= tolerance){ success = true; break; }else{ std::cout<<" y = "<<y<<" y_tmp = "<<y_tmp<<" e = "<<e<<std::endl; std::cout<<"e.dot(e) = "<<e.dot(e)<<" > tolerance: "<<tolerance<<std::endl; std::cout<<"du = "<<du(0)<<", "<<du(1)<<" ybar = "<<ybar(0)<<", "<<ybar(1)<<std::endl; } if(du!=du) { std::cout<<"du = "<<du<<" break!"<<std::endl; break; } } if(success){ y = ybar; vec = Eigen::Vector3d(y(0),y(1),1.0).normalized(); } return success; } void Cam::distortEquidist(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const { const double x2 = in(0) in(0); const double y2 = in(1) in(1); const double r = std::sqrt(x2 + y2); if(r < 1e-8){ out(0) = in(0); out(1) = in(1); J.setIdentity(); return; } const double r_x = 1/r in(0); const double r_y = 1/r in(1); const double th = atan(r); // 1/(r^2 + 1) const double th_r = 1/(r r+1); const double th2 = th th; const double th4 = th2 th2; const doubl/e th6 = th2 th4; const double th8 = th2 th6; const double thd = th (1.0 + k1_ th2 + k2 * th4 + k3 th6 + k4_ th8); const double thdth = 1.0 + 3 * k1 th2 + 5 k2 th4 + 7 k3 th6 + 9 k4_ th8; const double s = thd/r; const double s_r = thd_th th_r/r - thd/(r r); out(0) = in(0) s; out(1) = in(1) s; J(0,0) = s + in(0) s_r r_x; J(0,1) = in(0) s_r r_y; J(1,0) = in(1) s_r r_x; J(1,1) = s + in(1) s_r * r_y; }

[bloesch]

I see you do have quite a wide-angle camera, does it also diverge if you employ radial-tangential distortion model?