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

spurious vector

release/4.3a0
dellaert 2016-02-18 08:50:11 -08:00
parent a9b0d81be4
commit 3a50a0e19e
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/tests/testPoseVelocityBias.cpp
View File

@ -31,7 +31,7 @@ using namespace gtsam;
Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) {
Matrix3 R1 = pvb1.pose.rotation().matrix();
// Ri.transpose() translate the error from the global frame into pose1's frame
const Vector3 fp = R1.transpose() * (pvb2.pose.translation() - pvb1.pose.translation()).vector();
const Vector3 fp = R1.transpose() * (pvb2.pose.translation() - pvb1.pose.translation());
const Vector3 fv = R1.transpose() * (pvb2.velocity - pvb1.velocity);
const Rot3 R1BetweenR2 = pvb1.pose.rotation().between(pvb2.pose.rotation());
const Vector3 fR = Rot3::Logmap(R1BetweenR2);