From d4398fb0928d3fafddac28d758dceb7219cb7a7e Mon Sep 17 00:00:00 2001 From: Thomas Horstink Date: Fri, 4 Jan 2019 11:31:41 +0100 Subject: [PATCH 1/6] expression example of estimating trajectory, landmarks and sensor-body-transform simultaneously --- ...leExpressions_BearinRangeWithTransform.cpp | 122 ++++++++++++++++++ 1 file changed, 122 insertions(+) create mode 100644 examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp diff --git a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp new file mode 100644 index 000000000..a78328c48 --- /dev/null +++ b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp @@ -0,0 +1,122 @@ +/** + * @file Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp + * @brief A simultanious optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions + * @author Thomas Horstink + * @date January 4th, 2019 + */ + +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +typedef BearingRange BearingRange3D; + +// These functions are very similar to those in the SFM example, except that it you can give it a fixed delta in between poses. +/* ************************************************************************* */ +std::vector createPoints() { + + // Create the set of ground-truth landmarks + std::vector points; + points.push_back(Point3(10.0,10.0,10.0)); + points.push_back(Point3(-10.0,10.0,10.0)); + points.push_back(Point3(-10.0,-10.0,10.0)); + points.push_back(Point3(10.0,-10.0,10.0)); + points.push_back(Point3(10.0,10.0,-10.0)); + points.push_back(Point3(-10.0,10.0,-10.0)); + points.push_back(Point3(-10.0,-10.0,-10.0)); + points.push_back(Point3(10.0,-10.0,-10.0)); + + return points; +} + +/* ************************************************************************* */ +std::vector createPoses(Pose3 delta, int steps) { + + // Create the set of ground-truth poses + std::vector poses; + Pose3 pose = Pose3(); + int i = 0; + for(; i < steps; ++i) { + poses.push_back(pose); + pose = pose.compose(delta); + } + poses.push_back(pose); + + return poses; +} + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + // Move around so the whole state (including the sensor tf) is observable + Pose3 delta_pose1 = Pose3(Rot3().Yaw(2*M_PI/8).Pitch(M_PI/8), Point3(1, 0, 0)); + Pose3 delta_pose2 = Pose3(Rot3().Pitch(-M_PI/8), Point3(1, 0, 0)); + Pose3 delta_pose3 = Pose3(Rot3().Yaw(-2*M_PI/8), Point3(1, 0, 0)); + + int steps = 4; + auto poses = createPoses(delta_pose1, steps); + auto poses2 = createPoses(delta_pose2, steps); + auto poses3 = createPoses(delta_pose3, steps); + + // concatenate poses to create trajectory + poses.insert( poses.end(), poses2.begin(), poses2.end() ); + poses.insert( poses.end(), poses3.begin(), poses3.end() ); // std::vector of Pose3 + auto points = createPoints(); // std::vector of Point3 + + // (ground-truth) sensor pose in body frame, further an unknown variable + Pose3 body_T_sensor_gt(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + // a graph + ExpressionFactorGraph graph; + // Specify uncertainty on first pose prior and also for between factor (simplicity reasons) + auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished()); + // Uncertainty bearing range measurement; + auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished()); + // Expressions for body-frame at key 0 and sensor-tf + Pose3_ x_('x', 0); + Pose3_ body_T_sensor_('T', 0); + // add a prior on the body-pose and sensor-tf. + graph.addExpressionFactor(x_, poses[0], poseNoise); + // Simulated measurements from pose, adding them to the factor graph + for (size_t i = 0; i < poses.size(); ++i) { + auto world_T_sensor = poses[i].compose(body_T_sensor_gt); + for (size_t j = 0; j < points.size(); ++j) { + // Create the expression + auto prediction = Expression( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j)); + // Create a *perfect* measurement + auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j])); + graph.addExpressionFactor(prediction, measurement, bearingRangeNoise); + } + // and add a between factor + if (i > 0) + { + // And also we have a nice measurement for the between factor. + graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise); + } + } + + // Create perturbed initial + Values initial; + Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + for (size_t i = 0; i < poses.size(); ++i) + initial.insert(Symbol('x', i), poses[i].compose(delta)); + for (size_t j = 0; j < points.size(); ++j) + initial.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); + + // initialize body_T_sensor wrongly (because we do not know!) + initial.insert(Symbol('T',0), Pose3()); + + std::cout << "initial error: " << graph.error(initial) << std::endl; + Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + std::cout << "final error: " << graph.error(result) << std::endl; + + initial.at(Symbol('T',0)).print("\nInitial estimate body_T_sensor\n"); /* initial sensor_P_body estimate */ + result.at(Symbol('T',0)).print("\nFinal estimate body_T_sensor\n"); /* optimized sensor_P_body estimate */ + body_T_sensor_gt.print("\nGround truth body_T_sensor\n"); /* sensor_P_body ground truth */ + + return 0; +} +/* ************************************************************************* */ \ No newline at end of file From 7bb6863e7567a4f72d1f011d3505ca1339e62112 Mon Sep 17 00:00:00 2001 From: Thomas Horstink Date: Fri, 4 Jan 2019 11:50:20 +0100 Subject: [PATCH 2/6] little typo in a comment --- .../Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp index a78328c48..69409e87b 100644 --- a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp +++ b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp @@ -16,7 +16,7 @@ using namespace gtsam; typedef BearingRange BearingRange3D; -// These functions are very similar to those in the SFM example, except that it you can give it a fixed delta in between poses. +// These functions are very similar to those in the SFM example, except that you can give it a fixed delta in between poses for n steps. /* ************************************************************************* */ std::vector createPoints() { From 986346f2b9c43918a9d4f626f4f5f0859e25dd3d Mon Sep 17 00:00:00 2001 From: Thomas Horstink Date: Fri, 4 Jan 2019 11:53:50 +0100 Subject: [PATCH 3/6] another comment update --- ...xampleExpressions_BearinRangeWithTransform.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp index 69409e87b..6ba7caca3 100644 --- a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp +++ b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp @@ -72,28 +72,29 @@ int main(int argc, char* argv[]) { // a graph ExpressionFactorGraph graph; // Specify uncertainty on first pose prior and also for between factor (simplicity reasons) - auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished()); + auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished());ExpressiExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampon examp // Uncertainty bearing range measurement; auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished()); // Expressions for body-frame at key 0 and sensor-tf - Pose3_ x_('x', 0); + Pose3_ x_('x', 0);nice Pose3_ body_T_sensor_('T', 0); - // add a prior on the body-pose and sensor-tf. + // add a prior on the body-pose. graph.addExpressionFactor(x_, poses[0], poseNoise); - // Simulated measurements from pose, adding them to the factor graph + // Simulated measurements from pose for (size_t i = 0; i < poses.size(); ++i) { auto world_T_sensor = poses[i].compose(body_T_sensor_gt); for (size_t j = 0; j < points.size(); ++j) { // Create the expression auto prediction = Expression( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j)); - // Create a *perfect* measurement + // Create a *perfect* measurementExpression exampExpreExpression exampExpression exampExpression exampssion examp auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j])); + // Add factor graph.addExpressionFactor(prediction, measurement, bearingRangeNoise); } - // and add a between factor + // and add a between factor to the graph if (i > 0) { - // And also we have a nice measurement for the between factor. + // And also we have a *perfect* measurement for the between factor. graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise); } } From ba03b398f4d0c6290de1d6cbaac85dbed9726b6e Mon Sep 17 00:00:00 2001 From: Thomas Horstink Date: Fri, 4 Jan 2019 11:55:26 +0100 Subject: [PATCH 4/6] type in filename.... --- ...> Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename examples/{Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp => Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp} (98%) diff --git a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp similarity index 98% rename from examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp rename to examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp index 6ba7caca3..8faa6b182 100644 --- a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp +++ b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp @@ -1,5 +1,5 @@ /** - * @file Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp + * @file Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp * @brief A simultanious optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions * @author Thomas Horstink * @date January 4th, 2019 From 9c382b6c144fd778dec1ecc81c166bfe15a69ffb Mon Sep 17 00:00:00 2001 From: Thomas Horstink Date: Fri, 4 Jan 2019 16:17:33 +0100 Subject: [PATCH 5/6] changed the SFMdata functions so that it allows the passage of function arguments to generate a trajectory; default arguments result in the original behaviour (described in header). In the range bearing examples: fixed weirdo text-artifacts, add newline for readability, added underscore the prediction expression. --- ...eExpressions_BearingRangeWithTransform.cpp | 73 +++++++------------ examples/SFMdata.h | 28 +++---- 2 files changed, 40 insertions(+), 61 deletions(-) diff --git a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp index 8faa6b182..39157a4f6 100644 --- a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp +++ b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp @@ -11,86 +11,65 @@ #include #include #include +#include using namespace gtsam; typedef BearingRange BearingRange3D; -// These functions are very similar to those in the SFM example, except that you can give it a fixed delta in between poses for n steps. -/* ************************************************************************* */ -std::vector createPoints() { - - // Create the set of ground-truth landmarks - std::vector points; - points.push_back(Point3(10.0,10.0,10.0)); - points.push_back(Point3(-10.0,10.0,10.0)); - points.push_back(Point3(-10.0,-10.0,10.0)); - points.push_back(Point3(10.0,-10.0,10.0)); - points.push_back(Point3(10.0,10.0,-10.0)); - points.push_back(Point3(-10.0,10.0,-10.0)); - points.push_back(Point3(-10.0,-10.0,-10.0)); - points.push_back(Point3(10.0,-10.0,-10.0)); - - return points; -} - -/* ************************************************************************* */ -std::vector createPoses(Pose3 delta, int steps) { - - // Create the set of ground-truth poses - std::vector poses; - Pose3 pose = Pose3(); - int i = 0; - for(; i < steps; ++i) { - poses.push_back(pose); - pose = pose.compose(delta); - } - poses.push_back(pose); - - return poses; -} - /* ************************************************************************* */ int main(int argc, char* argv[]) { + // Move around so the whole state (including the sensor tf) is observable + Pose3 init_pose = Pose3(); Pose3 delta_pose1 = Pose3(Rot3().Yaw(2*M_PI/8).Pitch(M_PI/8), Point3(1, 0, 0)); Pose3 delta_pose2 = Pose3(Rot3().Pitch(-M_PI/8), Point3(1, 0, 0)); Pose3 delta_pose3 = Pose3(Rot3().Yaw(-2*M_PI/8), Point3(1, 0, 0)); int steps = 4; - auto poses = createPoses(delta_pose1, steps); - auto poses2 = createPoses(delta_pose2, steps); - auto poses3 = createPoses(delta_pose3, steps); + auto poses = createPoses(init_pose, delta_pose1, steps); + auto poses2 = createPoses(init_pose, delta_pose2, steps); + auto poses3 = createPoses(init_pose, delta_pose3, steps); - // concatenate poses to create trajectory + // Concatenate poses to create trajectory poses.insert( poses.end(), poses2.begin(), poses2.end() ); poses.insert( poses.end(), poses3.begin(), poses3.end() ); // std::vector of Pose3 auto points = createPoints(); // std::vector of Point3 // (ground-truth) sensor pose in body frame, further an unknown variable Pose3 body_T_sensor_gt(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - // a graph + + // The graph ExpressionFactorGraph graph; + // Specify uncertainty on first pose prior and also for between factor (simplicity reasons) - auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished());ExpressiExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampon examp + auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished()); + // Uncertainty bearing range measurement; auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished()); + // Expressions for body-frame at key 0 and sensor-tf - Pose3_ x_('x', 0);nice + Pose3_ x_('x', 0); Pose3_ body_T_sensor_('T', 0); - // add a prior on the body-pose. + + // Add a prior on the body-pose graph.addExpressionFactor(x_, poses[0], poseNoise); + // Simulated measurements from pose for (size_t i = 0; i < poses.size(); ++i) { auto world_T_sensor = poses[i].compose(body_T_sensor_gt); for (size_t j = 0; j < points.size(); ++j) { - // Create the expression - auto prediction = Expression( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j)); - // Create a *perfect* measurementExpression exampExpreExpression exampExpression exampExpression exampssion examp + + // This expression is the key feature of this example: it creates a differentiable expression of the measurement after being displaced by sensor transform. + auto prediction_ = Expression( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j)); + + // Create a *perfect* measurement auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j])); + // Add factor - graph.addExpressionFactor(prediction, measurement, bearingRangeNoise); + graph.addExpressionFactor(prediction_, measurement, bearingRangeNoise); } + // and add a between factor to the graph if (i > 0) { @@ -107,7 +86,7 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) initial.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); - // initialize body_T_sensor wrongly (because we do not know!) + // Initialize body_T_sensor wrongly (because we do not know!) initial.insert(Symbol('T',0), Pose3()); std::cout << "initial error: " << graph.error(initial) << std::endl; diff --git a/examples/SFMdata.h b/examples/SFMdata.h index 25442d527..af1f761ee 100644 --- a/examples/SFMdata.h +++ b/examples/SFMdata.h @@ -16,9 +16,10 @@ */ /** - * A structure-from-motion example with landmarks + * A structure-from-motion example with landmarks, default function arguments give * - The landmarks form a 10 meter cube * - The robot rotates around the landmarks, always facing towards the cube + * Passing function argument allows to specificy an initial position, a pose increment and step count. */ // As this is a full 3D problem, we will use Pose3 variables to represent the camera @@ -49,20 +50,19 @@ std::vector createPoints() { } /* ************************************************************************* */ -std::vector createPoses() { - +std::vector createPoses( + const gtsam::Pose3& init = gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2,0,-M_PI/2), gtsam::Point3(30, 0, 0)), + const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))), + int steps = 8) { + // Create the set of ground-truth poses + // Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center std::vector poses; - double radius = 30.0; - int i = 0; - double theta = 0.0; - gtsam::Point3 up(0,0,1); - gtsam::Point3 target(0,0,0); - for(; i < 8; ++i, theta += 2*M_PI/8) { - gtsam::Point3 position = gtsam::Point3(radius*cos(theta), radius*sin(theta), 0.0); - gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(position, target, up); - poses.push_back(camera.pose()); + int i = 1; + poses.push_back(init); + for(; i < steps; ++i) { + poses.push_back(poses[i-1].compose(delta)); } + return poses; -} -/* ************************************************************************* */ +} \ No newline at end of file From e7d6cd4faf4ecfe1bbf1b247379a8cdd39d55b35 Mon Sep 17 00:00:00 2001 From: Thomas Horstink Date: Fri, 4 Jan 2019 17:12:04 +0100 Subject: [PATCH 6/6] fixed typo in description --- .../Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp index 39157a4f6..68a3fd7e7 100644 --- a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp +++ b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp @@ -1,6 +1,6 @@ /** * @file Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp - * @brief A simultanious optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions + * @brief A simultaneous optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions * @author Thomas Horstink * @date January 4th, 2019 */