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

add testFunctions (h, H1, H2) for Pose Constraint

release/4.3a0
Viorela Ila 2009-12-07 23:17:03 +00:00
parent 53af03368f
commit 95260281d1
1 changed files with 52 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
cpp/testNonlinearFactor.cpp
View File

@ -14,6 +14,7 @@
#include "Matrix.h"
#include "smallExample.h"
#include "Simulated2DMeasurement.h"
#include "Pose2.h"
using namespace std;
using namespace gtsam;
@ -170,6 +171,57 @@ TEST( NonlinearFactor, size )
CHECK(factor2->size() == 2);
CHECK(factor3->size() == 2);
}
/* ************************************************************************* */
Vector RotatePoseDisplacement(Vector d, double theta) {
double co=cos(theta);
double si=sin(theta);
return Matrix_(3,3, co, -si, 0.0, si, co, 0.0, 0.0, 0.0, 1.0)*d;
}
Vector h(const Pose2& p1, const Pose2& p2) {
double dx= p2.x()-p1.x();
double dy= p2.y()-p1.y();
double dtheta= p2.theta()-p1.theta();
return RotatePoseDisplacement(Vector_(3,dx,dy,dtheta),-p1.theta());
}
Matrix H1(const Pose2& p1, const Pose2& p2) {
double dx= p2.x()-p1.x();
double dy= p2.y()-p1.y();
double co=cos(p1.theta());
double si=sin(p1.theta());
return Matrix_(3,3, -co, -si, -si*dx+co*dy, si, -co, -co*dx-si*dy, 0.0, 0.0, -1.0);
}
Matrix H2(const Pose2& p1) {
double co=cos(p1.theta());
double si=sin(p1.theta());
return Matrix_(3,3, co, si, 0.0, -si, co, 0.0, 0.0, 0.0, 1.0);
}
TEST( PoseConstraintFactor2, testFunctions )
{
Pose2 p1(0.0, 6.0, 0.0);
Pose2 p2(0.101826, 6.111236, 0.011499);
//expected
Vector expectedh = Vector_(3, 0.101826, 0.111236, 0.011499);
Matrix expectedH1 = Matrix_(3,3,-1.0, 0.0, 0.111236, 0.0, -1.0, -0.101826, 0.0, 0.0, -1.0);
Matrix expectedH2 = Matrix_(3,3, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
// actual
Vector actualh = h(p1,p2);
Matrix actualH1 = H1(p1,p2);
Matrix actualH2 = H2(p1);
CHECK(assert_equal(actualh,expectedh));
CHECK(assert_equal(actualH1,expectedH1));
CHECK(assert_equal(actualH2,expectedH2));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */