diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 579b07233..ae3a53a69 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -82,13 +82,7 @@ namespace gtsam { 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);