diff --git a/cpp/UrbanMeasurement.cpp b/cpp/UrbanMeasurement.cpp index 7ae7f5d74..097c0b3bb 100644 --- a/cpp/UrbanMeasurement.cpp +++ b/cpp/UrbanMeasurement.cpp @@ -10,94 +10,80 @@ using namespace std; -// Notation: -// ph = phi = roll -// th = theta = pitch -// ps = psi = yaw -// JC: Need to confirm that RQ does the extraction -// that I think it's doing. That should shake out with -// test data. #define _GET_TEMPORARIES \ - Vector rpy = RQ(pose.rotation().matrix()); \ - double dx = p.x() - pose.translation().x(); \ - double dy = p.y() - pose.translation().y(); \ - double s23 = (*sensorMatrix)(2, 3); \ - double cps = cos(rpy[2]); \ - double sps = sin(rpy[2]); \ - double secph = 1/cos(rpy[0]); \ - double tph = tan(rpy[0]); \ - double sph = sin(rpy[0]); \ - double sth = sin(rpy[1]); \ - double cth = cos(rpy[1]); \ - double tth = tan(rpy[1]); \ - double secth = 1/cth namespace gtsam { /* ************************************************************************* */ - // JC: Clearly there is a lot of code overlap (and redundant calculations) between + // JC: I'd like to break this out into component calculations, but there's too much + // Clearly there is a lot of code overlap (and redundant calculations) between // this and the calculation of the relevant jacobians. Need to look at cleaning // this up when I have a better grasp of the larger implications. I don't think // there's any reason we can't just merge transform_to and the two derivative // functions into a single function to remove the overlap. + - Point2 transform_to(const boost::shared_ptr &sensorMatrix, - const Pose3& pose, const Point2& p) { - _GET_TEMPORARIES; + + + UrbanMeasurement::Transform transform_to(const boost::shared_ptr &sensorMatrix, + const Pose3& robotpose, const Point2& lmpos, + bool getJacobians) { + // Notation: + // ph = phi = roll + // th = theta = pitch + // ps = psi = yaw + Vector rpy = RQ(robotpose.rotation().matrix()); + double dx = lmpos.x() - robotpose.translation().x(); + double dy = lmpos.y() - robotpose.translation().y(); + double s23 = (*sensorMatrix)(2, 3); + double cps = cos(rpy[2]); + double sps = sin(rpy[2]); + double secph = 1/cos(rpy[0]); + double tph = tan(rpy[0]); + double sph = sin(rpy[0]); + double sth = sin(rpy[1]); + double cth = cos(rpy[1]); + double tth = tan(rpy[1]); + double secth = 1/cth; - return Point2(cth*dy*sps+(-s23*secph+dy*sps*sth+dx*sps*tph)*tth - +cps*(cth*dx+dx*sth*tth-dy*tph*tth), - secph*(cps*dy+s23*sph-dx*sps)); - - // JC: This code still here for my reference. -#if 0 - // create a 3D point at our height (Z is assumed up) - Point3 global3D(p.x(),p.y(),pose.translation().z()); - // transform into local 3D point - Point3 local3D = transform_to(pose,global3D); - // take x and y as the local measurement - Point2 local2D(local3D.x(),local3D.y()); - return local2D; -#endif - } - - /* ************************************************************************* */ - Matrix Dtransform_to1(const boost::shared_ptr &sensorMatrix, - const Pose3& pose, const Point2& p) { - _GET_TEMPORARIES; - Matrix ret(2, 6); - ret(0, 0) = (-cps*secth-sps*tph*tth); - ret(0, 1) = (-secth*sps+cps*tph*tth); - ret(0, 2) = 0; - ret(0, 3) = -((secph*secph*(cps*dy+s23*sph-dx*sps)*tth)); - ret(0, 4) = (-((secth*(secth*(s23*secph+(cps*dy-dx*sps)*tph) - +(-cps*dx-dy*sps)*tth)))); - ret(0, 5) = ((cth*(cps*dy-dx*sps)+(cps*(dy*sth+dx*tph) - +sps*(-dx*sth+dy*tph))*tth)); + UrbanMeasurement::Transform ret; + ret.get<0>().reset(new Point2(cth*dy*sps + + (-s23*secph+dy*sps*sth+dx*sps*tph)*tth + + cps*(cth*dx+dx*sth*tth-dy*tph*tth), + secph*(cps*dy+s23*sph-dx*sps))); - ret(1, 0) = secph*sps; - ret(1, 1) = -cps*secph; - ret(1, 2) = 0; - ret(1, 3) = secph*(s23*secph+(cps*dy-dx*sps)*tph); - ret(1, 4) = 0; - ret(1, 5) = secph*(-cps*dx-dy*sps); + + if (getJacobians) { + ret.get<1>().reset(new Matrix(2, 6)); + Matrix &D1 = *(ret.get<1>()); + D1(0, 0) = (-cps*secth-sps*tph*tth); + D1(0, 1) = (-secth*sps+cps*tph*tth); + D1(0, 2) = 0; + D1(0, 3) = -((secph*secph*(cps*dy+s23*sph-dx*sps)*tth)); + D1(0, 4) = (-((secth*(secth*(s23*secph+(cps*dy-dx*sps)*tph) + +(-cps*dx-dy*sps)*tth)))); + D1(0, 5) = ((cth*(cps*dy-dx*sps)+(cps*(dy*sth+dx*tph) + +sps*(-dx*sth+dy*tph))*tth)); + + D1(1, 0) = secph*sps; + D1(1, 1) = -cps*secph; + D1(1, 2) = 0; + D1(1, 3) = secph*(s23*secph+(cps*dy-dx*sps)*tph); + D1(1, 4) = 0; + D1(1, 5) = secph*(-cps*dx-dy*sps); + + + ret.get<2>().reset(new Matrix(2, 2)); + Matrix &D2 = *(ret.get<2>()); + D2(0, 0) = cps*secth+sps*tph*tth; + D2(0, 1) = secth*sps-cps*tph*tth; + D2(1, 0) = -secph*sps; + D2(1, 1) = cps*secph; + } return ret; } - /* ************************************************************************* */ - Matrix Dtransform_to2(const boost::shared_ptr &sensorMatrix, - const Pose3& pose, const Point2& p) { - _GET_TEMPORARIES; - Matrix ret(2, 2); - ret(0, 0) = cps*secth+sps*tph*tth; - ret(0, 1) = secth*sps-cps*tph*tth; - ret(1, 0) = -secph*sps; - ret(1, 1) = cps*secph; - return zeros(2, 2); - } - -#undef _GET_TEMPORARIES /* ************************************************************************* */ UrbanMeasurement::UrbanMeasurement() { @@ -152,8 +138,14 @@ namespace gtsam { Pose3 pose = x.robotPose(robotPoseNumber_); Point2 landmark = x.landmarkPoint(landmarkNumber_); // Right-hand-side b = (z - h(x))/sigma - Point2 hx = transform_to(sensorMatrix_, pose,landmark); - return (z_ - hx.vector()); +// fprintf(stderr, "p: %.16g l: %.16g\n", +// pose.rotation().vector()[0], +// landmark.x()); + boost::shared_ptr h = transform_to(sensorMatrix_, pose,landmark, false).get<0>(); +// fprintf(stderr, "H: %.16g %.16g\nz: %.16g %.16g\n", +// h->vector()[0], h->vector()[1], +// z_[0], z_[1]); + return (z_ - h->vector()); } /* ************************************************************************* */ @@ -165,8 +157,11 @@ namespace gtsam { // // What should the keys be? Presumably I just need to match a convention. // - // Should the jacobians be premultiplied at GuassianFactor - // construction time? + // Should the jacobians be premultiplied at the time of + // GaussianFactor construction? + // + // Is GaussianFactor copying its constructor arguments? (If not, this is + // not safe, nor is what this code replaced). // // Why is sigma a double instead of a matrix? @@ -179,12 +174,15 @@ namespace gtsam { string lmkey = k.str(); k.clear(); + Transform tr = transform_to(sensorMatrix_, pose,landmark, true); + + return GaussianFactor::shared_ptr( new GaussianFactor(posekey, - Dtransform_to1(sensorMatrix_, pose, landmark), + *(tr.get<1>()), lmkey, - Dtransform_to2(sensorMatrix_, pose, landmark), - error_vector(config), + *(tr.get<2>()), + z_ - tr.get<0>()->vector(), sigma_)); } } // namespace gtsam diff --git a/cpp/UrbanMeasurement.h b/cpp/UrbanMeasurement.h index dc8315481..4592386e2 100644 --- a/cpp/UrbanMeasurement.h +++ b/cpp/UrbanMeasurement.h @@ -24,7 +24,12 @@ namespace gtsam { //boost::shared_ptr K_; // Calibration stored in each factor. FD: need to think about this. typedef NonlinearFactor ConvenientFactor; + public: + // Return type for the transform_to function + typedef boost::tuple, + boost::shared_ptr, + boost::shared_ptr > Transform; typedef boost::shared_ptr shared_ptr; // shorthand for a smart pointer to a factor //typedef boost::shared_ptr shared_ptrK; @@ -92,11 +97,8 @@ namespace gtsam { * Transform 2D landmark into 6D pose, and its derivatives */ // JC: These are exported only for testbenches? - Point2 transform_to(const boost::shared_ptr &sensorMatrix, - const Pose3& pose, const Point2& p); - Matrix Dtransform_to1(const boost::shared_ptr &sensorMatrix, - const Pose3& pose, const Point2& p); - Matrix Dtransform_to2(const boost::shared_ptr &sensorMatrix, - const Pose3& pose); + UrbanMeasurement::Transform transform_to(const boost::shared_ptr &sensorMatrix, + const Pose3& robotpose, const Point2& lmpos, + bool getJacobians); } // namespace gtsam diff --git a/cpp/testUrbanMeasurement.cpp b/cpp/testUrbanMeasurement.cpp index 52557ff5c..2b4e7e42e 100644 --- a/cpp/testUrbanMeasurement.cpp +++ b/cpp/testUrbanMeasurement.cpp @@ -11,36 +11,84 @@ using namespace std; using namespace gtsam; -static Point2 landmark(2,5); +/* + * This unit test cross checks UrbanMeasurement with the + * results from Justin's thesis codebase. + */ + +static boost::shared_ptr sensorMatrix(new Matrix(4, 4)); + +static bool _data_inited = false; + +static void _init() { + // Not MT-safe. Don't think that's an issue for CppUnitLite + if (_data_inited) { + return; + } + (*sensorMatrix)(0, 0) = 0.0251; + (*sensorMatrix)(0, 1) = 0.9996; + (*sensorMatrix)(0, 2) = 0.0091; + (*sensorMatrix)(0, 3) = 1.5574; + (*sensorMatrix)(1, 0) = -0.9996; + (*sensorMatrix)(1, 1) = 0.0253; + (*sensorMatrix)(1, 2) = -0.016; + (*sensorMatrix)(1, 3) = -0.86; + (*sensorMatrix)(2, 0) = -0.0163; + (*sensorMatrix)(2, 1) = -0.0087; + (*sensorMatrix)(2, 2) = 0.9998; + (*sensorMatrix)(2, 3) = -0.6478; + (*sensorMatrix)(3, 0) = 0; + (*sensorMatrix)(3, 1) = 0; + (*sensorMatrix)(3, 2) = 0; + (*sensorMatrix)(3, 3) = 1; + _data_inited = true; + +} /* Robot is at (0,0,0) looking in global "y" direction. * For the local frame we used Navlab convention, * x-looks forward, y-looks right, z- down*/ -static Pose3 robotPose(Matrix_(3,3, - 0., 1., 0., - 1., 0., 0., - 0., 0.,-1. - ), - Point3(0,0,0)); +static const Pose3 robotPose(Matrix_(4, 4, + -0.0848808908061426, 0.993833753552749900, 0.0713421661796702400, 377.7509309858, + -0.9963883079644135, -0.08449313834814816, -0.008440931458908668, 1922.5523404841, + -0.002360959078213238, -0.07180097402774338, 0.9974161849503438, -6369417.591562273, + 0.0, 0.0, 0.0, 1)); + +static const Point2 landmark(362.707949418, 1936.2843137418); + -static boost::shared_ptr sensorMatrix(new Matrix(4, 4)); -// JC: FIXME: The tests of these tests are commented out until I add -// generate the test cases for the added code. /* ************************************************************************* */ TEST( UrbanMeasurement, transform_to ) { - Point2 expected(5,2); - Point2 actual = transform_to(sensorMatrix, robotPose, landmark); -// CHECK(assert_equal(expected,actual)); + _init(); + UrbanMeasurement::Transform tr = transform_to(sensorMatrix, robotPose, landmark, true); + + // Check the transformation from global to sensor coordinates + CHECK(assert_equal(Point2(-12.40679724375481, -16.14944758846734),*(tr.get<0>()))); + + Point2 &foo = *(tr.get<0>()); + + // Check the jacobian w.r.t. the pose + CHECK(assert_equal(Matrix_(2, 6, + 0.08471201853652961, 0.9964082882836194, 0.0, 0.03822695627705205, -0.5427133154140961, -16.15221468780303, + -0.998969460290058, 0.0851007754705408, 0.0, 0.5147498807397776, 0.0, 12.43765251715327), + *(tr.get<1>()))); + + // Check the jacobian w.r.t the landmark + CHECK(assert_equal(Matrix_(2, 2, + -0.08471201853652961, -0.9964082882836194, + 0.998969460290058, -0.0851007754705408), + *(tr.get<2>()))); } /* ************************************************************************* */ TEST( UrbanMeasurement, error_vector ) { - // Create a measurement, no-noise measurement would yield 5,2 - Point2 z(4,2); + _init(); + + Point2 z(-12.42876033350471, -16.10191021850896); double sigma = 0.2; UrbanMeasurement factor(sensorMatrix, z,sigma,44,66); @@ -50,14 +98,16 @@ TEST( UrbanMeasurement, error_vector ) config.addLandmark(66,landmark); // test error_vector - Vector expected = Vector_(2, -1.0, 0.0); + Vector expected = Vector_(2, + -.02196308974990, .04753736995838); Vector actual = factor.error_vector(config); -// CHECK(assert_equal(expected,actual)); + CHECK(assert_equal(expected,actual)); - // test error - double expected2 = 12.5; // 0.5 * 5^2 + // test error. Expected was generated via manual calculation from + // error vector. + double expected2 = .03427723560; double actual2 = factor.error(config); -// DOUBLES_EQUAL(expected2,actual2,1e-9); + DOUBLES_EQUAL(expected2,actual2,1e-9); } /* ************************************************************************* */