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

create a backproject2, with optional Jacobians

release/4.3a0
zhaoyang 2015-06-11 11:40:40 -04:00
parent ed09e10331
commit 48d159120d
3 changed files with 70 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/geometry/StereoCamera.cpp
View File

@ -95,4 +95,27 @@ namespace gtsam {
return world_point;
}
/* ************************************************************************* */
Point3 StereoCamera::backproject2(const StereoPoint2& z, OptionalJacobian<3, 6> H1,
OptionalJacobian<3, 3> H2) {
const Cal3_S2Stereo& K = *K_;
const double fx = K.fx(), fy = K.fy(), cx = K.cx(), cy = K.cy(), b = K.baseline();
Vector measured = z.vector();
double Z = b * fx / (measured[0] - measured[1]);
double X = Z * (measured[0] - cx) / fx;
double Y = Z * (measured[2] - cy) / fy;
if(H1 || H2) {
if(H1) {
// do something here
}
if(H2) {
}
}
return leftCamPose_.transform_from(Point3(X, Y, Z));
}
}

8
gtsam/geometry/StereoCamera.h
View File

@ -137,6 +137,14 @@ public:
/// back-project a measurement
Point3 backproject(const StereoPoint2& z) const;
/** Back-project the 2D point and compute optional derivatives
* @param H1 derivative with respect to pose
* @param H2 derivative with respect to point
*/
Point3 backproject2(const StereoPoint2& z,
OptionalJacobian<3, 6> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/// @}
/// @name Deprecated
/// @{

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

@ -99,13 +99,13 @@ TEST( StereoCamera, Dproject)
Matrix actual1; stereoCam.project2(landmark, actual1, boost::none);
CHECK(assert_equal(expected1,actual1,1e-7));
Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K);
Matrix expected2 = numericalDerivative32(project3, camPose, landmark, *K);
Matrix actual2; stereoCam.project2(landmark, boost::none, actual2);
CHECK(assert_equal(expected2,actual2,1e-7));
}
/* ************************************************************************* */
TEST( StereoCamera, backproject)
TEST( StereoCamera, backproject_case1)
{
Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
StereoCamera stereoCam2(Pose3(), K2);
@ -117,7 +117,7 @@ TEST( StereoCamera, backproject)
}
/* ************************************************************************* */
TEST( StereoCamera, backproject2)
TEST( StereoCamera, backproject_case2)
{
Rot3 R(0.589511291, -0.804859792, 0.0683931805,
-0.804435942, -0.592650676, -0.0405925523,
@ -132,6 +132,42 @@ TEST( StereoCamera, backproject2)
CHECK(assert_equal(z, actual, 1e-3));
}
static Point3 backproject3(const Pose3& pose, const StereoPoint2& point, const Cal3_S2Stereo& K) {
return StereoCamera(pose, boost::make_shared<Cal3_S2Stereo>(K)).backproject(point);
}
/* ************************************************************************* */
TEST( StereoCamera, backproject2_case1)
{
Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
StereoCamera stereoCam2(Pose3(), K2);
Point3 expected_point(1.2, 2.3, 4.5);
StereoPoint2 stereo_point = stereoCam2.project(expected_point);
Matrix actual_jacobian_1, actual_jacobian_2;
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));
}
TEST( StereoCamera, backproject2_case2)
{
Rot3 R(0.589511291, -0.804859792, 0.0683931805,
-0.804435942, -0.592650676, -0.0405925523,
0.0732045588, -0.0310882277, -0.996832359);
Point3 t(53.5239823, 23.7866016, -4.42379876);
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612));
StereoCamera camera(Pose3(R,t), K);
StereoPoint2 z(184.812, 129.068, 714.768);
Point3 l = camera.backproject2(z);
StereoPoint2 actual = camera.project(l);
CHECK(assert_equal(z, actual, 1e-3));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */