diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 01c21e6a0..679a6548d 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -110,11 +110,10 @@ public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - /// shorthand for a pinhole camera - // TODO: Switch to stereoCamera: + /// shorthand for a StereoCamera // TODO: Get rid of this? typedef StereoCamera Camera; -// typedef StereoCamera Camera; + /// Vector of cameras typedef std::vector Cameras; /** @@ -278,6 +277,7 @@ public: } i += 1; } + std::cout << "totalReprojError error: " << totalReprojError << std::endl; // we discard smart factors that have large reprojection error if(dynamicOutlierRejectionThreshold_ > 0 && totalReprojError/m > dynamicOutlierRejectionThreshold_) @@ -612,7 +612,7 @@ public: if (nonDegenerate) return reprojectionError(myCameras); else - return zero(myCameras.size() * 2); + return zero(myCameras.size() * 3); } /**