Skip to content

Commit

Permalink
consolidate factors in one file
Browse files Browse the repository at this point in the history
  • Loading branch information
@truher
truher committed Dec 17, 2024
1 parent eec78d3 commit 14c7998
Show file tree
Hide file tree
Showing 5 changed files with 485 additions and 515 deletions.
332 changes: 248 additions & 84 deletions gtsam/slam/PlanarProjectionFactor.h
View file
Original file line number Diff line number Diff line change
@@ -1,24 +1,13 @@
/**
* Similar to GenericProjectionFactor, but:
*
* * 2d pose variable (robot on the floor)
* * constant landmarks
* * batched input
* * numeric differentiation
* ProjectionFactor, but with pose2 (robot on the floor)
*
* This factor is useful for high-school robotics competitions,
* which run robots on the floor, with use fixed maps and fiducial
* markers. The camera offset and calibration are fixed, perhaps
* found using PlanarSFMFactor.
*
* The python version of this factor uses batches, to save on calls
* across the C++/python boundary, but here the only extra cost
* is instantiating the camera, so there's no batch.
* markers.
*
* @see https://www.firstinspires.org/
* @see PlanarSFMFactor.h
*
* @file PlanarSFMFactor.h
* @file PlanarProjectionFactor.h
* @brief for planar smoothing
* @date Dec 2, 2024
* @author joel@truher.org
Expand All @@ -39,116 +28,291 @@


namespace gtsam {

/**
* @class PlanarProjectionFactor
* @brief Camera projection for robot on the floor.
* @class PlanarProjectionFactorBase
* @brief Camera projection for robot on the floor. Measurement is camera pixels.
*/
class PlanarProjectionFactor : public NoiseModelFactorN<Pose2> {
typedef NoiseModelFactorN<Pose2> Base;
class PlanarProjectionFactorBase {
protected:
PlanarProjectionFactorBase() {}

/**
* @param measured corresponding point in the camera frame
* @param poseKey index of the robot pose in the z=0 plane
*/
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
*/
Point2 h(
const Point3& landmark,
const Pose2& pose,
const Pose3& offset,
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
) const {
try {
// this is x-forward z-up
gtsam::Matrix H0; // 6x6
Pose3 offset_pose = Pose3(pose).compose(offset, H0);
// this is z-forward y-down
gtsam::Matrix H00; // 6x6
Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib);
if (H2 || H3) {
// Dpose is for pose3, 2x6 (R,t)
gtsam::Matrix Dpose;
Point2 result = camera.project(landmark, Dpose, H1, H4);
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
}
return result;
} else {
return camera.project(landmark, {}, {}, {});
}
} 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);
// return a large error
return Matrix::Constant(2, 1, 2.0 * calib.fx());
}
}

Point2 measured_; // pixel measurement

private:
static const Pose3 CAM_COORD;
};

protected:
// camera "zero" is facing +z; this turns it to face +x
const Pose3 PlanarProjectionFactorBase::CAM_COORD = Pose3(
Rot3(0, 0, 1,//
-1, 0, 0, //
0, -1, 0),
Vector3(0, 0, 0)
);

/**
* @class PlanarProjectionFactor1
* @brief One variable: the pose.
* Landmark, camera offset, camera calibration are constant.
* This is intended for localization within a known map.
*/
class PlanarProjectionFactor1
: public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2> {
public:
typedef NoiseModelFactorN<Pose2> Base;
using Base::evaluateError;
PlanarProjectionFactor1() {}

~PlanarProjectionFactor1() override {}

/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this)));
}


/**
* @param landmark point3 in the world
* @param measured corresponding point2 in the camera frame
* @param offset 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(
const Point3& landmark,
const Point2& measured,
const Pose3& offset,
const Cal3DS2& calib,
const SharedNoiseModel& model,
Key poseKey)
: PlanarProjectionFactorBase(measured),
NoiseModelFactorN(model, poseKey),
landmark_(landmark),
offset_(offset),
calib_(calib) {
assert(2 == model->dim());
}

/**
* @param pose estimated pose2
* @param H1 pose jacobian
*/
Vector evaluateError(
const Pose2& pose,
OptionalMatrixType H1 = OptionalNone) const override {
return h(landmark_, pose, offset_, calib_, {}, H1, {}, {}) - measured_;
}

private:
Point3 landmark_; // landmark
Point2 measured_; // pixel measurement
Pose3 offset_; // camera offset to robot pose
Cal3DS2 calib_; // camera calibration
};

template<>
struct traits<PlanarProjectionFactor1> :
public Testable<PlanarProjectionFactor1> {};

