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