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

Cleaned up, made default constructor a working StereoCamera

release/4.3a0
dellaert 2015-02-19 23:44:56 +01:00
parent b5d9ed4529
commit 03689e418e
1 changed files with 31 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
gtsam/geometry/StereoCamera.h
View File

@ -28,10 +28,11 @@ namespace gtsam {
class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error {
public:
StereoCheiralityException() : std::runtime_error("Stereo Cheirality Exception") {}
StereoCheiralityException() :
std::runtime_error("Stereo Cheirality Exception") {
}
};
/**
* A stereo camera class, parameterize by left camera pose and stereo calibration
* @addtogroup geometry
@ -52,16 +53,22 @@ private:
public:
enum { dimension = 6 };
enum {
dimension = 6
};
/// @name Standard Constructors
/// @{
StereoCamera() {
/// Default constructor allocates a calibration!
StereoCamera() :
K_(new Cal3_S2Stereo()) {
}
/// Construct from pose and shared calibration
StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
/// Return shared pointer to calibration
const Cal3_S2Stereo::shared_ptr calibration() const {
return K_;
}
@ -70,26 +77,28 @@ public:
/// @name Testable
/// @{
/// print
void print(const std::string& s = "") const {
leftCamPose_.print(s + ".camera.");
K_->print(s + ".calibration.");
}
/// equals
bool equals(const StereoCamera &camera, double tol = 1e-9) const {
return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals(
*camera.K_, tol);
return leftCamPose_.equals(camera.leftCamPose_, tol)
&& K_->equals(*camera.K_, tol);
}
/// @}
/// @name Manifold
/// @{
/** Dimensionality of the tangent space */
/// Dimensionality of the tangent space
inline size_t dim() const {
return 6;
}
/** Dimensionality of the tangent space */
/// Dimensionality of the tangent space
static inline size_t Dim() {
return 6;
}
@ -108,10 +117,12 @@ public:
/// @name Standard Interface
/// @{
/// pose
const Pose3& pose() const {
return leftCamPose_;
}
/// baseline
double baseline() const {
return K_->baseline();
}
@ -122,19 +133,16 @@ public:
* @param H2 derivative with respect to point
* @param H3 IGNORED (for calibration)
*/
StereoPoint2 project(const Point3& point,
OptionalJacobian<3, 6> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none,
StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1 =
boost::none, OptionalJacobian<3, 3> H2 = boost::none,
OptionalJacobian<3, 0> H3 = boost::none) const;
/**
*
*/
/// back-project a measurement
Point3 backproject(const StereoPoint2& z) const {
Vector measured = z.vector();
double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]);
double X = Z *(measured[0]- K_->px()) / K_->fx();
double Y = Z *(measured[2]- K_->py()) / K_->fy();
double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]);
double X = Z * (measured[0] - K_->px()) / K_->fx();
double Y = Z * (measured[2] - K_->py()) / K_->fy();
Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
return world_point;
}
@ -142,7 +150,8 @@ public:
/// @}
private:
/** utility functions */
/// utility function
Matrix3 Dproject_to_stereo_camera1(const Point3& P) const;
friend class boost::serialization::access;
@ -155,8 +164,10 @@ private:
};
template<>
struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {};
struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {
};
template<>
struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {};
struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {
};
}