/**
* @class PlanarProjectionFactor2
* @brief Two unknowns: the pose and the landmark.
* Camera offset and calibration are constant.
* This is similar to GeneralSFMFactor, used for SLAM.
*/
class PlanarProjectionFactor2 : public PlanarProjectionFactorBase, public NoiseModelFactorN<Point3, Pose2> {
public:
// Provide access to the Matrix& version of evaluateError:
typedef NoiseModelFactorN<Point3, Pose2> Base;
using Base::evaluateError;

PlanarProjectionFactor() {}
PlanarProjectionFactor2() {}

~PlanarProjectionFactor2() override {}

/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this)));
}

/**
* @param landmarks point in the world
* @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?
* @param poseKey index of the robot pose in the z=0 plane
* @param poseKey index of the robot pose2 in the z=0 plane
* @param landmarkKey index of the landmark point3
*/
PlanarProjectionFactor(
const Point3& landmark,
PlanarProjectionFactor2(
const Point2& measured,
const Pose3& offset,
const Cal3DS2& calib,
const SharedNoiseModel& model,
Key landmarkKey,
Key poseKey)
: NoiseModelFactorN(model, poseKey),
landmark_(landmark),
measured_(measured),
: PlanarProjectionFactorBase(measured),
NoiseModelFactorN(model, landmarkKey, poseKey),
offset_(offset),
calib_(calib)
{
calib_(calib) {
assert(2 == model->dim());
}

~PlanarProjectionFactor() override {}
/**
* @param landmark estimated landmark
* @param pose estimated pose2
* @param H1 landmark jacobian
* @param H2 pose jacobian
*/
Vector evaluateError(
const Point3& landmark,
const Pose2& pose,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
return h(landmark, pose, offset_, calib_, H1, H2, {}, {}) - measured_;
}

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

template<>
struct traits<PlanarProjectionFactor2> :
public Testable<PlanarProjectionFactor2> {};

/**
* @class PlanarProjectionFactor3
* @brief Three unknowns: the pose, the camera offset, and the camera calibration.
* Landmark is constant.
* This is intended to be used for camera calibration.
*/
class PlanarProjectionFactor3 : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Pose3, Cal3DS2> {
public:
typedef NoiseModelFactorN<Pose2, Pose3, Cal3DS2> Base;
using Base::evaluateError;

PlanarProjectionFactor3() {}

~PlanarProjectionFactor3() override {}

/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor(*this)));
gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor3(*this)));
}

Point2 h2(const Pose2& pose, OptionalMatrixType H1) const {
// this is x-forward z-up
gtsam::Matrix H0;
Pose3 offset_pose = Pose3(pose).compose(offset_, H0);
// this is z-forward y-down
gtsam::Matrix H00;
Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib_);
if (H1) {
// Dpose is 2x6 (R,t)
// H1 is 2x3 (x, y, theta)
gtsam::Matrix Dpose;
Point2 result = camera.project(landmark_, Dpose);
Dpose = Dpose * H00 * H0;
*H1 = Matrix::Zero(2, 3);
(*H1)(0, 0) = Dpose(0, 3); // du/dx
(*H1)(1, 0) = Dpose(1, 3); // dv/dx
(*H1)(0, 1) = Dpose(0, 4); // du/dy
(*H1)(1, 1) = Dpose(1, 4); // dv/dy
(*H1)(0, 2) = Dpose(0, 2); // du/dyaw
(*H1)(1, 2) = Dpose(1, 2); // dv/dyaw
return result;
}
else {
return camera.project(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)
: 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 calib calibration
* @param H1 pose jacobian
* @param H2 offset jacobian
* @param H3 calibration jacobian
*/
Vector evaluateError(
const Pose2& pose,
OptionalMatrixType H1 = OptionalNone) const override {
try {
return h2(pose, H1) - measured_;
} catch (CheiralityException& e) {
std::cout << "****** CHIRALITY EXCEPTION ******\n";
std::cout << "landmark " << landmark_ << "\n";
std::cout << "pose " << pose << "\n";
std::cout << "offset " << offset_ << "\n";
std::cout << "calib " << calib_ << "\n";
if (H1) *H1 = Matrix::Zero(2, 3);
// return a large error
return Matrix::Constant(2, 1, 2.0 * calib_.fx());
}
const Pose3& offset,
const Cal3DS2& calib,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone) const override {
return h(landmark_, pose, offset, calib, {}, H1, H2, H3) - measured_;
}
};

// camera "zero" is facing +z; this turns it to face +x
const Pose3 PlanarProjectionFactor::CAM_COORD = Pose3(
Rot3(0, 0, 1,//
-1, 0, 0, //
0, -1, 0),
Vector3(0, 0, 0)
);
private:
Point3 landmark_; // landmark
};

template<>
struct traits<PlanarProjectionFactor> :
public Testable<PlanarProjectionFactor > {
};
struct traits<PlanarProjectionFactor3> :
public Testable<PlanarProjectionFactor3> {};

} // namespace gtsam
} // namespace gtsam
Loading

0 comments on commit 14c7998

Please sign in to comment.