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

changed backProject to project to world coordinates

release/4.3a0
Chris Beall 2011-03-29 05:40:30 +00:00
parent f1746da36b
commit 12d7b05bd9
2 changed files with 18 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/geometry/StereoCamera.h
View File

@ -85,7 +85,8 @@ namespace gtsam {
double Z = K_.baseline()*K_.fx()/(measured[0]-measured[1]); double Z = K_.baseline()*K_.fx()/(measured[0]-measured[1]);
double X = Z *(measured[0]- K_.px()) / K_.fx(); double X = Z *(measured[0]- K_.px()) / K_.fx();
double Y = Z *(measured[2]- K_.py()) / K_.fy(); double Y = Z *(measured[2]- K_.py()) / K_.fy();
return Point3(X, Y, Z); Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
return world_point;
} }
/** Dimensionality of the tangent space */ /** Dimensionality of the tangent space */

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

@ -109,20 +109,22 @@ TEST( StereoCamera, backproject)
} }
/* ************************************************************************* */ /* ************************************************************************* */
//TEST( StereoCamera, backproject2) TEST( StereoCamera, backproject2)
//{ {
// Rot3 R(0.589511291, -0.804859792, 0.0683931805, Rot3 R(0.589511291, -0.804859792, 0.0683931805,
// -0.804435942, -0.592650676, -0.0405925523, -0.804435942, -0.592650676, -0.0405925523,
// 0.0732045588, -0.0310882277, -0.996832359); 0.0732045588, -0.0310882277, -0.996832359);
// Point3 t(53.5239823, 23.7866016, -4.42379876); Point3 t(53.5239823, 23.7866016, -4.42379876);
// Cal3_S2Stereo K(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612); Cal3_S2Stereo K(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612);
// StereoCamera camera(Pose3(R,t), K); StereoCamera camera(Pose3(R,t), K);
// StereoPoint2 z(184.812, 129.068, 714.768); StereoPoint2 z(184.812, 129.068, 714.768);
//
// Point3 l = camera.backproject(z); Point3 l = camera.backproject(z);
// StereoPoint2 actual = camera.project(l); StereoPoint2 actual = camera.project(l);
// CHECK(assert_equal(z, actual, 1e-8)); z.print("z");
//} actual.print("actual");
CHECK(assert_equal(z, actual, 1e-3));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}