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

address some comments

release/4.3a0
Joel Truher 2024-12-17 22:39:13 -08:00
parent 11f7eb6422
commit e4538d5b3e
5 changed files with 132 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/geometry/Pose3.cpp
View File

@ -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();

2
gtsam/geometry/Pose3.h
View File

@ -78,6 +78,8 @@ public:
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

168
gtsam/slam/PlanarProjectionFactor.h
View File

@ -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
@ -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
};
@ -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() {}
@ -204,43 +204,44 @@ namespace gtsam {
}
/**
* @param measured corresponding point in the camera frame
* @param offset constant camera offset from pose
* @param calib constant camera calibration
* @param model stddev of the measurements, ~one pixel?
* @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 bTc "body to camera": constant camera offset from pose
* @param calib constant camera calibration
* @param model stddev of the measurements, ~one pixel?
*/
PlanarProjectionFactor2(
const Point2& measured,
const Pose3& offset,
const Cal3DS2& calib,
const SharedNoiseModel& model,
Key poseKey,
Key landmarkKey,
Key poseKey)
const Point2& measured,
const Pose3& bTc,
const Cal3DS2& calib,
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
};
@ -270,6 +271,7 @@ 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?
@ -277,12 +279,12 @@ namespace gtsam {
* @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) {
@ -290,21 +292,21 @@ namespace gtsam {
}
/**
* @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:

36
gtsam/slam/slam.i
View File

@ -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;
};

24
gtsam/slam/tests/testPlanarProjectionFactor.cpp
View File

@ -39,7 +39,8 @@ TEST(PlanarProjectionFactor1, error1) {
Pose2 pose(0, 0, 0);
values.insert(X(0), pose);
PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
PlanarProjectionFactor1 factor(
X(0), landmark, measured, offset, calib, model);
CHECK_EQUAL(2, factor.dim());
CHECK(factor.active(values));
@ -65,7 +66,8 @@ TEST(PlanarProjectionFactor1, error2) {
Pose3 offset;
Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
PlanarProjectionFactor1 factor(
X(0), landmark, measured, offset, calib, model);
Values values;
Pose2 pose(0, 0, 0);
@ -96,7 +98,8 @@ TEST(PlanarProjectionFactor1, error3) {
// distortion
Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
PlanarProjectionFactor1 factor(
X(0), landmark, measured, offset, calib, model);
Values values;
Pose2 pose(0, 0, 0);
@ -131,7 +134,8 @@ TEST(PlanarProjectionFactor1, jacobian) {
Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
PlanarProjectionFactor1 factor(
X(0), landmark, measured, offset, calib, model);
Pose2 pose(s(g), s(g), s(g));
@ -164,7 +168,8 @@ TEST(PlanarProjectionFactor3, error1) {
values.insert(C(0), offset);
values.insert(K(0), calib);
PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
PlanarProjectionFactor3 factor(
X(0), C(0), K(0), landmark, measured, model);
CHECK_EQUAL(2, factor.dim());
CHECK(factor.active(values));
@ -213,7 +218,8 @@ TEST(PlanarProjectionFactor3, error2) {
Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
PlanarProjectionFactor3 factor(
X(0), C(0), K(0), landmark, measured, model);
Values values;
Pose2 pose(0, 0, 0);
@ -267,7 +273,8 @@ TEST(PlanarProjectionFactor3, error3) {
Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
PlanarProjectionFactor3 factor(
X(0), C(0), K(0), landmark, measured, model);
Values values;
Pose2 pose(0, 0, 0);
@ -328,7 +335,8 @@ TEST(PlanarProjectionFactor3, jacobian) {
Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
PlanarProjectionFactor3 factor(
X(0), C(0), K(0), landmark, measured, model);
Pose2 pose(s(g), s(g), s(g));