RangeFactor works
Frank Dellaert
parent
a552ba4539
commit
df62213619
44
.cproject
44
.cproject
|
|
@ -1,4 +1,4 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
|
||||
<?fileVersion 4.0.0?>
|
||||
|
||||
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
|
|
@ -298,6 +298,22 @@
|
|||
</storageModule>
|
||||
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
|
||||
<buildTargets>
|
||||
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-k</buildArguments>
|
||||
|
|
@ -452,7 +468,7 @@
|
|||
</target>
|
||||
<target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBayesTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -467,7 +483,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -723,7 +739,7 @@
|
|||
</target>
|
||||
<target name="testGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -737,10 +753,10 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testTupleConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testRangeFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testTupleConfig.run</buildTarget>
|
||||
<buildTarget>testRangeFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -769,22 +785,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
</buildTargets>
|
||||
</storageModule>
|
||||
</cconfiguration>
|
||||
|
|
|
|||
|
|
@ -214,11 +214,13 @@ testPose2Graph_SOURCES = $(example) testPose2Graph.cpp
|
|||
testPose2Graph_LDADD = libgtsam.la
|
||||
|
||||
# 2D Bearing and Range
|
||||
headers += BearingFactor.h
|
||||
headers += BearingFactor.h RangeFactor.h
|
||||
sources +=
|
||||
check_PROGRAMS += testBearingFactor
|
||||
check_PROGRAMS += testBearingFactor testRangeFactor
|
||||
testBearingFactor_SOURCES = $(example) testBearingFactor.cpp
|
||||
testBearingFactor_LDADD = libgtsam.la
|
||||
testRangeFactor_SOURCES = $(example) testRangeFactor.cpp
|
||||
testRangeFactor_LDADD = libgtsam.la
|
||||
|
||||
# 3D Pose constraints
|
||||
headers += Pose3Factor.h
|
||||
|
|
|
|||
|
|
@ -0,0 +1,68 @@
|
|||
/**
|
||||
* @file RangeFactor.H
|
||||
* @authors Frank Dellaert
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Pose2.h"
|
||||
#include "Point2.h"
|
||||
#include "NonlinearFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Calculate range to a landmark
|
||||
* @param pose 2D pose of robot
|
||||
* @param point 2D location of landmark
|
||||
* @return range (double)
|
||||
*/
|
||||
double range(const Pose2& pose, const Point2& point) {
|
||||
Point2 d = transform_to(pose, point);
|
||||
return d.norm();
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate range and optional derivative(s)
|
||||
*/
|
||||
double range(const Pose2& pose, const Point2& point,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
|
||||
if (!H1 && !H2) return range(pose, point);
|
||||
Point2 d = transform_to(pose, point);
|
||||
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
|
||||
Matrix D_result_d = Matrix_(1, 2, x / n, y / n);
|
||||
if (H1) *H1 = D_result_d * Dtransform_to1(pose, point);
|
||||
if (H2) *H2 = D_result_d * Dtransform_to2(pose, point);
|
||||
return n;
|
||||
}
|
||||
|
||||
/**
|
||||
* Non-linear factor for a constraint derived from a 2D measurement,
|
||||
* i.e. the main building block for visual SLAM.
|
||||
*/
|
||||
template<class Config, class PoseKey, class PointKey>
|
||||
class RangeFactor: public NonlinearFactor2<Config, PoseKey, Pose2, PointKey,
|
||||
Point2> {
|
||||
private:
|
||||
|
||||
double z_; /** measurement */
|
||||
|
||||
typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base;
|
||||
|
||||
public:
|
||||
|
||||
RangeFactor(); /* Default constructor */
|
||||
|
||||
RangeFactor(double z, double sigma, const PoseKey& i, const PointKey& j) :
|
||||
Base(sigma, i, j), z_(z) {
|
||||
}
|
||||
|
||||
/** h(x)-z */
|
||||
Vector evaluateError(const Pose2& pose, const Point2& point,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
double hx = gtsam::range(pose, point, H1, H2);
|
||||
return Vector_(1, hx - z_);
|
||||
}
|
||||
}; // RangeFactor
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
/**
|
||||
* @file testRangeFactor.cpp
|
||||
* @brief Unit tests for RangeFactor Class
|
||||
* @authors Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "numericalDerivative.h"
|
||||
#include "RangeFactor.h"
|
||||
#include "TupleConfig.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// typedefs
|
||||
typedef Symbol<Pose2, 'x'> PoseKey;
|
||||
typedef Symbol<Point2, 'l'> PointKey;
|
||||
typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
|
||||
typedef RangeFactor<Config, PoseKey, PointKey> MyFactor;
|
||||
|
||||
// some shared test values
|
||||
Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
|
||||
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( RangeFactor, range )
|
||||
{
|
||||
Matrix expectedH1, actualH1, expectedH2, actualH2;
|
||||
|
||||
// establish range is indeed zero
|
||||
DOUBLES_EQUAL(1,gtsam::range(x1,l1),1e-9);
|
||||
|
||||
// establish range is indeed 45 degrees
|
||||
DOUBLES_EQUAL(sqrt(2),gtsam::range(x1,l2),1e-9);
|
||||
|
||||
// Another pair
|
||||
double actual23 = gtsam::range(x2, l3, actualH1, actualH2);
|
||||
DOUBLES_EQUAL(sqrt(2),actual23,1e-9);
|
||||
|
||||
// Check numerical derivatives
|
||||
expectedH1 = numericalDerivative21(range, x2, l3, 1e-5);
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
expectedH2 = numericalDerivative22(range, x2, l3, 1e-5);
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
|
||||
// Another test
|
||||
double actual34 = gtsam::range(x3, l4, actualH1, actualH2);
|
||||
DOUBLES_EQUAL(2,actual34,1e-9);
|
||||
|
||||
// Check numerical derivatives
|
||||
expectedH1 = numericalDerivative21(range, x3, l4, 1e-5);
|
||||
expectedH2 = numericalDerivative22(range, x3, l4, 1e-5);
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( RangeFactor, evaluateError )
|
||||
{
|
||||
// Create factor
|
||||
double z(sqrt(2) - 0.22); // h(x) - z = 0.22
|
||||
double sigma = 0.1;
|
||||
MyFactor factor(z, sigma, 2, 3);
|
||||
|
||||
// create config
|
||||
Config c;
|
||||
c.insert(2, x2);
|
||||
c.insert(3, l3);
|
||||
|
||||
// Check error
|
||||
Vector actual = factor.error_vector(c);
|
||||
CHECK(assert_equal(Vector_(1,0.22),actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue