24 #include <gtsam/base/concepts.h> 26 #include <gtsam/base/ThreadsafeException.h> 27 #include <gtsam/dllexport.h> 28 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 29 #include <boost/serialization/nvp.hpp> 43 Key nearbyVariable()
const {
return j_;}
60 typedef Point3 Translation;
67 typedef Point2Vector MeasurementVector;
83 static Matrix26 Dpose(
const Point2& pn,
double d);
91 static Matrix23 Dpoint(
const Point2& pn,
double d,
const Matrix3& Rt);
107 static Pose3 LevelPose(
const Pose2& pose2,
double height);
147 virtual void print(
const std::string& s =
"PinholeBase")
const;
192 std::pair<Point2, bool> projectSafe(
const Point3& pw)
const;
212 static Point3 BackprojectFromCamera(
const Point2& p,
const double depth,
233 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 235 friend class boost::serialization::access;
236 template<
class Archive>
237 void serialize(Archive & ar,
const unsigned int ) {
238 ar & BOOST_SERIALIZATION_NVP(pose_);
322 void print(
const std::string& s =
"CalibratedCamera")
const override {
327 inline size_t dim()
const {
332 inline static size_t Dim() {
354 Matrix31 Dpoint_ddepth;
355 const Point3 point = BackprojectFromCamera(pn, depth,
356 Dresult_dp ? &Dpoint_dpn : 0,
357 Dresult_ddepth ? &Dpoint_ddepth : 0);
359 Matrix33 Dresult_dpoint;
362 Dresult_dp) ? &Dresult_dpoint : 0);
365 *Dresult_dp = Dresult_dpoint * Dpoint_dpn;
367 *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth;
380 return pose().
range(point, Dcamera, Dpoint);
390 return this->pose().
range(pose, Dcamera, Dpose);
401 return pose().
range(camera.
pose(), H1, H2);
411 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 413 friend class boost::serialization::access;
414 template<
class Archive>
415 void serialize(Archive & ar,
const unsigned int ) {
417 & boost::serialization::make_nvp(
"PinholeBase",
418 boost::serialization::base_object<PinholeBase>(*
this));
433 template <
typename T>
434 struct Range<CalibratedCamera, T> :
HasRange<CalibratedCamera, T, double> {};
PinholeBase(const Pose3 &pose)
Constructor with pose.
Definition: CalibratedCamera.h:128
double range(const CalibratedCamera &camera, OptionalJacobian< 1, 6 > H1={}, OptionalJacobian< 1, 6 > H2={}) const
Definition: CalibratedCamera.h:398
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself={}, OptionalJacobian< 1, 3 > Hpoint={}) const
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition: expressions.h:131
Definition: CalibratedCamera.h:251
Point2 Measurement
Definition: CalibratedCamera.h:66
static std::pair< size_t, size_t > translationInterval()
Definition: CalibratedCamera.h:225
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
Definition: BearingRange.h:197
Vector2 Point2
Definition: Point2.h:32
PinholeBase()
Default constructor.
Definition: CalibratedCamera.h:125
const Point3 & translation() const
get translation
Definition: CalibratedCamera.h:164
double range(const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 6 > Dpose={}) const
Definition: CalibratedCamera.h:388
Definition: CalibratedCamera.h:54
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
size_t dim() const
Definition: CalibratedCamera.h:327
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
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
Definition: Testable.h:112
void print(const std::string &s="CalibratedCamera") const override
print
Definition: CalibratedCamera.h:322
CalibratedCamera(const Pose3 &pose)
construct with pose
Definition: CalibratedCamera.h:267
static Pose3 Expmap(const Vector6 &xi, OptionalJacobian< 6, 6 > Hxi={})
Exponential map at identity - create a rotation from canonical coordinates .
const Pose3 & pose() const
return pose, constant version
Definition: CalibratedCamera.h:154
Base class and basic functions for Manifold types.
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
Definition: ThreadsafeException.h:40
GTSAM_EXPORT void print(const Matrix &A, const std::string &s, std::ostream &stream)
CalibratedCamera(const Vector &v)
construct from vector
Definition: CalibratedCamera.h:307
double range(const Point3 &point, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 3 > Dpoint={}) const
Definition: CalibratedCamera.h:377
Definition: BearingRange.h:41
Point3 backproject(const Point2 &pn, double depth, OptionalJacobian< 3, 6 > Dresult_dpose={}, OptionalJacobian< 3, 2 > Dresult_dp={}, OptionalJacobian< 3, 1 > Dresult_ddepth={}) const
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition: CalibratedCamera.h:348
Definition: chartTesting.h:28
Definition: OptionalJacobian.h:38
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
static size_t Dim()
Definition: CalibratedCamera.h:332
CalibratedCamera()
default constructor
Definition: CalibratedCamera.h:263
const Rot3 & rotation() const
get rotation
Definition: CalibratedCamera.h:159
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: CalibratedCamera.h:34
Vector3 Point3
Definition: Point3.h:38
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Rot3 Rotation
Definition: CalibratedCamera.h:59
virtual void print(const std::string &s="PinholeBase") const
print