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
* \nosubgrouping
*/
class Cal3_S2Stereo: public Cal3_S2 {
class Cal3_S2Stereo {
private:
Cal3_S2 K_;
double b_;
public:
@ -39,12 +41,12 @@ namespace gtsam {
/// default calibration leaves coordinates unchanged
Cal3_S2Stereo() :
Cal3_S2(1, 1, 0, 0, 0), b_(1.0) {
K_(1, 1, 0, 0, 0), b_(1.0) {
}
/// constructor from doubles
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 {
gtsam::print(matrix(), s);
std::cout << "Baseline: " << b_ << std::endl;
K_.print(s+"K: ");
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
/// @{
//TODO: remove?
// /**
// * Check if equal up to specified tolerance
// */
// bool equals(const Cal3_S2& K, double tol = 10e-9) const;
/// focal length x
inline double fx() const { return K_.fx();}
/// 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_; }
/// @}
@ -81,10 +101,8 @@ namespace gtsam {
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Cal3_S2Stereo",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(K_);
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);
}
/*
* 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 {
Vector measured = z.vector();
double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]);