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

unstable/slam

release/4.3a0
kartik arcot 2023-01-12 14:02:31 -08:00
parent 4e2f0cc36b
commit d7f60353c9
12 changed files with 35 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -106,8 +106,8 @@ private:
Matrix Jacobian_wrt_t0_Overall_;
boost::optional<IMUBIAS> Bias_initial_; // Bias used when pre-integrating IMU measurements
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
std::optional<IMUBIAS> Bias_initial_; // Bias used when pre-integrating IMU measurements
std::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
public:
@ -126,7 +126,7 @@ public:
double dt12, const Vector world_g, const Vector world_rho,
const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent,
const Matrix& Jacobian_wrt_t0_Overall,
boost::optional<IMUBIAS> Bias_initial = boost::none, boost::optional<POSE> body_P_sensor = boost::none) :
std::optional<IMUBIAS> Bias_initial = {}, std::optional<POSE> body_P_sensor = {}) :
Base(model_equivalent, Pose1, Vel1, IMUBias1, Pose2, Vel2),
delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles),
dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall),
@ -385,7 +385,7 @@ public:
const Vector& delta_pos_in_t0, const Vector3& delta_angles,
double dt12, const Vector world_g, const Vector world_rho,
const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall,
const boost::optional<IMUBIAS>& Bias_initial = boost::none) {
const std::optional<IMUBIAS>& Bias_initial = {}) {
// Correct delta_pos_in_t0_ using (Bias1 - Bias_t0)
@ -414,7 +414,7 @@ public:
static inline VELOCITY PredictVelocityFromPreIntegration(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1,
const Vector& delta_vel_in_t0, double dt12, const Vector world_g, const Vector world_rho,
const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall,
const boost::optional<IMUBIAS>& Bias_initial = boost::none) {
const std::optional<IMUBIAS>& Bias_initial = {}) {
// Correct delta_vel_in_t0_ using (Bias1 - Bias_t0)
Vector delta_BiasAcc = Bias1.accelerometer();
@ -436,7 +436,7 @@ public:
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
double dt12, const Vector world_g, const Vector world_rho,
const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall,
const boost::optional<IMUBIAS>& Bias_initial = boost::none) {
const std::optional<IMUBIAS>& Bias_initial = {}) {
Pose2 = PredictPoseFromPreIntegration(Pose1, Vel1, Bias1, delta_pos_in_t0, delta_angles, dt12, world_g, world_rho, world_omega_earth, Jacobian_wrt_t0_Overall, Bias_initial);
Vel2 = PredictVelocityFromPreIntegration(Pose1, Vel1, Bias1, delta_vel_in_t0, dt12, world_g, world_rho, world_omega_earth, Jacobian_wrt_t0_Overall, Bias_initial);
@ -447,7 +447,7 @@ public:
Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t,
const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0 = IMUBIAS(),
boost::optional<POSE> p_body_P_sensor = boost::none){
std::optional<POSE> p_body_P_sensor = {}){
// Note: all delta terms refer to an IMU\sensor system at t0
// Note: Earth-related terms are not accounted here but are incorporated in predict functions.

6
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -105,7 +105,7 @@ private:
Matrix Jacobian_wrt_t0_Overall_;
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
std::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
public:
@ -124,7 +124,7 @@ public:
double dt12, const Vector world_g, const Vector world_rho,
const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent,
const Matrix& Jacobian_wrt_t0_Overall,
boost::optional<POSE> body_P_sensor = boost::none) :
std::optional<POSE> body_P_sensor = {}) :
Base(model_equivalent, Pose1, Vel1, Pose2, Vel2),
delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles),
dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall),
@ -352,7 +352,7 @@ public:
Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t,
const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall,
boost::optional<POSE> p_body_P_sensor = boost::none){
std::optional<POSE> p_body_P_sensor = {}){
// Note: all delta terms refer to an IMU\sensor system at t0
// Note: Earth-related terms are not accounted here but are incorporated in predict functions.

4
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -92,7 +92,7 @@ private:
Vector world_rho_;
Vector world_omega_earth_;
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
std::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
public:
@ -108,7 +108,7 @@ public:
/** Constructor */
InertialNavFactor_GlobalVelocity(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2,
const Vector& measurement_acc, const Vector& measurement_gyro, const double measurement_dt, const Vector world_g, const Vector world_rho,
const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_continuous, boost::optional<POSE> body_P_sensor = boost::none) :
const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_continuous, std::optional<POSE> body_P_sensor = {}) :
Base(calc_descrete_noise_model(model_continuous, measurement_dt ),
Pose1, Vel1, IMUBias1, Pose2, Vel2), measurement_acc_(measurement_acc), measurement_gyro_(measurement_gyro),
dt_(measurement_dt), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), body_P_sensor_(body_P_sensor) { }

2
gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp
View File

@ -43,7 +43,7 @@ Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& wTwi,
// Calculate the error between measured and estimated planes in sensor frame.
const Vector3 err = measured_p_.errorVector(i_plane,
boost::none, (H1 || H2 || H3) ? &error_H_predicted : nullptr);
{}, (H1 || H2 || H3) ? &error_H_predicted : nullptr);
// Apply the chain rule to calculate the derivatives.
if (H1) {

6
gtsam_unstable/slam/MultiProjectionFactor.h
View File

@ -39,7 +39,7 @@ namespace gtsam {
// Keep a copy of measurement and calibration for I/O
Vector measured_; ///< 2D measurement for each of the n views
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
std::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
// verbosity handling for Cheirality Exceptions
@ -72,7 +72,7 @@ namespace gtsam {
*/
MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
KeySet poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
boost::optional<POSE> body_P_sensor = boost::none) :
std::optional<POSE> body_P_sensor = {}) :
Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(false), verboseCheirality_(false) {
keys_.assign(poseKeys.begin(), poseKeys.end());
@ -94,7 +94,7 @@ namespace gtsam {
MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
KeySet poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
bool throwCheirality, bool verboseCheirality,
boost::optional<POSE> body_P_sensor = boost::none) :
std::optional<POSE> body_P_sensor = {}) :
Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}

