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

cleaned up

release/4.3a0
Chris Beall 2011-03-29 02:02:57 +00:00
parent b6783e8b89
commit 78a8b86d1e
1 changed files with 0 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/StereoCamera.h
View File

@ -82,13 +82,7 @@ namespace gtsam {
Point3 backproject(const StereoPoint2& z) { Point3 backproject(const StereoPoint2& z) {
Vector measured = z.vector(); 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]); double Z = K_.baseline()*K_.fx()/(measured[0]-measured[1]);
std::cout << "Z " << Z;
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); return Point3(X, Y, Z);