Skip to content

Commit

Permalink
address some comments
Browse files Browse the repository at this point in the history
  • Loading branch information
@truher
truher committed Dec 18, 2024
1 parent 11f7eb6 commit e4538d5
Show file tree
Hide file tree
Showing 5 changed files with 130 additions and 107 deletions.
11 changes: 11 additions & 0 deletions gtsam/geometry/Pose3.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,17 @@ Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR,
return Pose3(R, t);
}

Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) {
if (H) *H << (gtsam::Matrix(6, 3) << //
0., 0., 0., //
0., 0., 0.,//
0., 0., 1.,//
1., 0., 0.,//
0., 1., 0.,//
0., 0., 0.).finished();
return Pose3(p);
}

/* ************************************************************************* */
Pose3 Pose3::inverse() const {
Rot3 Rt = R_.inverse();
Expand Down
2 changes: 2 additions & 0 deletions gtsam/geometry/Pose3.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,8 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
OptionalJacobian<6, 3> HR = {},
OptionalJacobian<6, 3> Ht = {});

static Pose3 FromPose2(const Pose2& p, OptionalJacobian<6,3> H = {});

/**
* Create Pose3 by aligning two point pairs
* A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point
Expand Down
164 changes: 83 additions & 81 deletions gtsam/slam/PlanarProjectionFactor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,69 +38,67 @@ namespace gtsam {
PlanarProjectionFactorBase() {}

/**
* @param measured corresponding point in the camera frame
* @param poseKey index of the robot pose in the z=0 plane
* @param measured pixels in the camera frame
*/
PlanarProjectionFactorBase(const Point2& measured) : measured_(measured) {}

/**
* @param landmark point3
* @param pose
* @param offset oriented parallel to pose2 zero i.e. +x
* @param calib
* @param H1 landmark jacobian
* @param H2 pose jacobian
* @param H3 offset jacobian
* @param H4 calib jacobian
* Predict the projection of the landmark in camera pixels.
*
* @param landmark point3 of the target
* @param wTb "world to body": planar pose2 of vehicle body frame in world frame
* @param bTc "body to camera": camera pose in body frame, oriented parallel to pose2 zero i.e. +x
* @param calib camera calibration with distortion
* @param Hlandmark jacobian
* @param HwTb jacobian
* @param HbTc jacobian
* @param Hcalib jacobian
*/
Point2 h(
Point2 predict(
const Point3& landmark,
const Pose2& pose,
const Pose3& offset,
const Pose2& wTb,
const Pose3& bTc,
const Cal3DS2& calib,
OptionalMatrixType H1, // 2x3 (x, y, z)
OptionalMatrixType H2, // 2x3 (x, y, theta)
OptionalMatrixType H3, // 2x6 (rx, ry, rz, x, y, theta)
OptionalMatrixType H4 // 2x9
OptionalMatrixType Hlandmark = nullptr, // 2x3 (x, y, z)
OptionalMatrixType HwTb = nullptr, // 2x3 (x, y, theta)
OptionalMatrixType HbTc = nullptr, // 2x6 (rx, ry, rz, x, y, theta)
OptionalMatrixType Hcalib = nullptr // 2x9
) const {
#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
try {
// this is x-forward z-up
#endif
gtsam::Matrix Hp; // 6x3
gtsam::Matrix H0; // 6x6
Pose3 offset_pose = Pose3(pose).compose(offset, H0);
// this is x-forward z-up
Pose3 wTc = Pose3::FromPose2(wTb, Hp).compose(bTc, HwTb ? &H0 : nullptr);
// this is z-forward y-down
gtsam::Matrix H00; // 6x6
Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
Pose3 camera_pose = wTc.compose(CAM_COORD, H00);
PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib);
if (H2 || H3) {
if (HwTb || HbTc) {
// Dpose is for pose3, 2x6 (R,t)
gtsam::Matrix Dpose;
Point2 result = camera.project(landmark, Dpose, H1, H4);
Point2 result = camera.project(landmark, Dpose, Hlandmark, Hcalib);
gtsam::Matrix DposeOffset = Dpose * H00; // 2x6
if (H3)
*H3 = DposeOffset; // with Eigen this is a deep copy (!)
if (H2) {
gtsam::Matrix DposeOffsetFwd = DposeOffset * H0;
*H2 = Matrix::Zero(2, 3);
(*H2)(0, 0) = DposeOffsetFwd(0, 3); // du/dx
(*H2)(1, 0) = DposeOffsetFwd(1, 3); // dv/dx
(*H2)(0, 1) = DposeOffsetFwd(0, 4); // du/dy
(*H2)(1, 1) = DposeOffsetFwd(1, 4); // dv/dy
(*H2)(0, 2) = DposeOffsetFwd(0, 2); // du/dyaw
(*H2)(1, 2) = DposeOffsetFwd(1, 2); // dv/dyaw
}
if (HbTc)
*HbTc = DposeOffset; // with Eigen this is a deep copy (!)
if (HwTb)
*HwTb = DposeOffset * H0 * Hp;
return result;
} else {
return camera.project(landmark, {}, {}, {});
}
#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
} catch (CheiralityException& e) {
std::cout << "****** CHIRALITY EXCEPTION ******\n";
if (H1) *H1 = Matrix::Zero(2, 3);
if (H2) *H2 = Matrix::Zero(2, 3);
if (H3) *H3 = Matrix::Zero(2, 6);
if (H4) *H4 = Matrix::Zero(2, 9);
if (Hlandmark) *Hlandmark = Matrix::Zero(2, 3);
if (HwTb) *HwTb = Matrix::Zero(2, 3);
if (HbTc) *HbTc = Matrix::Zero(2, 6);
if (Hcalib) *Hcalib = Matrix::Zero(2, 9);
// return a large error
return Matrix::Constant(2, 1, 2.0 * calib.fx());
}
#endif
}

Point2 measured_; // pixel measurement
Expand Down Expand Up @@ -140,41 +138,42 @@ namespace gtsam {


/**
* @brief constructor for known landmark, offset, and calibration
* @param poseKey index of the robot pose2 in the z=0 plane
* @param landmark point3 in the world
* @param measured corresponding point2 in the camera frame
* @param offset constant camera offset from pose
* @param bTc "body to camera": constant camera offset from pose
* @param calib constant camera calibration
* @param model stddev of the measurements, ~one pixel?
* @param poseKey index of the robot pose2 in the z=0 plane
*/
PlanarProjectionFactor1(
Key poseKey,
const Point3& landmark,
const Point2& measured,
const Pose3& offset,
const Pose3& bTc,
const Cal3DS2& calib,
const SharedNoiseModel& model,
Key poseKey)
const SharedNoiseModel& model)
: PlanarProjectionFactorBase(measured),
NoiseModelFactorN(model, poseKey),
landmark_(landmark),
offset_(offset),
bTc_(bTc),
calib_(calib) {
assert(2 == model->dim());
}

/**
* @param pose estimated pose2
* @param H1 pose jacobian
* @param wTb "world to body": estimated pose2
* @param HwTb jacobian
*/
Vector evaluateError(
const Pose2& pose,
OptionalMatrixType H1 = OptionalNone) const override {
return h(landmark_, pose, offset_, calib_, {}, H1, {}, {}) - measured_;
const Pose2& wTb,
OptionalMatrixType HwTb = OptionalNone) const override {
return predict(landmark_, wTb, bTc_, calib_, {}, HwTb, {}, {}) - measured_;
}

private:
Point3 landmark_; // landmark
Pose3 offset_; // camera offset to robot pose
Pose3 bTc_; // "body to camera": camera offset to robot pose
Cal3DS2 calib_; // camera calibration
};

