12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

addressed comments and added an additional test

release/4.3a0
Varun Agrawal 2021-06-03 11:06:13 -04:00
parent fb784eea9c
commit b844026951
2 changed files with 29 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/Cal3Bundler.cpp
View File

@ -96,11 +96,9 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
OptionalJacobian<2, 2> Dp) const {
// Copied from Cal3DS2
// but specialized with k1, k2 non-zero only and fx=fy and s=0
double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
const Point2 invKPi(x, y);
double px = (pi.x() - u0_) / fx_, py = (pi.y() - v0_) / fx_;
const Point2 invKPi(px, py);
Point2 pn;
double px = pi.x(), py = pi.y();
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;

27
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -89,6 +89,33 @@ TEST(Cal3Bundler, DcalibrateDefault) {
CHECK(assert_equal(numerical2, Dp, 1e-5));
}
/* ************************************************************************* */
TEST(Cal3Bundler, DuncalibratePrincipalPoint) {
Cal3Bundler K(5, 0, 0, 2, 2);
Matrix Dcal, Dp;
Point2 actual = K.uncalibrate(p, Dcal, Dp);
Point2 expected(12, 17);
CHECK(assert_equal(expected, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical1, Dcal, 1e-7));
CHECK(assert_equal(numerical2, Dp, 1e-7));
}
/* ************************************************************************* */
TEST(Cal3Bundler, DcalibratePrincipalPoint) {
Cal3Bundler K(2, 0, 0, 2, 2);
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 actual = K.calibrate(pi, Dcal, Dp);
CHECK(assert_equal(pn, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical1, Dcal, 1e-5));
CHECK(assert_equal(numerical2, Dp, 1e-5));
}
/* ************************************************************************* */
TEST(Cal3Bundler, Duncalibrate) {
Matrix Dcal, Dp;