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

Fixed localCoordinates bug in all three Calibration objects.

release/4.3a0
dellaert 2014-02-25 21:29:38 -05:00
parent 49a1b7abca
commit c3f0bf4949
6 changed files with 42 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Cal3Bundler.cpp
View File

@ -152,7 +152,7 @@ Cal3Bundler Cal3Bundler::retract(const Vector& d) const {
/* ************************************************************************* */
Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const {
return vector() - T2.vector();
return T2.vector() - vector();
}
}

21
gtsam/geometry/Cal3DS2.cpp
View File

@ -28,13 +28,20 @@ Cal3DS2::Cal3DS2(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){}
/* ************************************************************************* */
Matrix Cal3DS2::K() const { return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); }
Matrix Cal3DS2::K() const {
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
}
/* ************************************************************************* */
Vector Cal3DS2::vector() const { return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); }
Vector Cal3DS2::vector() const {
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_);
}
/* ************************************************************************* */
void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K"); gtsam::print(Vector(k()), s + ".k"); }
void Cal3DS2::print(const std::string& s) const {
gtsam::print(K(), s + ".K");
gtsam::print(Vector(k()), s + ".k");
}
/* ************************************************************************* */
bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
@ -173,10 +180,14 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
}
/* ************************************************************************* */
Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d); }
Cal3DS2 Cal3DS2::retract(const Vector& d) const {
return Cal3DS2(vector() + d);
}
/* ************************************************************************* */
Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const { return vector() - T2.vector(); }
Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const {
return T2.vector() - vector();
}
}
/* ************************************************************************* */

2
gtsam/geometry/Cal3_S2.h
View File

@ -186,7 +186,7 @@ public:
/// Unretraction for the calibration
Vector localCoordinates(const Cal3_S2& T2) const {
return vector() - T2.vector();
return T2.vector() - vector();
}
/// @}

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

@ -82,6 +82,7 @@ TEST( Cal3Bundler, retract)
d << 10, 1e-3, 1e-3;
Cal3Bundler actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-7));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
}
/* ************************************************************************* */

12
gtsam/geometry/tests/testCal3DS2.cpp
View File

@ -76,6 +76,18 @@ TEST( Cal3DS2, assert_equal)
CHECK(assert_equal(K,K,1e-5));
}
/* ************************************************************************* */
TEST( Cal3DS2, retract)
{
Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6,
2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9);
Vector d(9);
d << 1,2,3,4,5,6,7,8,9;
Cal3DS2 actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-7));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

11
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -82,6 +82,17 @@ TEST( Cal3_S2, assert_equal)
CHECK(assert_equal(K,K1,1e-9));
}
/* ************************************************************************* */
TEST( Cal3_S2, retract)
{
Cal3_S2 expected(500+1, 500+2, 0.1+3, 640 / 2+4, 480 / 2+5);
Vector d(5);
d << 1,2,3,4,5;
Cal3_S2 actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-7));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
}
/* ************************************************************************* */
int main() {
TestResult tr;