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

move Line3 transformTo definition to header to resolve ambiguity

release/4.3a0
Varun Agrawal 2022-02-17 11:13:40 -05:00
parent 38425f1164
commit 1e1e2c26ee
2 changed files with 26 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/geometry/Line3.cpp
View File

@ -91,30 +91,4 @@ Point3 Line3::point(double distance) const {
return rotated_center + distance * R_.r3();
}
Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
OptionalJacobian<4, 6> Dpose, OptionalJacobian<4, 4> Dline) {
Rot3 wRc = wTc.rotation();
Rot3 cRw = wRc.inverse();
Rot3 cRl = cRw * wL.R_;
Vector2 w_ab;
Vector3 t = ((wL.R_).transpose() * wTc.translation());
Vector2 c_ab(wL.a_ - t[0], wL.b_ - t[1]);
if (Dpose) {
Matrix3 lRc = (cRl.matrix()).transpose();
Dpose->setZero();
// rotation
Dpose->block<2, 3>(0, 0) = -lRc.block<2, 3>(0, 0);
// translation
Dpose->block<2, 3>(2, 3) = -lRc.block<2, 3>(0, 0);
}
if (Dline) {
Dline->setIdentity();
(*Dline)(0, 3) = -t[2];
(*Dline)(1, 2) = t[2];
}
return Line3(cRl, c_ab[0], c_ab[1]);
}
}
} // namespace gtsam

27
gtsam/geometry/Line3.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class Line3 {
class GTSAM_EXPORT Line3 {
private:
Rot3 R_; // Rotation of line about x and y in world frame
double a_, b_; // Intersection of line with the world x-y plane rotated by R_
@ -146,7 +146,30 @@ class Line3 {
*/
Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
OptionalJacobian<4, 6> Dpose = boost::none,
OptionalJacobian<4, 4> Dline = boost::none);
OptionalJacobian<4, 4> Dline = boost::none) {
Rot3 wRc = wTc.rotation();
Rot3 cRw = wRc.inverse();
Rot3 cRl = cRw * wL.R_;
Vector2 w_ab;
Vector3 t = ((wL.R_).transpose() * wTc.translation());
Vector2 c_ab(wL.a_ - t[0], wL.b_ - t[1]);
if (Dpose) {
Matrix3 lRc = (cRl.matrix()).transpose();
Dpose->setZero();
// rotation
Dpose->block<2, 3>(0, 0) = -lRc.block<2, 3>(0, 0);
// translation
Dpose->block<2, 3>(2, 3) = -lRc.block<2, 3>(0, 0);
}
if (Dline) {
Dline->setIdentity();
(*Dline)(0, 3) = -t[2];
(*Dline)(1, 2) = t[2];
}
return Line3(cRl, c_ab[0], c_ab[1]);
}
template<>
struct traits<Line3> : public internal::Manifold<Line3> {};