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

add test case. correct constness for backproject2 definition

release/4.3a0
lvzhaoyang 2015-06-12 10:47:02 -04:00
parent cb76d321d3
commit a5d74f77d7
2 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/StereoCamera.cpp
View File

@ -97,9 +97,9 @@ namespace gtsam {
/* ************************************************************************* */
Point3 StereoCamera::backproject2(const StereoPoint2& z, OptionalJacobian<3, 6> H1,
OptionalJacobian<3, 3> H2) {
OptionalJacobian<3, 3> H2) const {
const Cal3_S2Stereo& K = *K_;
const double fx = K.fx(), fy = K.fy(), cx = K.cx(), cy = K.cy(), b = K.baseline();
const double fx = K.fx(), fy = K.fy(), cx = K.px(), cy = K.py(), b = K.baseline();
Vector3 measured = z.vector(); // u_L, u_R, v
double d = measured[0] - measured[1]; // disparity
@ -110,7 +110,7 @@ namespace gtsam {
if(H1 || H2) {
if(H1) {
// do something here, w.r.t pose
}
if(H2) {
double d_2 = d*d;

17
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -149,8 +149,11 @@ TEST( StereoCamera, backproject2_case1)
Point3 actual_point = stereoCam2.backproject2(stereo_point, actual_jacobian_1, actual_jacobian_2);
CHECK(assert_equal(expected_point, actual_point, 1e-8));
Matrix expected_jacobian = numericalDerivative32(backproject3, Pose3(), stereo_point, *K2);
CHECK(assert_equal(expected_jacobian, actual_jacobian_1, acutal_jacobian_2));
Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, Pose3(), stereo_point, *K2);
CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-3));
Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, Pose3(), stereo_point, *K2);
CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-3));
}
TEST( StereoCamera, backproject2_case2)
@ -163,9 +166,17 @@ TEST( StereoCamera, backproject2_case2)
StereoCamera camera(Pose3(R,t), K);
StereoPoint2 z(184.812, 129.068, 714.768);
Point3 l = camera.backproject2(z);
Matrix actual_jacobian_1, actual_jacobian_2;
Point3 l = camera.backproject2(z, actual_jacobian_1, actual_jacobian_2);
StereoPoint2 actual = camera.project(l);
CHECK(assert_equal(z, actual, 1e-3));
Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, camera, z, *K);
CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-3));
Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, camera, z, *K);
CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-3));
}
/* ************************************************************************* */