TrajectoryAlignerSim3 - add point3 constraints if available, optionally use GNC#2445
TrajectoryAlignerSim3 - add point3 constraints if available, optionally use GNC#2445
Conversation
There was a problem hiding this comment.
Pull request overview
This PR extends TrajectoryAlignerSim3 to optionally accept point3-to-point3 correspondence constraints between overlapping trajectories and optionally use a GNC (Graduated Non-Convexity) optimizer instead of the default Levenberg-Marquardt optimizer.
Changes:
- Extended the single constructor with two new optional parameters:
overlapping_pointsanduse_gnc_optimizer - Added new overloaded constructors in the SWIG interface to expose the new functionality
- Implemented the point3 constraint graph building and conditional GNC optimizer selection
Reviewed changes
Copilot reviewed 3 out of 3 changed files in this pull request and generated 5 comments.
| File | Description |
|---|---|
gtsam/sfm/TrajectoryAlignerSim3.cpp |
Core implementation of point3 constraints and GNC optimizer branching |
gtsam/sfm/TrajectoryAlignerSim3.h |
Header updated with new optional parameters and new member field |
gtsam/sfm/sfm.i |
SWIG interface updated with new constructor overloads |
gtsam/sfm/TrajectoryAlignerSim3.cpp
Outdated
| const Expression<Similarity3> bSa(simKey); | ||
| for (const auto &[p1, p2] : overlap) { | ||
| const Point3_ b_p2_(bSa, &Similarity3::transformFrom, p1); | ||
| graph_.addExpressionFactor(b_p2_, p2, noiseModel::Isotropic::Sigma(3, 3-2)); |
There was a problem hiding this comment.
The noise sigma value 3-2 evaluates to 1 at compile time. This looks like an unintentional leftover expression — likely either 3.0 or some other meaningful constant was intended as the sigma. The magic arithmetic makes the intent unclear and could mask a bug if the wrong value was left in accidentally. Replace with the intended numeric literal and, ideally, name it.
gtsam/sfm/TrajectoryAlignerSim3.cpp
Outdated
| const Point3_ b_p2_(bSa, &Similarity3::transformFrom, p1); | ||
| graph_.addExpressionFactor(b_p2_, p2, noiseModel::Isotropic::Sigma(3, 3-2)); |
There was a problem hiding this comment.
The variable b_p2_ has a trailing underscore, which in the GTSAM codebase conventionally denotes an Expression<> type, not a plain value. While that is technically correct here, the name itself is confusing: p1 is the point in the a frame being transformed to the b frame, so it represents the b-frame version of p1, not of p2. A clearer name such as b_p1_ would match the semantics.
| const Point3_ b_p2_(bSa, &Similarity3::transformFrom, p1); | |
| graph_.addExpressionFactor(b_p2_, p2, noiseModel::Isotropic::Sigma(3, 3-2)); | |
| const Point3_ b_p1_(bSa, &Similarity3::transformFrom, p1); | |
| graph_.addExpressionFactor(b_p1_, p2, noiseModel::Isotropic::Sigma(3, 3-2)); |
gtsam/sfm/TrajectoryAlignerSim3.cpp
Outdated
|
|
||
|
|
There was a problem hiding this comment.
The closing brace on line 122 closes the constructor body, but the if (!overlapping_points.empty()) block opened on line 106 is never explicitly closed — there is a missing } to close the if block (and its inner for loop is also correctly closed). As written, the constructor body terminates prematurely, meaning any code that should follow this block (e.g., the use_gnc_optimizer_ member initialization) is never reached, and the constructor is also missing the assignment use_gnc_optimizer_ = use_gnc_optimizer;. The missing closing brace for the if block must be added before line 122.
| } | |
| use_gnc_optimizer_ = use_gnc_optimizer; |
gtsam/sfm/TrajectoryAlignerSim3.h
Outdated
| const std::vector<Similarity3> &bSa_all = {}, | ||
| const std::vector<std::vector<std::pair<Point3, Point3>>> &overlapping_points = {}, | ||
| const bool use_gnc_optimizer = false); |
There was a problem hiding this comment.
The new constructor signature adds overlapping_points and use_gnc_optimizer as optional parameters to the existing constructor, but the corresponding .cpp implementation shown in the diff only adds overlapping_points — the use_gnc_optimizer parameter is absent from the definition's parameter list. This mismatch means use_gnc_optimizer_ is never set from the constructor argument, so the GNC path is never taken regardless of what the caller passes.
gtsam/sfm/sfm.i
Outdated
| TrajectoryAlignerSim3( | ||
| const std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>& aTi, | ||
| const std::vector<std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>>& bTi_all, | ||
| const std::vector<gtsam::Similarity3>& bSa_all, | ||
| const std::vector<std::vector<std::pair<gtsam::Point3, gtsam::Point3>>> &overlapping_points, | ||
| const bool use_gnc_optimizer); |
There was a problem hiding this comment.
Because the C++ header now exposes a single constructor with all parameters defaulted, the SWIG interface should mirror this instead of declaring multiple overloads. Having separate overloads in the .i file that differ only by the presence of overlapping_points and use_gnc_optimizer creates redundancy and can diverge from the header over time. Consider using a single SWIG declaration with default arguments matching the header, or at minimum ensuring the overloads stay in sync with any future signature changes.
timeSFMBAL benchmark
No head benchmark results were found. Worker runs
|
dellaert
left a comment
dellaert
left a comment
There was a problem hiding this comment.
LGTM but some CI failures. Make sure to compile with warnings treated as errors.
dellaert
left a comment
dellaert
left a comment
There was a problem hiding this comment.
LGTM< will merge after CI succeeds
No description provided.