12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add camera Jacobian

release/4.3a0
Fan Jiang 2022-04-15 13:57:01 -04:00
parent 415e41b0f5
commit 939416c3d0
2 changed files with 15 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/PinholePose.h
View File

@ -121,6 +121,19 @@ public:
return _project(pw, Dpose, Dpoint, Dcal);
}
/// project, but for Python use
Point2 project(const Point3& pw, Eigen::Ref<Matrix> Dpose, Eigen::Ref<Matrix> Dpoint, Eigen::Ref<Matrix> Dcal) const {
Eigen::Matrix<double, 2, 6> Dpose_;
Eigen::Matrix<double, 2, 3> Dpoint_;
Eigen::Matrix<double, 2, DimK> Dcal_;
auto ret = _project(pw, Dpose_, Dpoint_, Dcal_);
Dpose = Dpose_;
Dpoint = Dpoint_;
Dcal = Dcal_;
return ret;
}
/// project a 3D point from world coordinates into the image
Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none,

2
gtsam/geometry/geometry.i
View File

@ -824,6 +824,7 @@ template <CALIBRATION>
class PinholeCamera {
// Standard Constructors and Named Constructors
PinholeCamera();
PinholeCamera(const gtsam::PinholeCamera<CALIBRATION> other);
PinholeCamera(const gtsam::Pose3& pose);
PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K);
static This Level(const CALIBRATION& K, const gtsam::Pose2& pose,
@ -850,6 +851,7 @@ class PinholeCamera {
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point2 project(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dpose, Eigen::Ref<Eigen::MatrixXd> Dpoint, Eigen::Ref<Eigen::MatrixXd> Dcal);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
double range(const gtsam::Point3& point);
double range(const gtsam::Pose3& pose);