72 lines
1.5 KiB
C++
72 lines
1.5 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) const;
|
|
|
|
};
|
|
|
|
/** Dimensionality of the tangent space */
|
|
inline size_t dim(const StereoCamera& obj) {
|
|
return 6;
|
|
}
|
|
|
|
/** Exponential map around p0 */
|
|
template<> inline StereoCamera expmap<StereoCamera>(const StereoCamera& p0, const Vector& d) {
|
|
return StereoCamera(expmap(p0.pose(),d),p0.K(),p0.baseline());
|
|
}
|
|
|
|
inline StereoPoint2 project(const StereoCamera& camera, const Point3& point) {
|
|
return camera.project(point);
|
|
}
|
|
|
|
Matrix Dproject_stereo_pose(const StereoCamera& camera, const Point3& point);
|
|
Matrix Dproject_stereo_point(const StereoCamera& camera, const Point3& point);
|
|
|
|
Matrix
|
|
Dproject_to_stereo_camera1(const StereoCamera& camera, const Point3& P);
|
|
Matrix Duncalibrate2(const Cal3_S2& K);
|
|
|
|
}
|