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

Slight modernization

release/4.3a0
Frank Dellaert 2024-10-24 11:43:40 -07:00
parent 8a0ab6e094
commit 00fc2ecc2b
1 changed files with 50 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
examples/SFMdata.h
View File

@ -16,56 +16,74 @@
*/
/**
* A structure-from-motion example with landmarks, default function arguments give
* A structure-from-motion example with landmarks, default 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.
* Passing function argument allows to specify an initial position, a pose
* increment and step count.
*/
#pragma once
// 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>
// 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).
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
// We will also need a camera object to hold calibration information and perform projections.
#include <gtsam/geometry/PinholeCamera.h>
// We will also need a camera object to hold calibration information and perform
// projections.
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
/* ************************************************************************* */
std::vector<gtsam::Point3> createPoints() {
namespace gtsam {
// 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));
/// Create a set of ground-truth landmarks
std::vector<Point3> createPoints() {
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<gtsam::Pose3> 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 a set of ground-truth poses
* Default values give a circular trajectory, radius 30 at pi/4 intervals,
* always facing the circle center
*/
std::vector<Pose3> createPoses(
const Pose3& init = Pose3(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {30, 0, 0}),
const Pose3& delta = Pose3(Rot3::Ypr(0, -M_PI_4, 0),
{sin(M_PI_4) * 30, 0, 30 * (1 - sin(M_PI_4))}),
int steps = 8) {
std::vector<Pose3> poses;
poses.reserve(steps);
// 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<gtsam::Pose3> poses;
int i = 1;
poses.push_back(init);
for(; i < steps; ++i) {
poses.push_back(poses[i-1].compose(delta));
for (int i = 1; i < steps; ++i) {
poses.push_back(poses[i - 1].compose(delta));
}
return poses;
}
/**
* Create regularly spaced poses with specified radius and number of cameras
*/
std::vector<Pose3> posesOnCircle(int num_cameras = 8, double radius = 30) {
const Pose3 init(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {radius, 0, 0});
const double theta = M_PI / num_cameras;
const Pose3 delta(
Rot3::Ypr(0, -2 * theta, 0),
{sin(2 * theta) * radius, 0, radius * (1 - sin(2 * theta))});
return createPoses(init, delta, num_cameras);
}
} // namespace gtsam