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

Make Cal3_S2Stereo its own class, not derived

release/4.3a0
Frank Dellaert 2012-06-04 21:39:29 +00:00
parent cc2351da68
commit a17c3c8df1
2 changed files with 33 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -26,8 +26,10 @@ namespace gtsam {
* @ingroup geometry * @ingroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class Cal3_S2Stereo: public Cal3_S2 { class Cal3_S2Stereo {
private: private:
Cal3_S2 K_;
double b_; double b_;
public: public:
@ -39,12 +41,12 @@ namespace gtsam {
/// default calibration leaves coordinates unchanged /// default calibration leaves coordinates unchanged
Cal3_S2Stereo() : Cal3_S2Stereo() :
Cal3_S2(1, 1, 0, 0, 0), b_(1.0) { K_(1, 1, 0, 0, 0), b_(1.0) {
} }
/// constructor from doubles /// constructor from doubles
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) : Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) :
Cal3_S2(fx, fy, s, u0, v0), b_(b) { K_(fx, fy, s, u0, v0), b_(b) {
} }
/// @} /// @}
@ -52,23 +54,41 @@ namespace gtsam {
/// @{ /// @{
void print(const std::string& s = "") const { void print(const std::string& s = "") const {
gtsam::print(matrix(), s); K_.print(s+"K: ");
std::cout << "Baseline: " << b_ << std::endl; std::cout << s << "Baseline: " << b_ << std::endl;
} }
/// @} /// Check if equal up to specified tolerance
bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const {
if (fabs(b_ - other.b_) > tol) return false;
return K_.equals(other.K_,tol);
}
/// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
//TODO: remove? /// focal length x
// /** inline double fx() const { return K_.fx();}
// * Check if equal up to specified tolerance
// */
// bool equals(const Cal3_S2& K, double tol = 10e-9) const;
/// focal length x
inline double fy() const { return K_.fy();}
/// skew
inline double skew() const { return K_.skew();}
///TODO: comment /// image center in x
inline double px() const { return K_.px();}
/// image center in y
inline double py() const { return K_.py();}
/// return the principal point
Point2 principalPoint() const {
return K_.principalPoint();
}
/// return baseline
inline double baseline() const { return b_; } inline double baseline() const { return b_; }
/// @} /// @}
@ -81,10 +101,8 @@ namespace gtsam {
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) void serialize(Archive & ar, const unsigned int version)
{ {
ar & boost::serialization::make_nvp("Cal3_S2Stereo", ar & BOOST_SERIALIZATION_NVP(K_);
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(b_); ar & BOOST_SERIALIZATION_NVP(b_);
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Cal3_S2);
} }
/// @} /// @}

10
gtsam/geometry/StereoCamera.h
View File

@ -71,16 +71,6 @@ public:
return project(point, H1, H2); return project(point, H1, H2);
} }
/*
* backproject using left camera calibration, up to scale only
* i.e. does not rely on baseline
*/
Point3 backproject(const Point2& projection, const double scale) const {
Point2 intrinsic = K_->calibrate(projection);
Point3 cameraPoint = Point3(intrinsic.x() * scale, intrinsic.y() * scale, scale);;
return pose().transform_from(cameraPoint);
}
Point3 backproject(const StereoPoint2& z) const { Point3 backproject(const StereoPoint2& z) const {
Vector measured = z.vector(); Vector measured = z.vector();
double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]); double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]);