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

added Cheirality exception as in ProjectionFactor

release/4.3a0
Chris Beall 2012-09-27 19:06:30 +00:00
parent c323f41e8f
commit f6ef1e1d9d
3 changed files with 20 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/StereoCamera.cpp
View File

@ -39,6 +39,8 @@ namespace gtsam {
const Point3 q = leftCamPose_.transform_to(point); const Point3 q = leftCamPose_.transform_to(point);
#endif #endif
if ( q.z() <= 0 ) throw StereoCheiralityException();
// get calibration // get calibration
const Cal3_S2Stereo& K = *K_; const Cal3_S2Stereo& K = *K_;
const double fx = K.fx(), fy = K.fy(), b = K.baseline(); const double fx = K.fx(), fy = K.fy(), b = K.baseline();

8
gtsam/geometry/StereoCamera.h
View File

@ -11,7 +11,7 @@
/** /**
* @file StereoCamera.h * @file StereoCamera.h
* @brief A Stereo Camera based on two Simple Cameras * @brief A Rectified Stereo Camera
* @author Chris Beall * @author Chris Beall
*/ */
@ -26,6 +26,12 @@
namespace gtsam { namespace gtsam {
class StereoCheiralityException: public std::runtime_error {
public:
StereoCheiralityException() : std::runtime_error("Stereo Cheirality Exception") {}
};
/** /**
* A stereo camera class, parameterize by left camera pose and stereo calibration * A stereo camera class, parameterize by left camera pose and stereo calibration
* @addtogroup geometry * @addtogroup geometry

16
gtsam/slam/StereoFactor.h
View File

@ -88,8 +88,16 @@ public:
/** h(x)-z */ /** h(x)-z */
Vector evaluateError(const Pose3& pose, const Point3& point, Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
StereoCamera stereoCam(pose, K_); try {
return (stereoCam.project(point, H1, H2) - measured_).vector(); StereoCamera stereoCam(pose, K_);
return (stereoCam.project(point, H1, H2) - measured_).vector();
} catch(StereoCheiralityException& e) {
if (H1) *H1 = zeros(3,6);
if (H2) *H2 = zeros(3,3);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
}
return ones(3) * 2.0 * K_->fx();
} }
/** return the measured */ /** return the measured */
@ -102,7 +110,6 @@ public:
return K_; return K_;
} }
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
@ -115,5 +122,4 @@ private:
} }
}; };
} // \ namespace gtsam
}