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

timePose2 updates

release/4.3a0
kartik arcot 2023-01-10 16:04:44 -08:00
parent bbb997f895
commit 376c910e2b
1 changed files with 4 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
timing/timePose2.cpp
View File

@ -37,7 +37,7 @@ Pose2 Pose2betweenDefault(const Pose2& r1, const Pose2& r2) {
/* ************************************************************************* */
Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
OptionalJacobian<3,3> H1 = {}, OptionalJacobian<3,3> H2 = {}) {
// get cosines and sines from rotation matrices
const Rot2& R1 = r1.r(), R2 = r2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
@ -68,43 +68,9 @@ Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2,
return Pose2(R,t);
}
/* ************************************************************************* */
Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2)
{
// get cosines and sines from rotation matrices
const Rot2& R1 = r1.r(), R2 = r2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Assert that R1 and R2 are normalized
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = r2.t() - r1.t();
double x = dt.x(), y = dt.y();
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;
*H1 = Matrix3(); (*H1) <<
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0;
}
if (H2) *H2 = I_3x3;
return Pose2(R,t);
}
/* ************************************************************************* */
Vector Pose2BetweenFactorEvaluateErrorDefault(const Pose2& measured, const Pose2& p1, const Pose2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2)
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2)
{
Pose2 hx = p1.between(p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
@ -113,7 +79,7 @@ Vector Pose2BetweenFactorEvaluateErrorDefault(const Pose2& measured, const Pose2
/* ************************************************************************* */
Vector Pose2BetweenFactorEvaluateErrorOptimizedBetween(const Pose2& measured, const Pose2& p1, const Pose2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2)
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2)
{
Pose2 hx = Pose2betweenOptimized(p1, p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
@ -122,7 +88,7 @@ Vector Pose2BetweenFactorEvaluateErrorOptimizedBetween(const Pose2& measured, co
/* ************************************************************************* */
Vector Pose2BetweenFactorEvaluateErrorOptimizedBetweenFixed(const Pose2& measured, const Pose2& p1, const Pose2& p2,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2)
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2)
{
// TODO: Implement
Pose2 hx = Pose2betweenOptimized(p1, p2, H1, H2); // h(x)