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 thd_th = 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?