68 lines
1.3 KiB
C++
68 lines
1.3 KiB
C++
/**
|
|
* @file StereoCamera.h
|
|
* @brief A Stereo Camera based on two Simple Cameras
|
|
* @author Chris Beall
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "boost/tuple/tuple.hpp"
|
|
#include <gtsam/geometry/Cal3_S2.h>
|
|
#include <gtsam/geometry/Pose3.h>
|
|
#include <gtsam/base/Lie.h>
|
|
#include <gtsam/geometry/StereoPoint2.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* A stereo camera class
|
|
*/
|
|
class StereoCamera {
|
|
|
|
private:
|
|
Pose3 leftCamPose_;
|
|
Cal3_S2 K_;
|
|
double baseline_;
|
|
double fx_, fy_;
|
|
double cx_, cy_;
|
|
public:
|
|
StereoCamera() {
|
|
}
|
|
|
|
StereoCamera(const Pose3& leftCamPose, const Cal3_S2& K, double baseline);
|
|
|
|
const Cal3_S2& K() const {
|
|
return K_;
|
|
}
|
|
|
|
const Pose3& pose() const {
|
|
return leftCamPose_;
|
|
}
|
|
|
|
const double baseline() const {
|
|
return baseline_;
|
|
}
|
|
|
|
StereoPoint2 project(const Point3& point,
|
|
boost::optional<Matrix&> Dproject_stereo_pose = boost::none,
|
|
boost::optional<Matrix&> Dproject_stereo_point = boost::none) const;
|
|
|
|
/** Dimensionality of the tangent space */
|
|
inline size_t dim() const {
|
|
return 6;
|
|
}
|
|
|
|
/** Exponential map around p0 */
|
|
inline StereoCamera expmap(const Vector& d) const {
|
|
return StereoCamera(pose().expmap(d),K(),baseline());
|
|
}
|
|
|
|
private:
|
|
/** utility functions */
|
|
Matrix Dproject_to_stereo_camera1(const Point3& P) const;
|
|
static Matrix Duncalibrate2(const Cal3_S2& K);
|
|
|
|
};
|
|
|
|
}
|