@@ -182,15 +182,20 @@ class TrajectoryAlignerSim3 {
182182 TrajectoryAlignerSim3 (
183183 const std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>& aTi,
184184 const std::vector<std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>>& bTi_all,
185- const std::vector<gtsam::Similarity3>& bSa_all,
186- const std::vector<std::vector<std::pair<gtsam::Point3, gtsam::Point3>>> &overlapping_points);
185+ const std::vector<gtsam::Similarity3>& bSa_all, const bool use_gnc_optimizer);
187186
188187 TrajectoryAlignerSim3 (
189188 const std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>& aTi,
190189 const std::vector<std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>>& bTi_all,
191- const std::vector<gtsam::Similarity3>& bSa_all,
190+ const std::vector<gtsam::Similarity3>& bSa_all, const bool use_gnc_optimizer,
191+ const std::vector<std::vector<std::pair<gtsam::Point3, gtsam::Point3>>> &overlapping_points);
192+
193+ TrajectoryAlignerSim3 (
194+ const std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>& aTi,
195+ const std::vector<std::vector<gtsam::UnaryMeasurement<gtsam::Pose3>>>& bTi_all,
196+ const std::vector<gtsam::Similarity3>& bSa_all, const bool use_gnc_optimizer,
192197 const std::vector<std::vector<std::pair<gtsam::Point3, gtsam::Point3>>> &overlapping_points,
193- const bool use_gnc_optimizer );
198+ const float point3_factor_sigma );
194199
195200 gtsam::Values solve () const ;
196201 gtsam::Marginals marginalize (
0 commit comments