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

Fixing compile/interface issues, making (failing) testBetweenFactor more verbose

release/4.3a0
Alex Cunningham 2013-08-08 17:59:19 +00:00
parent 389f2dd6cc
commit cfd81093bd
4 changed files with 26 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -643,6 +643,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBetweenFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>testBetweenFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>

7
gtsam/slam/tests/testBetweenFactor.cpp
View File

@ -15,6 +15,7 @@ using namespace gtsam;
using namespace gtsam::symbol_shorthand;
using namespace gtsam::noiseModel;
/* ************************************************************************* */
TEST(BetweenFactor, Rot3) {
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.4, 0.5, 0.6);
@ -26,19 +27,19 @@ TEST(BetweenFactor, Rot3) {
Vector actual = factor.evaluateError(R1, R2, actualH1, actualH2);
Vector expected = Rot3::Logmap(measured.inverse() * R1.between(R2));
CHECK(assert_equal(expected,actual, 1e-100));
EXPECT(assert_equal(expected,actual, 1e-100));
Matrix numericalH1 = numericalDerivative21(
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
boost::none)), R1, R2, 1e-5);
CHECK(assert_equal(numericalH1,actualH1));
EXPECT(assert_equal(numericalH1,actualH1, 1e-5));
Matrix numericalH2 = numericalDerivative22(
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
boost::none)), R1, R2, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));
EXPECT(assert_equal(numericalH2,actualH2, 1e-5));
}
/* ************************************************************************* */

24
gtsam/slam/tests/testReferenceFrameFactor.cpp
View File

@ -136,15 +136,15 @@ TEST( ReferenceFrameFactor, converge_trans ) {
// Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 2.0); // beyond pi/2 - fails
NonlinearFactorGraph graph;
graph.add(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.add(PointReferenceFrameFactor(lB2, tA1, lA2));
graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.push_back(PointReferenceFrameFactor(lB2, tA1, lA2));
// hard constraints on points
double error_gain = 1000.0;
graph.add(NonlinearEquality<gtsam::Point2>(lA1, local1, error_gain));
graph.add(NonlinearEquality<gtsam::Point2>(lA2, local2, error_gain));
graph.add(NonlinearEquality<gtsam::Point2>(lB1, global1, error_gain));
graph.add(NonlinearEquality<gtsam::Point2>(lB2, global2, error_gain));
graph.push_back(NonlinearEquality<gtsam::Point2>(lA1, local1, error_gain));
graph.push_back(NonlinearEquality<gtsam::Point2>(lA2, local2, error_gain));
graph.push_back(NonlinearEquality<gtsam::Point2>(lB1, global1, error_gain));
graph.push_back(NonlinearEquality<gtsam::Point2>(lB2, global2, error_gain));
// create initial estimate
Values init;
@ -186,9 +186,9 @@ TEST( ReferenceFrameFactor, converge_local ) {
NonlinearFactorGraph graph;
double error_gain = 1000.0;
graph.add(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.add(NonlinearEquality<gtsam::Point2>(lB1, global, error_gain));
graph.add(NonlinearEquality<gtsam::Pose2>(tA1, trans, error_gain));
graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.push_back(NonlinearEquality<gtsam::Point2>(lB1, global, error_gain));
graph.push_back(NonlinearEquality<gtsam::Pose2>(tA1, trans, error_gain));
// create initial estimate
Values init;
@ -222,9 +222,9 @@ TEST( ReferenceFrameFactor, converge_global ) {
NonlinearFactorGraph graph;
double error_gain = 1000.0;
graph.add(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.add(NonlinearEquality<gtsam::Point2>(lA1, local, error_gain));
graph.add(NonlinearEquality<gtsam::Pose2>(tA1, trans, error_gain));
graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.push_back(NonlinearEquality<gtsam::Point2>(lA1, local, error_gain));
graph.push_back(NonlinearEquality<gtsam::Pose2>(tA1, trans, error_gain));
// create initial estimate
Values init;

4
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -189,11 +189,11 @@ TEST( StereoFactor, singlePoint)
{
NonlinearFactorGraph graph;
graph.add(NonlinearEquality<Pose3>(X(1), camera1));
graph.push_back(NonlinearEquality<Pose3>(X(1), camera1));
StereoPoint2 measurement(320, 320.0-50, 240);
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
graph.add(GenericStereoFactor<Pose3, Point3>(measurement, model, X(1), L(1), K));
graph.push_back(GenericStereoFactor<Pose3, Point3>(measurement, model, X(1), L(1), K));
// Create a configuration corresponding to the ground truth
Values values;