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

Fixed RangeFactor

release/4.3a0
dellaert 2014-12-04 14:08:46 +01:00
parent add93f19a6
commit f7ebe4bfc4
3 changed files with 225 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
.cproject
View File

@ -592,6 +592,7 @@
</target> </target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget> <buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -599,6 +600,7 @@
</target> </target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget> <buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -646,6 +648,7 @@
</target> </target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget> <buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -653,6 +656,7 @@
</target> </target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget> <buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -660,6 +664,7 @@
</target> </target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget> <buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -675,6 +680,7 @@
</target> </target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget> <buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -848,6 +854,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testSmartStereoProjectionPoseFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSmartStereoProjectionPoseFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -1122,6 +1136,7 @@
</target> </target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget> <buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1351,6 +1366,46 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1433,7 +1488,6 @@
</target> </target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget> <buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1473,7 +1527,6 @@
</target> </target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget> <buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1481,7 +1534,6 @@
</target> </target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget> <buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1495,46 +1547,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -1792,6 +1804,7 @@
</target> </target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget> <buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1799,6 +1812,7 @@
</target> </target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget> <buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1806,6 +1820,7 @@
</target> </target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget> <buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1813,6 +1828,7 @@
</target> </target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget> <buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2627,6 +2643,7 @@
</target> </target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget> <buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2634,18 +2651,12 @@
</target> </target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget> <buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianISAM.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGaussianISAM.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -2902,6 +2913,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testRangeFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testRangeFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -3184,7 +3203,6 @@
</target> </target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget> <buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>

110
gtsam/slam/RangeFactor.h
View File

@ -26,12 +26,11 @@ namespace gtsam {
* Binary factor for a range measurement * Binary factor for a range measurement
* @addtogroup SLAM * @addtogroup SLAM
*/ */
template<class T1, class T2> template<class T1, class T2 = T1>
class RangeFactor: public NoiseModelFactor2<T1, T2> { class RangeFactor: public NoiseModelFactor2<T1, T2> {
private: private:
double measured_; /** measurement */ double measured_; /** measurement */
boost::optional<T1> body_P_sensor_; ///< The pose of the sensor in the body frame
typedef RangeFactor<T1, T2> This; typedef RangeFactor<T1, T2> This;
typedef NoiseModelFactor2<T1, T2> Base; typedef NoiseModelFactor2<T1, T2> Base;
@ -45,10 +44,8 @@ public:
} /* Default constructor */ } /* Default constructor */
RangeFactor(Key key1, Key key2, double measured, RangeFactor(Key key1, Key key2, double measured,
const SharedNoiseModel& model, boost::optional<T1> body_P_sensor = const SharedNoiseModel& model) :
boost::none) : Base(model, key1, key2), measured_(measured) {
Base(model, key1, key2), measured_(measured), body_P_sensor_(
body_P_sensor) {
} }
virtual ~RangeFactor() { virtual ~RangeFactor() {
@ -64,16 +61,92 @@ public:
Vector evaluateError(const T1& t1, const T2& t2, boost::optional<Matrix&> H1 = Vector evaluateError(const T1& t1, const T2& t2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const { boost::none, boost::optional<Matrix&> H2 = boost::none) const {
double hx; double hx;
if (body_P_sensor_) { hx = t1.range(t2, H1, H2);
if (H1) { return (Vector(1) << hx - measured_).finished();
Matrix H0; }
hx = t1.compose(*body_P_sensor_, H0).range(t2, H1, H2);
*H1 = *H1 * H0; /** return the measured */
} else { double measured() const {
hx = t1.compose(*body_P_sensor_).range(t2, H1, H2); return measured_;
} }
/** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected,
double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol)
&& fabs(this->measured_ - e->measured_) < tol;
}
/** print contents */
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "RangeFactor, range = " << measured_ << std::endl;
Base::print("", keyFormatter);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
// RangeFactor
/**
* Binary factor for a range measurement, with a transform applied
* @addtogroup SLAM
*/
template<class POSE, class T2 = POSE>
class RangeFactorWithTransform: public NoiseModelFactor2<POSE, T2> {
private:
double measured_; /** measurement */
POSE body_P_sensor_; ///< The pose of the sensor in the body frame
typedef RangeFactorWithTransform<POSE, T2> This;
typedef NoiseModelFactor2<POSE, T2> Base;
// Concept requirements for this factor
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, T2)
public:
RangeFactorWithTransform() {
} /* Default constructor */
RangeFactorWithTransform(Key key1, Key key2, double measured,
const SharedNoiseModel& model, const POSE& body_P_sensor) :
Base(model, key1, key2), measured_(measured), body_P_sensor_(
body_P_sensor) {
}
virtual ~RangeFactorWithTransform() {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** h(x)-z */
Vector evaluateError(const POSE& t1, const T2& t2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
double hx;
if (H1) {
Matrix H0;
hx = t1.compose(body_P_sensor_, H0).range(t2, H1, H2);
*H1 = *H1 * H0;
} else { } else {
hx = t1.range(t2, H1, H2); hx = t1.compose(body_P_sensor_).range(t2, H1, H2);
} }
return (Vector(1) << hx - measured_).finished(); return (Vector(1) << hx - measured_).finished();
} }
@ -89,17 +162,14 @@ public:
const This *e = dynamic_cast<const This*>(&expected); const This *e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) return e != NULL && Base::equals(*e, tol)
&& fabs(this->measured_ - e->measured_) < tol && fabs(this->measured_ - e->measured_) < tol
&& ((!body_P_sensor_ && !e->body_P_sensor_) && body_P_sensor_.equals(e->body_P_sensor_);
|| (body_P_sensor_ && e->body_P_sensor_
&& body_P_sensor_->equals(*e->body_P_sensor_)));
} }
/** print contents */ /** print contents */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const { DefaultKeyFormatter) const {
std::cout << s << "RangeFactor, range = " << measured_ << std::endl; std::cout << s << "RangeFactor, range = " << measured_ << std::endl;
if (this->body_P_sensor_) this->body_P_sensor_.print(" sensor pose in body frame: ");
this->body_P_sensor_->print(" sensor pose in body frame: ");
Base::print("", keyFormatter); Base::print("", keyFormatter);
} }

91
gtsam/slam/tests/testRangeFactor.cpp
View File

@ -34,14 +34,30 @@ static SharedNoiseModel model(noiseModel::Unit::Create(1));
typedef RangeFactor<Pose2, Point2> RangeFactor2D; typedef RangeFactor<Pose2, Point2> RangeFactor2D;
typedef RangeFactor<Pose3, Point3> RangeFactor3D; typedef RangeFactor<Pose3, Point3> RangeFactor3D;
typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;
typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D;
/* ************************************************************************* */ /* ************************************************************************* */
Vector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) { Vector factorError2D(const Pose2& pose, const Point2& point,
const RangeFactor2D& factor) {
return factor.evaluateError(pose, point); return factor.evaluateError(pose, point);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) { Vector factorError3D(const Pose3& pose, const Point3& point,
const RangeFactor3D& factor) {
return factor.evaluateError(pose, point);
}
/* ************************************************************************* */
Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point,
const RangeFactorWithTransform2D& factor) {
return factor.evaluateError(pose, point);
}
/* ************************************************************************* */
Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point,
const RangeFactorWithTransform3D& factor) {
return factor.evaluateError(pose, point); return factor.evaluateError(pose, point);
} }
@ -61,10 +77,13 @@ TEST( RangeFactor, ConstructorWithTransform) {
Key pointKey(2); Key pointKey(2);
double measurement(10.0); double measurement(10.0);
Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2); Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
RangeFactor2D factor2D(poseKey, pointKey, measurement, model, body_P_sensor_2D); RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model,
RangeFactor3D factor3D(poseKey, pointKey, measurement, model, body_P_sensor_3D); body_P_sensor_2D);
RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model,
body_P_sensor_3D);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -90,14 +109,19 @@ TEST( RangeFactor, EqualsWithTransform ) {
Key pointKey(2); Key pointKey(2);
double measurement(10.0); double measurement(10.0);
Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2); Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model, body_P_sensor_2D); RangeFactorWithTransform2D factor2D_1(poseKey, pointKey, measurement, model,
RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model, body_P_sensor_2D); body_P_sensor_2D);
RangeFactorWithTransform2D factor2D_2(poseKey, pointKey, measurement, model,
body_P_sensor_2D);
CHECK(assert_equal(factor2D_1, factor2D_2)); CHECK(assert_equal(factor2D_1, factor2D_2));
RangeFactor3D factor3D_1(poseKey, pointKey, measurement, model, body_P_sensor_3D); RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model,
RangeFactor3D factor3D_2(poseKey, pointKey, measurement, model, body_P_sensor_3D); body_P_sensor_3D);
RangeFactorWithTransform3D factor3D_2(poseKey, pointKey, measurement, model,
body_P_sensor_3D);
CHECK(assert_equal(factor3D_1, factor3D_2)); CHECK(assert_equal(factor3D_1, factor3D_2));
} }
@ -130,7 +154,8 @@ TEST( RangeFactor, Error2DWithTransform ) {
Key pointKey(2); Key pointKey(2);
double measurement(10.0); double measurement(10.0);
Pose2 body_P_sensor(0.25, -0.10, -M_PI_2); Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor); RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model,
body_P_sensor);
// Set the linearization point // Set the linearization point
Rot2 R(0.57); Rot2 R(0.57);
@ -176,8 +201,10 @@ TEST( RangeFactor, Error3DWithTransform ) {
Key poseKey(1); Key poseKey(1);
Key pointKey(2); Key pointKey(2);
double measurement(10.0); double measurement(10.0);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor); Point3(0.25, -0.10, 1.0));
RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
body_P_sensor);
// Set the linearization point // Set the linearization point
Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75); Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
@ -213,8 +240,10 @@ TEST( RangeFactor, Jacobian2D ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose); H1Expected = numericalDerivative11<Vector, Pose2>(
H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point); boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(
boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct // Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -228,7 +257,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
Key pointKey(2); Key pointKey(2);
double measurement(10.0); double measurement(10.0);
Pose2 body_P_sensor(0.25, -0.10, -M_PI_2); Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor); RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model,
body_P_sensor);
// Set the linearization point // Set the linearization point
Rot2 R(0.57); Rot2 R(0.57);
@ -242,8 +272,10 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose); H1Expected = numericalDerivative11<Vector, Pose2>(
H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point); boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(
boost::bind(&factorErrorWithTransform2D, pose, _1, factor), point);
// Verify the Jacobians are correct // Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -268,8 +300,10 @@ TEST( RangeFactor, Jacobian3D ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose); H1Expected = numericalDerivative11<Vector, Pose3>(
H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point); boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(
boost::bind(&factorError3D, pose, _1, factor), point);
// Verify the Jacobians are correct // Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -282,8 +316,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
Key poseKey(1); Key poseKey(1);
Key pointKey(2); Key pointKey(2);
double measurement(10.0); double measurement(10.0);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor); Point3(0.25, -0.10, 1.0));
RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
body_P_sensor);
// Set the linearization point // Set the linearization point
Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75); Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
@ -297,8 +333,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose); H1Expected = numericalDerivative11<Vector, Pose3>(
H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point); boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(
boost::bind(&factorErrorWithTransform3D, pose, _1, factor), point);
// Verify the Jacobians are correct // Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -306,6 +344,9 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */