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

remove unnecessary malloc

release/4.3a0
thduynguyen 2014-12-25 08:53:53 -05:00
parent 5d0e8f202f
commit 34592f21c4
1 changed files with 10 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/geometry/Pose2.cpp
View File

@ -150,17 +150,16 @@ Matrix3 Pose2::ExpmapDerivative(const Vector3& v) {
*/
double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha;
double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha;
J = (Matrix(3,3) <<
sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha,
c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha,
0, 0, 1).finished();
J << sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha,
c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha,
0, 0, 1;
}
else {
// Thanks to Krunal: Apply L'Hospital rule to several times to
// compute the limits when alpha -> 0
J = (Matrix(3,3) << 1,0,-0.5*v[1],
J << 1,0,-0.5*v[1],
0,1, 0.5*v[0],
0,0, 1).finished();
0,0, 1;
}
return J;
@ -175,15 +174,14 @@ Matrix3 Pose2::LogmapDerivative(const Vector3& v) {
double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha));
double v1 = v[0], v2 = v[1];
J = (Matrix(3,3) <<
alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2,
0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha,
0, 0, 1).finished();
J << alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2,
0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha,
0, 0, 1;
}
else {
J = (Matrix(3,3) << 1,0, 0.5*v[1],
J << 1,0, 0.5*v[1],
0,1, -0.5*v[0],
0,0, 1).finished();
0,0, 1;
}
return J;
}