123 lines
5.8 KiB
C++
123 lines
5.8 KiB
C++
/**
|
|
* @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
|
|
*/
|
|
|
|
#include <gtsam/inference/Symbol.h>
|
|
#include <gtsam/geometry/BearingRange.h>
|
|
#include <gtsam/slam/expressions.h>
|
|
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
|
|
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
|
#include <gtsam/nonlinear/Values.h>
|
|
|
|
using namespace gtsam;
|
|
|
|
typedef BearingRange<Pose3, Point3> 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<Point3> createPoints() {
|
|
|
|
// Create the set of ground-truth landmarks
|
|
std::vector<Point3> 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<Pose3> createPoses(Pose3 delta, int steps) {
|
|
|
|
// Create the set of ground-truth poses
|
|
std::vector<Pose3> 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());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);nice
|
|
Pose3_ body_T_sensor_('T', 0);
|
|
// 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>( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j));
|
|
// 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 to the graph
|
|
if (i > 0)
|
|
{
|
|
// 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);
|
|
}
|
|
}
|
|
|
|
// 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<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
|
|
|
|
// initialize body_T_sensor wrongly (because we do not know!)
|
|
initial.insert<Pose3>(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<Pose3>(Symbol('T',0)).print("\nInitial estimate body_T_sensor\n"); /* initial sensor_P_body estimate */
|
|
result.at<Pose3>(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;
|
|
}
|
|
/* ************************************************************************* */ |