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

Added optional sensor to body transformation in the stereo factor

release/4.3a0
Stephen Williams 2012-11-01 15:38:59 +00:00
parent 46c1d0c512
commit 65a28b751d
2 changed files with 168 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
gtsam/slam/StereoFactor.h
View File

@ -34,6 +34,7 @@ private:
// Keep a copy of measurement and calibration for I/O
StereoPoint2 measured_; ///< the measurement
boost::shared_ptr<Cal3_S2Stereo> K_; ///< shared pointer to calibration
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
public:
@ -56,8 +57,10 @@ public:
* @param landmarkKey the landmark variable key
* @param K the constant calibration
*/
GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, const shared_ptrKStereo& K) :
Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {
GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model,
Key poseKey, Key landmarkKey, const shared_ptrKStereo& K,
boost::optional<POSE> body_P_sensor = boost::none) :
Base(model, poseKey, landmarkKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor) {
}
virtual ~GenericStereoFactor() {} ///< Virtual destructor
@ -75,22 +78,40 @@ public:
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
/**
* equals
*/
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
const GenericStereoFactor* p = dynamic_cast<const GenericStereoFactor*> (&f);
return p && Base::equals(f) && measured_.equals(p->measured_, tol);
const GenericStereoFactor* e = dynamic_cast<const GenericStereoFactor*> (&f);
return e
&& Base::equals(f)
&& measured_.equals(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_)));
}
/** h(x)-z */
Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
try {
StereoCamera stereoCam(pose, K_);
return (stereoCam.project(point, H1, H2) - measured_).vector();
if(body_P_sensor_) {
if(H1) {
gtsam::Matrix H0;
StereoCamera stereoCam(pose.compose(*body_P_sensor_, H0), K_);
StereoPoint2 reprojectionError(stereoCam.project(point, H1, H2) - measured_);
*H1 = *H1 * H0;
return reprojectionError.vector();
} else {
StereoCamera stereoCam(pose.compose(*body_P_sensor_), K_);
return (stereoCam.project(point, H1, H2) - measured_).vector();
}
} else {
StereoCamera stereoCam(pose, K_);
return (stereoCam.project(point, H1, H2) - measured_).vector();
}
} catch(StereoCheiralityException& e) {
if (H1) *H1 = zeros(3,6);
if (H2) *H2 = zeros(3,3);
@ -119,6 +140,7 @@ private:
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(K_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
};

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

@ -42,12 +42,148 @@ static boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 24
// point X Y Z in meters
static Point3 p(0, 0, 5);
static SharedNoiseModel sigma(noiseModel::Unit::Create(3));
static SharedNoiseModel model(noiseModel::Unit::Create(3));
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
typedef GenericStereoFactor<Pose3, Point3> TestStereoFactor;
/* ************************************************************************* */
TEST( StereoFactor, Constructor) {
StereoPoint2 measurement(323, 318-50, 241);
TestStereoFactor factor(measurement, model, X(1), L(1), K);
}
/* ************************************************************************* */
TEST( StereoFactor, ConstructorWithTransform) {
StereoPoint2 measurement(323, 318-50, 241);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
TestStereoFactor factor(measurement, model, X(1), L(1), K, body_P_sensor);
}
/* ************************************************************************* */
TEST( StereoFactor, Equals ) {
// Create two identical factors and make sure they're equal
StereoPoint2 measurement(323, 318-50, 241);
TestStereoFactor factor1(measurement, model, X(1), L(1), K);
TestStereoFactor factor2(measurement, model, X(1), L(1), K);
CHECK(assert_equal(factor1, factor2));
}
/* ************************************************************************* */
TEST( StereoFactor, EqualsWithTransform ) {
// Create two identical factors and make sure they're equal
StereoPoint2 measurement(323, 318-50, 241);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
TestStereoFactor factor1(measurement, model, X(1), L(1), K, body_P_sensor);
TestStereoFactor factor2(measurement, model, X(1), L(1), K, body_P_sensor);
CHECK(assert_equal(factor1, factor2));
}
/* ************************************************************************* */
TEST( StereoFactor, Error ) {
// Create the factor with a measurement that is 3 pixels off in x
StereoPoint2 measurement(323, 318-50, 241);
TestStereoFactor factor(measurement, model, X(1), L(1), K);
// Set the linearization point
Pose3 pose(Rot3(), Point3(0.0, 0.0, -6.25));
Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose, point));
// The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance
Vector expectedError = Vector_(3, -3.0, +2.0, -1.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( StereoFactor, ErrorWithTransform ) {
// Create the factor with a measurement that is 3 pixels off in x
StereoPoint2 measurement(323, 318-50, 241);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
TestStereoFactor factor(measurement, model, X(1), L(1), K, body_P_sensor);
// Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
Pose3 pose(Rot3(), Point3(-6.50, 0.10 , -1.0));
Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose, point));
// The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance
Vector expectedError = Vector_(3, -3.0, +2.0, -1.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( StereoFactor, Jacobian ) {
// Create the factor with a measurement that is 3 pixels off in x
StereoPoint2 measurement(323, 318-50, 241);
TestStereoFactor factor(measurement, model, X(1), L(1), K);
// Set the linearization point
Pose3 pose(Rot3(), Point3(0.0, 0.0, -6.25));
Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
// The expected Jacobians
Matrix H1Expected = Matrix_(3, 6, 0.0, -625.0, 0.0, -100.0, 0.0, 0.0,
0.0, -625.0, 0.0, -100.0, 0.0, -8.0,
625.0, 0.0, 0.0, 0.0, -100.0, 0.0);
Matrix H2Expected = Matrix_(3, 3, 100.0, 0.0, 0.0,
100.0, 0.0, 8.0,
0.0, 100.0, 0.0);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
CHECK(assert_equal(H2Expected, H2Actual, 1e-3));
}
/* ************************************************************************* */
TEST( StereoFactor, JacobianWithTransform ) {
// Create the factor with a measurement that is 3 pixels off in x
StereoPoint2 measurement(323, 318-50, 241);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
TestStereoFactor factor(measurement, model, X(1), L(1), K, body_P_sensor);
// Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
Pose3 pose(Rot3(), Point3(-6.50, 0.10 , -1.0));
Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
// The expected Jacobians
Matrix H1Expected = Matrix_(3, 6, -100.0, 0.0, 650.0, 0.0, 100.0, 0.0,
-100.0, -8.0, 649.2, -8.0, 100.0, 0.0,
-10.0, -650.0, 0.0, 0.0, 0.0, 100.0);
Matrix H2Expected = Matrix_(3, 3, 0.0, -100.0, 0.0,
8.0, -100.0, 0.0,
0.0, 0.0, -100.0);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
CHECK(assert_equal(H2Expected, H2Actual, 1e-3));
}
/* ************************************************************************* */
TEST( StereoFactor, singlePoint)
{
@ -55,9 +191,9 @@ TEST( StereoFactor, singlePoint)
graph.add(NonlinearEquality<Pose3>(X(1), camera1));
StereoPoint2 z14(320, 320.0-50, 240);
StereoPoint2 measurement(320, 320.0-50, 240);
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
graph.add(GenericStereoFactor<Pose3, Point3>(z14, sigma, X(1), L(1), K));
graph.add(GenericStereoFactor<Pose3, Point3>(measurement, model, X(1), L(1), K));
// Create a configuration corresponding to the ground truth
Values values;