Fixed RangeFactor
dellaert
parent
add93f19a6
commit
f7ebe4bfc4
120
.cproject
120
.cproject
|
|
@ -592,6 +592,7 @@
|
|||
</target>
|
||||
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testBayesTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -599,6 +600,7 @@
|
|||
</target>
|
||||
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBinaryBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -646,6 +648,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -653,6 +656,7 @@
|
|||
</target>
|
||||
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -660,6 +664,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -675,6 +680,7 @@
|
|||
</target>
|
||||
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testBayesTree</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -848,6 +854,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -1122,6 +1136,7 @@
|
|||
</target>
|
||||
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testErrors.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1351,6 +1366,46 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1433,7 +1488,6 @@
|
|||
</target>
|
||||
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated2DOriented.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1473,7 +1527,6 @@
|
|||
</target>
|
||||
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated2D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1481,7 +1534,6 @@
|
|||
</target>
|
||||
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated3D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1495,46 +1547,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -1792,6 +1804,7 @@
|
|||
</target>
|
||||
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>-G DEB</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1799,6 +1812,7 @@
|
|||
</target>
|
||||
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>-G RPM</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1806,6 +1820,7 @@
|
|||
</target>
|
||||
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>-G TGZ</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1813,6 +1828,7 @@
|
|||
</target>
|
||||
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2627,6 +2643,7 @@
|
|||
</target>
|
||||
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2634,18 +2651,12 @@
|
|||
</target>
|
||||
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testJunctionTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -2902,6 +2913,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -3184,7 +3203,6 @@
|
|||
</target>
|
||||
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testGaussianISAM2</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
|
|||
|
|
@ -26,12 +26,11 @@ namespace gtsam {
|
|||
* Binary factor for a range measurement
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
template<class T1, class T2>
|
||||
template<class T1, class T2 = T1>
|
||||
class RangeFactor: public NoiseModelFactor2<T1, T2> {
|
||||
private:
|
||||
|
||||
double measured_; /** measurement */
|
||||
boost::optional<T1> body_P_sensor_; ///< The pose of the sensor in the body frame
|
||||
|
||||
typedef RangeFactor<T1, T2> This;
|
||||
typedef NoiseModelFactor2<T1, T2> Base;
|
||||
|
|
@ -45,10 +44,8 @@ public:
|
|||
} /* Default constructor */
|
||||
|
||||
RangeFactor(Key key1, Key key2, double measured,
|
||||
const SharedNoiseModel& model, boost::optional<T1> body_P_sensor =
|
||||
boost::none) :
|
||||
Base(model, key1, key2), measured_(measured), body_P_sensor_(
|
||||
body_P_sensor) {
|
||||
const SharedNoiseModel& model) :
|
||||
Base(model, key1, key2), measured_(measured) {
|
||||
}
|
||||
|
||||
virtual ~RangeFactor() {
|
||||
|
|
@ -64,16 +61,92 @@ public:
|
|||
Vector evaluateError(const T1& t1, const T2& t2, boost::optional<Matrix&> H1 =
|
||||
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
|
||||
double hx;
|
||||
if (body_P_sensor_) {
|
||||
hx = t1.range(t2, H1, H2);
|
||||
return (Vector(1) << hx - measured_).finished();
|
||||
}
|
||||
|
||||
/** return the measured */
|
||||
double measured() const {
|
||||
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);
|
||||
hx = t1.compose(body_P_sensor_, H0).range(t2, H1, H2);
|
||||
*H1 = *H1 * H0;
|
||||
} else {
|
||||
hx = t1.compose(*body_P_sensor_).range(t2, H1, H2);
|
||||
}
|
||||
} else {
|
||||
hx = t1.range(t2, H1, H2);
|
||||
hx = t1.compose(body_P_sensor_).range(t2, H1, H2);
|
||||
}
|
||||
return (Vector(1) << hx - measured_).finished();
|
||||
}
|
||||
|
|
@ -89,17 +162,14 @@ public:
|
|||
const This *e = dynamic_cast<const This*>(&expected);
|
||||
return e != NULL && Base::equals(*e, tol)
|
||||
&& fabs(this->measured_ - e->measured_) < tol
|
||||
&& ((!body_P_sensor_ && !e->body_P_sensor_)
|
||||
|| (body_P_sensor_ && e->body_P_sensor_
|
||||
&& body_P_sensor_->equals(*e->body_P_sensor_)));
|
||||
&& body_P_sensor_.equals(e->body_P_sensor_);
|
||||
}
|
||||
|
||||
/** print contents */
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const {
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -34,14 +34,30 @@ static SharedNoiseModel model(noiseModel::Unit::Create(1));
|
|||
|
||||
typedef RangeFactor<Pose2, Point2> RangeFactor2D;
|
||||
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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -61,10 +77,13 @@ TEST( RangeFactor, ConstructorWithTransform) {
|
|||
Key pointKey(2);
|
||||
double measurement(10.0);
|
||||
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);
|
||||
RangeFactor3D factor3D(poseKey, pointKey, measurement, model, body_P_sensor_3D);
|
||||
RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model,
|
||||
body_P_sensor_2D);
|
||||
RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model,
|
||||
body_P_sensor_3D);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -90,14 +109,19 @@ TEST( RangeFactor, EqualsWithTransform ) {
|
|||
Key pointKey(2);
|
||||
double measurement(10.0);
|
||||
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);
|
||||
RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model, body_P_sensor_2D);
|
||||
RangeFactorWithTransform2D factor2D_1(poseKey, pointKey, measurement, model,
|
||||
body_P_sensor_2D);
|
||||
RangeFactorWithTransform2D factor2D_2(poseKey, pointKey, measurement, model,
|
||||
body_P_sensor_2D);
|
||||
CHECK(assert_equal(factor2D_1, factor2D_2));
|
||||
|
||||
RangeFactor3D factor3D_1(poseKey, pointKey, measurement, model, body_P_sensor_3D);
|
||||
RangeFactor3D factor3D_2(poseKey, pointKey, measurement, model, body_P_sensor_3D);
|
||||
RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model,
|
||||
body_P_sensor_3D);
|
||||
RangeFactorWithTransform3D factor3D_2(poseKey, pointKey, measurement, model,
|
||||
body_P_sensor_3D);
|
||||
CHECK(assert_equal(factor3D_1, factor3D_2));
|
||||
}
|
||||
|
||||
|
|
@ -130,7 +154,8 @@ TEST( RangeFactor, Error2DWithTransform ) {
|
|||
Key pointKey(2);
|
||||
double measurement(10.0);
|
||||
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
|
||||
Rot2 R(0.57);
|
||||
|
|
@ -176,8 +201,10 @@ TEST( RangeFactor, Error3DWithTransform ) {
|
|||
Key poseKey(1);
|
||||
Key pointKey(2);
|
||||
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));
|
||||
RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor);
|
||||
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
|
||||
Point3(0.25, -0.10, 1.0));
|
||||
RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
|
||||
body_P_sensor);
|
||||
|
||||
// Set the linearization point
|
||||
Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
|
||||
|
|
@ -213,8 +240,10 @@ TEST( RangeFactor, Jacobian2D ) {
|
|||
|
||||
// Use numerical derivatives to calculate the Jacobians
|
||||
Matrix H1Expected, H2Expected;
|
||||
H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
|
||||
H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
|
||||
H1Expected = numericalDerivative11<Vector, Pose2>(
|
||||
boost::bind(&factorError2D, _1, point, factor), pose);
|
||||
H2Expected = numericalDerivative11<Vector, Point2>(
|
||||
boost::bind(&factorError2D, pose, _1, factor), point);
|
||||
|
||||
// Verify the Jacobians are correct
|
||||
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
|
||||
|
|
@ -228,7 +257,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
|
|||
Key pointKey(2);
|
||||
double measurement(10.0);
|
||||
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
|
||||
Rot2 R(0.57);
|
||||
|
|
@ -242,8 +272,10 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
|
|||
|
||||
// Use numerical derivatives to calculate the Jacobians
|
||||
Matrix H1Expected, H2Expected;
|
||||
H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
|
||||
H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
|
||||
H1Expected = numericalDerivative11<Vector, Pose2>(
|
||||
boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose);
|
||||
H2Expected = numericalDerivative11<Vector, Point2>(
|
||||
boost::bind(&factorErrorWithTransform2D, pose, _1, factor), point);
|
||||
|
||||
// Verify the Jacobians are correct
|
||||
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
|
||||
|
|
@ -268,8 +300,10 @@ TEST( RangeFactor, Jacobian3D ) {
|
|||
|
||||
// Use numerical derivatives to calculate the Jacobians
|
||||
Matrix H1Expected, H2Expected;
|
||||
H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
|
||||
H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
|
||||
H1Expected = numericalDerivative11<Vector, Pose3>(
|
||||
boost::bind(&factorError3D, _1, point, factor), pose);
|
||||
H2Expected = numericalDerivative11<Vector, Point3>(
|
||||
boost::bind(&factorError3D, pose, _1, factor), point);
|
||||
|
||||
// Verify the Jacobians are correct
|
||||
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
|
||||
|
|
@ -282,8 +316,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
|
|||
Key poseKey(1);
|
||||
Key pointKey(2);
|
||||
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));
|
||||
RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor);
|
||||
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
|
||||
Point3(0.25, -0.10, 1.0));
|
||||
RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
|
||||
body_P_sensor);
|
||||
|
||||
// Set the linearization point
|
||||
Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
|
||||
|
|
@ -297,8 +333,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
|
|||
|
||||
// Use numerical derivatives to calculate the Jacobians
|
||||
Matrix H1Expected, H2Expected;
|
||||
H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
|
||||
H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
|
||||
H1Expected = numericalDerivative11<Vector, Pose3>(
|
||||
boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose);
|
||||
H2Expected = numericalDerivative11<Vector, Point3>(
|
||||
boost::bind(&factorErrorWithTransform3D, pose, _1, factor), point);
|
||||
|
||||
// Verify the Jacobians are correct
|
||||
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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue