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

add "between" so Cal3_S2 can be used with BetweenFactor

release/4.3a0
cbeall3 2014-08-20 13:05:14 -04:00
parent bc205cf6a4
commit 3bb9b38c1f
2 changed files with 21 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/Cal3_S2.h
View File

@ -165,6 +165,16 @@ public:
*/
Vector3 calibrate(const Vector3& p) const;
/// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
inline Cal3_S2 between(const Cal3_S2& q,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(5);
if(H2) *H2 = eye(5);
return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
}
/// @}
/// @name Manifold
/// @{

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

@ -93,6 +93,17 @@ TEST( Cal3_S2, retract)
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
}
/* ************************************************************************* */
TEST(Cal3_S2, between) {
Cal3_S2 k1(5, 5, 5, 5, 5), k2(5, 6, 7, 8, 9);
Matrix H1, H2;
EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2)));
EXPECT(assert_equal(-eye(5), H1));
EXPECT(assert_equal(eye(5), H2));
}
/* ************************************************************************* */
int main() {
TestResult tr;