4
gtsam_unstable/slam/PoseBetweenFactor.h
View File

@ -37,7 +37,7 @@ namespace gtsam {
typedef NoiseModelFactorN<POSE, POSE> Base;
POSE measured_; /** The measurement */
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
std::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
/** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
@ -55,7 +55,7 @@ namespace gtsam {
/** Constructor */
PoseBetweenFactor(Key key1, Key key2, const POSE& measured,
const SharedNoiseModel& model, boost::optional<POSE> body_P_sensor = boost::none) :
const SharedNoiseModel& model, std::optional<POSE> body_P_sensor = {}) :
Base(model, key1, key2), measured_(measured), body_P_sensor_(body_P_sensor) {
}

4
gtsam_unstable/slam/PosePriorFactor.h
View File

@ -34,7 +34,7 @@ namespace gtsam {
typedef NoiseModelFactorN<POSE> Base;
POSE prior_; /** The measurement */
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
std::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
/** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
@ -54,7 +54,7 @@ namespace gtsam {
/** Constructor */
PosePriorFactor(Key key, const POSE& prior, const SharedNoiseModel& model,
boost::optional<POSE> body_P_sensor = boost::none) :
std::optional<POSE> body_P_sensor = {}) :
Base(model, key), prior_(prior), body_P_sensor_(body_P_sensor) {
}

4
gtsam_unstable/slam/ProjectionFactorPPP.h
View File

@ -130,13 +130,13 @@ namespace gtsam {
if(H1 || H2 || H3) {
Matrix H0, H02;
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_);
Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
Point2 reprojectionError(camera.project(point, H1, H3, {}) - measured_);
*H2 = *H1 * H02;
*H1 = *H1 * H0;
return reprojectionError;
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(transform), *K_);
return camera.project(point, H1, H3, boost::none) - measured_;
return camera.project(point, H1, H3, {}) - measured_;
}
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);

4
gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp
View File

@ -31,7 +31,7 @@ Vector ProjectionFactorRollingShutter::evaluateError(
gtsam::Matrix HbodySensor;
PinholeCamera<Cal3_S2> camera(
pose.compose(*body_P_sensor_, HbodySensor), *K_);
Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) -
Point2 reprojectionError(camera.project(point, Hprj, H3, {}) -
measured_);
if (H1) *H1 = Hprj * HbodySensor * (*H1);
if (H2) *H2 = Hprj * HbodySensor * (*H2);
@ -42,7 +42,7 @@ Vector ProjectionFactorRollingShutter::evaluateError(
}
} else {
PinholeCamera<Cal3_S2> camera(pose, *K_);
Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) -
Point2 reprojectionError(camera.project(point, Hprj, H3, {}) -
measured_);
if (H1) *H1 = Hprj * (*H1);
if (H2) *H2 = Hprj * (*H2);

6
gtsam_unstable/slam/ProjectionFactorRollingShutter.h
View File

@ -49,7 +49,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
double alpha_; ///< interpolation parameter in [0,1] corresponding to the
///< point2 measurement
boost::shared_ptr<Cal3_S2> K_; ///< shared pointer to calibration object
boost::optional<Pose3>
std::optional<Pose3>
body_P_sensor_; ///< The pose of the sensor in the body frame
// verbosity handling for Cheirality Exceptions
@ -96,7 +96,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
const Point2& measured, double alpha, const SharedNoiseModel& model,
Key poseKey_a, Key poseKey_b, Key pointKey,
const boost::shared_ptr<Cal3_S2>& K,
boost::optional<Pose3> body_P_sensor = boost::none)
std::optional<Pose3> body_P_sensor = {})
: Base(model, poseKey_a, poseKey_b, pointKey),
measured_(measured),
alpha_(alpha),
@ -127,7 +127,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
Key poseKey_a, Key poseKey_b, Key pointKey,
const boost::shared_ptr<Cal3_S2>& K, bool throwCheirality,
bool verboseCheirality,
boost::optional<Pose3> body_P_sensor = boost::none)
std::optional<Pose3> body_P_sensor = {})
: Base(model, poseKey_a, poseKey_b, pointKey),
measured_(measured),
alpha_(alpha),

4
gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
View File

@ -165,8 +165,8 @@ namespace gtsam {
T currA_T_currB_pred;
if (H) {
Matrix H_compose, H_between1;
T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, boost::none);
currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, boost::none, H_between1);
T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, {});
currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, {}, H_between1);
(*H)[0] = H_compose * H_between1;
}
else {

12
gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp
View File

@ -342,24 +342,24 @@ TEST(ProjectionFactorRollingShutter, cheirality) {
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
std::placeholders::_3, {}, {},
{})),
pose1, pose2, point);
Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
std::placeholders::_3, {}, {},
{})),
pose1, pose2, point);
Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none)),
std::placeholders::_3, {}, {},
{})),
pose1, pose2, point);
CHECK(assert_equal(H1Expected, H1Actual, 1e-5));