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

remove Pose2/3 casts from Sim2/3

release/4.3a0
Varun Agrawal 2022-04-29 17:53:07 -04:00
parent 5be6309a58
commit 53b370566e
5 changed files with 11 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Similarity2.cpp
View File

@ -211,6 +211,4 @@ Matrix3 Similarity2::matrix() const {
return T;
}
Similarity2::operator Pose2() const { return Pose2(R_, s_ * t_); }
} // namespace gtsam

3
gtsam/geometry/Similarity2.h
View File

@ -156,9 +156,6 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
/// Return the scale
double scale() const { return s_; }
/// Convert to a rigid body pose (R, s*t)
operator Pose2() const;
/// Dimensionality of tangent space = 4 DOF - used to autodetect sizes
inline static size_t Dim() { return 4; }

4
gtsam/geometry/Similarity3.cpp
View File

@ -290,8 +290,4 @@ Matrix4 Similarity3::matrix() const {
return T;
}
Similarity3::operator Pose3() const {
return Pose3(R_, s_ * t_);
}
} // namespace gtsam

5
gtsam/geometry/Similarity3.h
View File

@ -190,11 +190,6 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
/// Return the scale
double scale() const { return s_; }
/// Convert to a rigid body pose (R, s*t)
/// TODO(frank): why is this here? Red flag! Definitely don't have it as a
/// cast.
operator Pose3() const;
/// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
inline static size_t Dim() { return 7; }

22
gtsam/geometry/tests/testSimilarity3.cpp
View File

@ -458,18 +458,18 @@ TEST(Similarity3, Optimization2) {
Values result;
result = LevenbergMarquardtOptimizer(graph, initial).optimize();
//result.print("Optimized Estimate\n");
Pose3 p1, p2, p3, p4, p5;
p1 = Pose3(result.at<Similarity3>(X(1)));
p2 = Pose3(result.at<Similarity3>(X(2)));
p3 = Pose3(result.at<Similarity3>(X(3)));
p4 = Pose3(result.at<Similarity3>(X(4)));
p5 = Pose3(result.at<Similarity3>(X(5)));
Similarity3 p1, p2, p3, p4, p5;
p1 = result.at<Similarity3>(X(1));
p2 = result.at<Similarity3>(X(2));
p3 = result.at<Similarity3>(X(3));
p4 = result.at<Similarity3>(X(4));
p5 = result.at<Similarity3>(X(5));
//p1.print("Pose1");
//p2.print("Pose2");
//p3.print("Pose3");
//p4.print("Pose4");
//p5.print("Pose5");
//p1.print("Similarity1");
//p2.print("Similarity2");
//p3.print("Similarity3");
//p4.print("Similarity4");
//p5.print("Similarity5");
Similarity3 expected(0.7);
EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4));