Expand All @@ -188,9 +187,10 @@ namespace gtsam {
* Camera offset and calibration are constant.
* This is similar to GeneralSFMFactor, used for SLAM.
*/
class PlanarProjectionFactor2 : public PlanarProjectionFactorBase, public NoiseModelFactorN<Point3, Pose2> {
class PlanarProjectionFactor2
: public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Point3> {
public:
typedef NoiseModelFactorN<Point3, Pose2> Base;
typedef NoiseModelFactorN<Pose2, Point3> Base;
using Base::evaluateError;

PlanarProjectionFactor2() {}
Expand All @@ -204,43 +204,44 @@ namespace gtsam {
}

/**
* @brief constructor for variable landmark, known offset and calibration
* @param poseKey index of the robot pose2 in the z=0 plane
* @param landmarkKey index of the landmark point3
* @param measured corresponding point in the camera frame
* @param offset constant camera offset from pose
* @param bTc "body to camera": constant camera offset from pose
* @param calib constant camera calibration
* @param model stddev of the measurements, ~one pixel?
* @param poseKey index of the robot pose2 in the z=0 plane
* @param landmarkKey index of the landmark point3
*/
PlanarProjectionFactor2(
Key poseKey,
Key landmarkKey,
const Point2& measured,
const Pose3& offset,
const Pose3& bTc,
const Cal3DS2& calib,
const SharedNoiseModel& model,
Key landmarkKey,
Key poseKey)
const SharedNoiseModel& model)
: PlanarProjectionFactorBase(measured),
NoiseModelFactorN(model, landmarkKey, poseKey),
offset_(offset),
bTc_(bTc),
calib_(calib) {
assert(2 == model->dim());
}

/**
* @param wTb "world to body": estimated pose2
* @param landmark estimated landmark
* @param pose estimated pose2
* @param H1 landmark jacobian
* @param H2 pose jacobian
* @param HwTb jacobian
* @param Hlandmark jacobian
*/
Vector evaluateError(
const Pose2& wTb,
const Point3& landmark,
const Pose2& pose,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
return h(landmark, pose, offset_, calib_, H1, H2, {}, {}) - measured_;
OptionalMatrixType HwTb = OptionalNone,
OptionalMatrixType Hlandmark = OptionalNone) const override {
return predict(landmark, wTb, bTc_, calib_, Hlandmark, HwTb, {}, {}) - measured_;
}

private:
Pose3 offset_; // camera offset to robot pose
Pose3 bTc_; // "body to camera": camera offset to robot pose
Cal3DS2 calib_; // camera calibration
};

Expand Down Expand Up @@ -270,41 +271,42 @@ namespace gtsam {
}

/**
* @brief constructor for variable pose, offset, and calibration, known landmark.
* @param landmark point3 in the world
* @param measured corresponding point2 in the camera frame
* @param model stddev of the measurements, ~one pixel?
* @param poseKey index of the robot pose2 in the z=0 plane
* @param offsetKey index of camera offset from pose
* @param calibKey index of camera calibration */
PlanarProjectionFactor3(
const Point3& landmark,
const Point2& measured,
const SharedNoiseModel& model,
Key poseKey,
Key offsetKey,
Key calibKey)
Key calibKey,
const Point3& landmark,
const Point2& measured,
const SharedNoiseModel& model)
: PlanarProjectionFactorBase(measured),
NoiseModelFactorN(model, poseKey, offsetKey, calibKey),
landmark_(landmark) {
assert(2 == model->dim());
}

