From 9c382b6c144fd778dec1ecc81c166bfe15a69ffb Mon Sep 17 00:00:00 2001 From: Thomas Horstink Date: Fri, 4 Jan 2019 16:17:33 +0100 Subject: [PATCH] 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