Go to the documentation of this file.
22 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
23 #include <boost/serialization/nvp.hpp>
33 template <
class CALIBRATION>
37 std::shared_ptr<CALIBRATION>
k_;
61 inline const std::shared_ptr<CALIBRATION>&
calibration()
const {
return k_; }
64 void print(
const std::string&
s =
"")
const {
66 k_.print(
"calibration");
77 double theta = pw(3), phi = pw(4);
79 return ray_base +
m/rho;
91 OptionalJacobian<2,1> H3 = {})
const {
94 double theta = pw(3), phi = pw(4);
100 if (!H1 && !H2 && !H3) {
111 double cos_theta =
cos(theta);
112 double sin_theta =
sin(theta);
113 double cos_phi =
cos(phi);
114 double sin_phi =
sin(phi);
115 double rho2 = rho * rho;
121 double H14 = -cos_phi*sin_theta/rho;
122 double H15 = -cos_theta*sin_phi/rho;
127 double H24 = cos_phi*cos_theta/rho;
128 double H25 = -sin_phi*sin_theta/rho;
134 double H35 = cos_phi/rho;
136 *H2 = J2 * (
Matrix(3, 5) <<
137 H11, H12, H13, H14, H15,
138 H21, H22, H23, H24, H25,
139 H31, H32, H33, H34, H35).finished();
142 double H16 = -cos_phi*cos_theta/rho2;
143 double H26 = -cos_phi*sin_theta/rho2;
144 double H36 = -sin_phi/rho2;
145 *H3 = J2 * (
Matrix(3, 1) <<
166 double theta =
atan2(ray.y(), ray.x());
167 double phi =
atan2(ray.z(),
sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
168 return std::make_pair((Vector5() <<
pt.x(),
pt.y(),
pt.z(), theta, phi).finished(),
178 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
180 friend class boost::serialization::access;
181 template<
class Archive>
182 void serialize(Archive & ar,
const unsigned int ) {
183 ar & BOOST_SERIALIZATION_NVP(
pose_);
184 ar & BOOST_SERIALIZATION_NVP(
k_);
typedef and functions to augment Eigen's VectorXd
Jet< T, N > sin(const Jet< T, N > &f)
typedef and functions to augment Eigen's MatrixXd
std::shared_ptr< CALIBRATION > k_
The fixed camera calibration.
static const Point3 pt(1.0, 2.0, 3.0)
Jet< T, N > cos(const Jet< T, N > &f)
void print(const std::string &s="") const
print with optional string
Pose3 & pose()
return pose
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
static Point3 landmark(0, 0, 5)
Base class for all pinhole cameras.
Pose3 pose_
The camera pose.
int RealScalar int RealScalar int RealScalar * pc
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
Non-linear factor base classes.
gtsam::Point2 project(const Vector5 &pw, double rho, OptionalJacobian< 2, 6 > H1={}, OptionalJacobian< 2, 5 > H2={}, OptionalJacobian< 2, 1 > H3={}) const
void print(const std::string &s="") const
print
std::pair< Vector5, double > backproject(const gtsam::Point2 &pi, const double depth) const
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in Pose coordinates and transforms it to world coordinates
static gtsam::Point3 invDepthTo3D(const Vector5 &pw, double rho)
static const CalibratedCamera camera(kDefaultPose)
const std::shared_ptr< CALIBRATION > & calibration() const
return calibration
InvDepthCamera3(const Pose3 &pose, const std::shared_ptr< CALIBRATION > &k)
Jet< T, N > sqrt(const Jet< T, N > &f)
3D Pose manifold SO(3) x R^3 and group SE(3)
virtual ~InvDepthCamera3()
gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:42