/**
* @param pose estimated pose2
* @param offset pose3 offset from pose2 +x
* @param wTb "world to body": estimated pose2
* @param bTc "body to camera": pose3 offset from pose2 +x
* @param calib calibration
* @param H1 pose jacobian
* @param HwTb pose jacobian
* @param H2 offset jacobian
* @param H3 calibration jacobian
*/
Vector evaluateError(
const Pose2& pose,
const Pose3& offset,
const Pose2& wTb,
const Pose3& bTc,
const Cal3DS2& calib,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone) const override {
return h(landmark_, pose, offset, calib, {}, H1, H2, H3) - measured_;
OptionalMatrixType HwTb = OptionalNone,
OptionalMatrixType HbTc = OptionalNone,
OptionalMatrixType Hcalib = OptionalNone) const override {
return predict(landmark_, wTb, bTc, calib, {}, HwTb, HbTc, Hcalib) - measured_;
}

private:
Expand Down
36 changes: 18 additions & 18 deletions gtsam/slam/slam.i
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,32 +27,32 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
#include <gtsam/slam/PlanarProjectionFactor.h>
virtual class PlanarProjectionFactor1 : gtsam::NoiseModelFactor {
PlanarProjectionFactor1(
const gtsam::Point3& landmark,
const gtsam::Point2& measured,
const gtsam::Pose3& offset,
const gtsam::Cal3DS2& calib,
const gtsam::noiseModel::Base* model,
size_t poseKey);
size_t poseKey,
const gtsam::Point3& landmark,
const gtsam::Point2& measured,
const gtsam::Pose3& bTc,
const gtsam::Cal3DS2& calib,
const gtsam::noiseModel::Base* model);
void serialize() const;
};
virtual class PlanarProjectionFactor2 : gtsam::NoiseModelFactor {
PlanarProjectionFactor2(
const gtsam::Point2& measured,
const gtsam::Pose3& offset,
const gtsam::Cal3DS2& calib,
const gtsam::noiseModel::Base* model,
size_t landmarkKey,
size_t poseKey);
size_t poseKey,
size_t landmarkKey,
const gtsam::Point2& measured,
const gtsam::Pose3& bTc,
const gtsam::Cal3DS2& calib,
const gtsam::noiseModel::Base* model);
void serialize() const;
};
virtual class PlanarProjectionFactor3 : gtsam::NoiseModelFactor {
PlanarProjectionFactor3(
const gtsam::Point3& landmark,
const gtsam::Point2& measured,
const gtsam::noiseModel::Base* model,
size_t poseKey,
size_t offsetKey,
size_t calibKey);
size_t poseKey,
size_t offsetKey,
size_t calibKey,
const gtsam::Point3& landmark,
const gtsam::Point2& measured,
const gtsam::noiseModel::Base* model);
void serialize() const;
};

Expand Down
Loading

0 comments on commit e4538d5

Please sign in to comment.