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

69 lines
2.7 KiB
C
Raw Normal View History

File with common simulated data.
2013-08-30 12:23:45 +08:00
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
replaced/appended all calls to SimpleCamera with PinholeCameraCal3_S2
2020-02-22 08:42:55 +08:00
* @file SFMdata.h
renamed Visual SLAM -> SFM, reserve SLAM for problems where we have odometry
2013-10-18 13:31:55 +08:00
* @brief Simple example for the structure-from-motion problems
File with common simulated data.
2013-08-30 12:23:45 +08:00
* @author Duy-Nguyen Ta
*/
/**
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.
2019-01-04 23:17:33 +08:00
* A structure-from-motion example with landmarks, default function arguments give
File with common simulated data.
2013-08-30 12:23:45 +08:00
* - The landmarks form a 10 meter cube
* - The robot rotates around the landmarks, always facing towards the cube
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.
2019-01-04 23:17:33 +08:00
* Passing function argument allows to specificy an initial position, a pose increment and step count.
File with common simulated data.
2013-08-30 12:23:45 +08:00
*/
// As this is a full 3D problem, we will use Pose3 variables to represent the camera
// positions and Point3 variables (x, y, z) to represent the landmark coordinates.
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
// We will also need a camera object to hold calibration information and perform projections.
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
// We will also need a camera object to hold calibration information and perform projections.
replaced/appended all calls to SimpleCamera with PinholeCameraCal3_S2
2020-02-22 08:42:55 +08:00
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
File with common simulated data.
2013-08-30 12:23:45 +08:00
/* ************************************************************************* */
std::vector<gtsam::Point3> createPoints() {
// Create the set of ground-truth landmarks
std::vector<gtsam::Point3> points;
points.push_back(gtsam::Point3(10.0,10.0,10.0));
points.push_back(gtsam::Point3(-10.0,10.0,10.0));
points.push_back(gtsam::Point3(-10.0,-10.0,10.0));
points.push_back(gtsam::Point3(10.0,-10.0,10.0));
points.push_back(gtsam::Point3(10.0,10.0,-10.0));
points.push_back(gtsam::Point3(-10.0,10.0,-10.0));
points.push_back(gtsam::Point3(-10.0,-10.0,-10.0));
points.push_back(gtsam::Point3(10.0,-10.0,-10.0));
return points;
}
/* ************************************************************************* */
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.
2019-01-04 23:17:33 +08:00
std::vector<gtsam::Pose3> createPoses(
const gtsam::Pose3& init = gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2,0,-M_PI/2), gtsam::Point3(30, 0, 0)),
remove trailing spaces
2019-02-11 22:39:48 +08:00
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)))),
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.
2019-01-04 23:17:33 +08:00
int steps = 8) {
remove trailing spaces
2019-02-11 22:39:48 +08:00
File with common simulated data.
2013-08-30 12:23:45 +08:00
// Create the set of ground-truth poses
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.
2019-01-04 23:17:33 +08:00
// Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center
File with common simulated data.
2013-08-30 12:23:45 +08:00
std::vector<gtsam::Pose3> poses;
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.
2019-01-04 23:17:33 +08:00
int i = 1;
poses.push_back(init);
for(; i < steps; ++i) {
poses.push_back(poses[i-1].compose(delta));
File with common simulated data.
2013-08-30 12:23:45 +08:00
}
remove trailing spaces
2019-02-11 22:39:48 +08:00
File with common simulated data.
2013-08-30 12:23:45 +08:00
return poses;
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.
2019-01-04 23:17:33 +08:00
}