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 X = Z *(measured[0]- K_.px()) / K_.fx();
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 */

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

@ -109,20 +109,22 @@ TEST( StereoCamera, backproject)
}
/* ************************************************************************* */
//TEST( StereoCamera, backproject2)
//{
// 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 K(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.backproject(z);
// StereoPoint2 actual = camera.project(l);
// CHECK(assert_equal(z, actual, 1e-8));
//}
TEST( StereoCamera, backproject2)
{
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 K(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.backproject(z);
StereoPoint2 actual = camera.project(l);
z.print("z");
actual.print("actual");
CHECK(assert_equal(z, actual, 1e-3));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}