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

add backproject to StereoCamera

release/4.3a0
Chris Beall 2011-03-29 01:54:59 +00:00
parent a581d03022
commit b6783e8b89
2 changed files with 22 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/geometry/StereoCamera.h
View File

@ -80,6 +80,20 @@ namespace gtsam {
return pose().transform_from(cameraPoint);
}
Point3 backproject(const StereoPoint2& z) {
Vector measured = z.vector();
// std::cout << K_.baseline() << std::endl;
// std::cout << K_.fx() << std::endl;
// std:: cout << measured[0] << std::endl;
// std:: cout << measured[1] << std::endl;
double Z = K_.baseline()*K_.fx()/(measured[0]-measured[1]);
std::cout << "Z " << Z;
double X = Z *(measured[0]- K_.px()) / K_.fx();
double Y = Z *(measured[2]- K_.py()) / K_.fy();
return Point3(X, Y, Z);
}
/** Dimensionality of the tangent space */
inline size_t dim() const {
return 6;

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

@ -100,6 +100,14 @@ TEST( StereoCamera, Dproject_stereo_point)
CHECK(assert_equal(expected,actual,1e-8));
}
TEST( StereoCamera, backproject)
{
Point3 expected(1.2, 2.3, 4.5);
StereoPoint2 stereo_point = stereoCam.project(expected);
Point3 actual = stereoCam.backproject(stereo_point);
CHECK(assert_equal(expected,actual,1e-8));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */