Merge pull request #1885 from borglab/feature/tripletFactor
TransferFactor with numerical derivativesrelease/4.3a0
Frank Dellaert
GitHub
commit
d48b1fc840
|
|
@ -39,7 +39,7 @@
|
|||
// Finally, once all of the factors have been added to our factor graph, we will want to
|
||||
// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
|
||||
// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
|
||||
// trust-region method known as Powell's Degleg
|
||||
// trust-region method known as Powell's Dogleg
|
||||
#include <gtsam/nonlinear/DoglegOptimizer.h>
|
||||
|
||||
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
|
||||
|
|
@ -57,7 +57,7 @@ using namespace gtsam;
|
|||
/* ************************************************************************* */
|
||||
int main(int argc, char* argv[]) {
|
||||
// Define the camera calibration parameters
|
||||
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
|
||||
auto K = std::make_shared<Cal3_S2>(50.0, 50.0, 0.0, 50.0, 50.0);
|
||||
|
||||
// Define the camera observation noise model
|
||||
auto measurementNoise =
|
||||
|
|
|
|||
|
|
@ -16,56 +16,89 @@
|
|||
*/
|
||||
|
||||
/**
|
||||
* 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 R = 30) {
|
||||
const double theta = 2 * M_PI / num_cameras;
|
||||
|
||||
// Initial pose at angle 0, position (R, 0, 0), facing the center with Y-axis
|
||||
// pointing down
|
||||
const Pose3 init(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {R, 0, 0});
|
||||
|
||||
// Delta rotation: rotate by -theta around Z-axis (counterclockwise movement)
|
||||
Rot3 delta_rotation = Rot3::Ypr(0, -theta, 0);
|
||||
|
||||
// Delta translation in world frame
|
||||
Vector3 delta_translation_world(R * (cos(theta) - 1), R * sin(theta), 0);
|
||||
|
||||
// Transform delta translation to local frame of the camera
|
||||
Vector3 delta_translation_local =
|
||||
init.rotation().inverse() * delta_translation_world;
|
||||
|
||||
// Define delta pose
|
||||
const Pose3 delta(delta_rotation, delta_translation_local);
|
||||
|
||||
// Generate poses using createPoses
|
||||
return createPoses(init, delta, num_cameras);
|
||||
}
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,136 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file ViewGraphExample.cpp
|
||||
* @brief View-graph calibration on a simulated dataset, a la Sweeney 2015
|
||||
* @author Frank Dellaert
|
||||
* @date October 2024
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/inference/EdgeKey.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/sfm/TransferFactor.h>
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "SFMdata.h"
|
||||
#include "gtsam/inference/Key.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(int argc, char* argv[]) {
|
||||
// Define the camera calibration parameters
|
||||
Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
|
||||
|
||||
// Create the set of 8 ground-truth landmarks
|
||||
vector<Point3> points = createPoints();
|
||||
|
||||
// Create the set of 4 ground-truth poses
|
||||
vector<Pose3> poses = posesOnCircle(4, 30);
|
||||
|
||||
// Calculate ground truth fundamental matrices, 1 and 2 poses apart
|
||||
auto F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K);
|
||||
auto F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K);
|
||||
|
||||
// Simulate measurements from each camera pose
|
||||
std::array<std::array<Point2, 8>, 4> p;
|
||||
for (size_t i = 0; i < 4; ++i) {
|
||||
PinholeCamera<Cal3_S2> camera(poses[i], K);
|
||||
for (size_t j = 0; j < 8; ++j) {
|
||||
p[i][j] = camera.project(points[j]);
|
||||
}
|
||||
}
|
||||
|
||||
// This section of the code is inspired by the work of Sweeney et al.
|
||||
// [link](sites.cs.ucsb.edu/~holl/pubs/Sweeney-2015-ICCV.pdf) on view-graph
|
||||
// calibration. The graph is made up of transfer factors that enforce the
|
||||
// epipolar constraint between corresponding points across three views, as
|
||||
// described in the paper. Rather than adding one ternary error term per point
|
||||
// in a triplet, we add three binary factors for sparsity during optimization.
|
||||
// In this version, we only include triplets between 3 successive cameras.
|
||||
NonlinearFactorGraph graph;
|
||||
using Factor = TransferFactor<FundamentalMatrix>;
|
||||
|
||||
for (size_t a = 0; a < 4; ++a) {
|
||||
size_t b = (a + 1) % 4; // Next camera
|
||||
size_t c = (a + 2) % 4; // Camera after next
|
||||
|
||||
// Vectors to collect tuples for each factor
|
||||
std::vector<std::tuple<Point2, Point2, Point2>> tuples1, tuples2, tuples3;
|
||||
|
||||
// Collect data for the three factors
|
||||
for (size_t j = 0; j < 8; ++j) {
|
||||
tuples1.emplace_back(p[a][j], p[b][j], p[c][j]);
|
||||
tuples2.emplace_back(p[a][j], p[c][j], p[b][j]);
|
||||
tuples3.emplace_back(p[c][j], p[b][j], p[a][j]);
|
||||
}
|
||||
|
||||
// Add transfer factors between views a, b, and c. Note that the EdgeKeys
|
||||
// are crucial in performing the transfer in the right direction. We use
|
||||
// exactly 8 unique EdgeKeys, corresponding to 8 unknown fundamental
|
||||
// matrices we will optimize for.
|
||||
graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(b, c), tuples1);
|
||||
graph.emplace_shared<Factor>(EdgeKey(a, b), EdgeKey(b, c), tuples2);
|
||||
graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(a, b), tuples3);
|
||||
}
|
||||
|
||||
auto formatter = [](Key key) {
|
||||
EdgeKey edge(key);
|
||||
return (std::string)edge;
|
||||
};
|
||||
|
||||
graph.print("Factor Graph:\n", formatter);
|
||||
|
||||
// Create a delta vector to perturb the ground truth
|
||||
// We can't really go far before convergence becomes problematic :-(
|
||||
Vector7 delta;
|
||||
delta << 1, 2, 3, 4, 5, 6, 7;
|
||||
delta *= 1e-5;
|
||||
|
||||
// Create the data structure to hold the initial estimate to the solution
|
||||
Values initialEstimate;
|
||||
for (size_t a = 0; a < 4; ++a) {
|
||||
size_t b = (a + 1) % 4; // Next camera
|
||||
size_t c = (a + 2) % 4; // Camera after next
|
||||
initialEstimate.insert(EdgeKey(a, b), F1.retract(delta));
|
||||
initialEstimate.insert(EdgeKey(a, c), F2.retract(delta));
|
||||
}
|
||||
initialEstimate.print("Initial Estimates:\n", formatter);
|
||||
graph.printErrors(initialEstimate, "errors: ", formatter);
|
||||
|
||||
/* Optimize the graph and print results */
|
||||
LevenbergMarquardtParams params;
|
||||
params.setlambdaInitial(1000.0); // Initialize lambda to a high value
|
||||
params.setVerbosityLM("SUMMARY");
|
||||
Values result =
|
||||
LevenbergMarquardtOptimizer(graph, initialEstimate, params).optimize();
|
||||
|
||||
cout << "initial error = " << graph.error(initialEstimate) << endl;
|
||||
cout << "final error = " << graph.error(result) << endl;
|
||||
|
||||
result.print("Final results:\n", formatter);
|
||||
|
||||
cout << "Ground Truth F1:\n" << F1.matrix() << endl;
|
||||
cout << "Ground Truth F2:\n" << F2.matrix() << endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -10,8 +10,8 @@
|
|||
namespace gtsam {
|
||||
|
||||
//*************************************************************************
|
||||
Point2 Transfer(const Matrix3& Fca, const Point2& pa, //
|
||||
const Matrix3& Fcb, const Point2& pb) {
|
||||
Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa, //
|
||||
const Matrix3& Fcb, const Point2& pb) {
|
||||
// Create lines in camera a from projections of the other two cameras
|
||||
Vector3 line_a = Fca * Vector3(pa.x(), pa.y(), 1);
|
||||
Vector3 line_b = Fcb * Vector3(pb.x(), pb.y(), 1);
|
||||
|
|
@ -24,6 +24,7 @@ Point2 Transfer(const Matrix3& Fca, const Point2& pa, //
|
|||
|
||||
return intersectionPoint.head<2>(); // Return the 2D point
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
FundamentalMatrix::FundamentalMatrix(const Matrix3& F) {
|
||||
// Perform SVD
|
||||
|
|
@ -71,9 +72,7 @@ Matrix3 FundamentalMatrix::matrix() const {
|
|||
}
|
||||
|
||||
void FundamentalMatrix::print(const std::string& s) const {
|
||||
std::cout << s << "U:\n"
|
||||
<< U_.matrix() << "\ns: " << s_ << "\nV:\n"
|
||||
<< V_.matrix() << std::endl;
|
||||
std::cout << s << matrix() << std::endl;
|
||||
}
|
||||
|
||||
bool FundamentalMatrix::equals(const FundamentalMatrix& other,
|
||||
|
|
@ -98,20 +97,20 @@ FundamentalMatrix FundamentalMatrix::retract(const Vector& delta) const {
|
|||
}
|
||||
|
||||
//*************************************************************************
|
||||
Matrix3 SimpleFundamentalMatrix::leftK() const {
|
||||
Matrix3 SimpleFundamentalMatrix::Ka() const {
|
||||
Matrix3 K;
|
||||
K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1;
|
||||
return K;
|
||||
}
|
||||
|
||||
Matrix3 SimpleFundamentalMatrix::rightK() const {
|
||||
Matrix3 SimpleFundamentalMatrix::Kb() const {
|
||||
Matrix3 K;
|
||||
K << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1;
|
||||
return K;
|
||||
}
|
||||
|
||||
Matrix3 SimpleFundamentalMatrix::matrix() const {
|
||||
return leftK().transpose().inverse() * E_.matrix() * rightK().inverse();
|
||||
return Ka().transpose().inverse() * E_.matrix() * Kb().inverse();
|
||||
}
|
||||
|
||||
void SimpleFundamentalMatrix::print(const std::string& s) const {
|
||||
|
|
|
|||
|
|
@ -54,6 +54,23 @@ class GTSAM_EXPORT FundamentalMatrix {
|
|||
*/
|
||||
FundamentalMatrix(const Matrix3& F);
|
||||
|
||||
/**
|
||||
* @brief Construct from calibration matrices Ka, Kb, and pose aPb
|
||||
*
|
||||
* Initializes the FundamentalMatrix from the given calibration
|
||||
* matrices Ka and Kb, and the pose aPb.
|
||||
*
|
||||
* @tparam CAL Calibration type, expected to have a matrix() method
|
||||
* @param Ka Calibration matrix for the left camera
|
||||
* @param aPb Pose from the left to the right camera
|
||||
* @param Kb Calibration matrix for the right camera
|
||||
*/
|
||||
template <typename CAL>
|
||||
FundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb)
|
||||
: FundamentalMatrix(Ka.K().transpose().inverse() *
|
||||
EssentialMatrix::FromPose3(aPb).matrix() *
|
||||
Kb.K().inverse()) {}
|
||||
|
||||
/// Return the fundamental matrix representation
|
||||
Matrix3 matrix() const;
|
||||
|
||||
|
|
@ -97,25 +114,32 @@ class GTSAM_EXPORT SimpleFundamentalMatrix {
|
|||
Point2 ca_; ///< Principal point for left camera
|
||||
Point2 cb_; ///< Principal point for right camera
|
||||
|
||||
/// Return the left calibration matrix
|
||||
Matrix3 Ka() const;
|
||||
|
||||
/// Return the right calibration matrix
|
||||
Matrix3 Kb() const;
|
||||
|
||||
public:
|
||||
/// Default constructor
|
||||
SimpleFundamentalMatrix()
|
||||
: E_(), fa_(1.0), fb_(1.0), ca_(0.0, 0.0), cb_(0.0, 0.0) {}
|
||||
|
||||
/// Construct from essential matrix and focal lengths
|
||||
/**
|
||||
* @brief Construct from essential matrix and focal lengths
|
||||
* @param E Essential matrix
|
||||
* @param fa Focal length for left camera
|
||||
* @param fb Focal length for right camera
|
||||
* @param ca Principal point for left camera
|
||||
* @param cb Principal point for right camera
|
||||
*/
|
||||
SimpleFundamentalMatrix(const EssentialMatrix& E, //
|
||||
double fa, double fb,
|
||||
const Point2& ca = Point2(0.0, 0.0),
|
||||
const Point2& cb = Point2(0.0, 0.0))
|
||||
double fa, double fb, const Point2& ca,
|
||||
const Point2& cb)
|
||||
: E_(E), fa_(fa), fb_(fb), ca_(ca), cb_(cb) {}
|
||||
|
||||
/// Return the left calibration matrix
|
||||
Matrix3 leftK() const;
|
||||
|
||||
/// Return the right calibration matrix
|
||||
Matrix3 rightK() const;
|
||||
|
||||
/// Return the fundamental matrix representation
|
||||
/// F = Ka^(-T) * E * Kb^(-1)
|
||||
Matrix3 matrix() const;
|
||||
|
||||
/// @name Testable
|
||||
|
|
@ -147,8 +171,8 @@ class GTSAM_EXPORT SimpleFundamentalMatrix {
|
|||
* Take two fundamental matrices Fca and Fcb, and two points pa and pb, and
|
||||
* returns the 2D point in view (c) where the epipolar lines intersect.
|
||||
*/
|
||||
GTSAM_EXPORT Point2 Transfer(const Matrix3& Fca, const Point2& pa,
|
||||
const Matrix3& Fcb, const Point2& pb);
|
||||
GTSAM_EXPORT Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa,
|
||||
const Matrix3& Fcb, const Point2& pb);
|
||||
|
||||
/// Represents a set of three fundamental matrices for transferring points
|
||||
/// between three cameras.
|
||||
|
|
@ -158,17 +182,17 @@ struct TripleF {
|
|||
|
||||
/// Transfers a point from cameras b,c to camera a.
|
||||
Point2 transferToA(const Point2& pb, const Point2& pc) {
|
||||
return Transfer(Fab.matrix(), pb, Fca.matrix().transpose(), pc);
|
||||
return EpipolarTransfer(Fab.matrix(), pb, Fca.matrix().transpose(), pc);
|
||||
}
|
||||
|
||||
/// Transfers a point from camera a,c to camera b.
|
||||
Point2 transferToB(const Point2& pa, const Point2& pc) {
|
||||
return Transfer(Fab.matrix().transpose(), pa, Fbc.matrix(), pc);
|
||||
return EpipolarTransfer(Fab.matrix().transpose(), pa, Fbc.matrix(), pc);
|
||||
}
|
||||
|
||||
/// Transfers a point from cameras a,b to camera c.
|
||||
Point2 transferToC(const Point2& pa, const Point2& pb) {
|
||||
return Transfer(Fca.matrix(), pa, Fbc.matrix().transpose(), pb);
|
||||
return EpipolarTransfer(Fca.matrix(), pa, Fbc.matrix().transpose(), pb);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,36 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file EdgeKey.cpp
|
||||
* @date Oct 24, 2024
|
||||
* @author: Frank Dellaert
|
||||
* @author: Akshay Krishnan
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/EdgeKey.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
EdgeKey::operator std::string() const {
|
||||
return "{" + std::to_string(i_) + ", " + std::to_string(j_) + "}";
|
||||
}
|
||||
|
||||
GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const EdgeKey& key) {
|
||||
os << (std::string)key;
|
||||
return os;
|
||||
}
|
||||
|
||||
void EdgeKey::print(const std::string& s) const {
|
||||
std::cout << s << *this << std::endl;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
#pragma once
|
||||
|
||||
/**
|
||||
* @file EdgeKey.h
|
||||
* @date Oct 24, 2024
|
||||
* @author: Frank Dellaert
|
||||
* @author: Akshay Krishnan
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
namespace gtsam {
|
||||
class GTSAM_EXPORT EdgeKey {
|
||||
protected:
|
||||
std::uint32_t i_; ///< Upper 32 bits
|
||||
std::uint32_t j_; ///< Lower 32 bits
|
||||
|
||||
public:
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/// Default constructor
|
||||
EdgeKey() : i_(0), j_(0) {}
|
||||
|
||||
/// Constructor
|
||||
EdgeKey(std::uint32_t i, std::uint32_t j) : i_(i), j_(j) {}
|
||||
|
||||
EdgeKey(Key key)
|
||||
: i_(static_cast<std::uint32_t>(key >> 32)),
|
||||
j_(static_cast<std::uint32_t>(key)) {}
|
||||
|
||||
/// @}
|
||||
/// @name API
|
||||
/// @{
|
||||
|
||||
/// Cast to Key
|
||||
operator Key() const { return ((std::uint64_t)i_ << 32) | j_; }
|
||||
|
||||
/// Retrieve high 32 bits
|
||||
inline std::uint32_t i() const { return i_; }
|
||||
|
||||
/// Retrieve low 32 bits
|
||||
inline std::uint32_t j() const { return j_; }
|
||||
|
||||
/** Create a string from the key */
|
||||
operator std::string() const;
|
||||
|
||||
/// Output stream operator
|
||||
friend GTSAM_EXPORT std::ostream& operator<<(std::ostream&, const EdgeKey&);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// Prints the EdgeKey with an optional prefix string.
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/// Checks if this EdgeKey is equal to another, tolerance is ignored.
|
||||
bool equals(const EdgeKey& expected, double tol = 0.0) const {
|
||||
return (*this) == expected;
|
||||
}
|
||||
/// @}
|
||||
};
|
||||
|
||||
/// traits
|
||||
template <>
|
||||
struct traits<EdgeKey> : public Testable<EdgeKey> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -19,9 +19,11 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
|
||||
#include <functional>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
|
|
@ -33,113 +35,143 @@ namespace gtsam {
|
|||
* which allows expressing "Pose 7 from robot B" as "xB7".
|
||||
*/
|
||||
class GTSAM_EXPORT LabeledSymbol {
|
||||
protected:
|
||||
protected:
|
||||
unsigned char c_, label_;
|
||||
std::uint64_t j_;
|
||||
|
||||
public:
|
||||
/** Default constructor */
|
||||
public:
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/// Default constructor
|
||||
LabeledSymbol();
|
||||
|
||||
/** Copy constructor */
|
||||
/// Copy constructor
|
||||
LabeledSymbol(const LabeledSymbol& key);
|
||||
|
||||
/** Constructor */
|
||||
/// Constructor fro characters c and label, and integer j
|
||||
LabeledSymbol(unsigned char c, unsigned char label, std::uint64_t j);
|
||||
|
||||
/** Constructor that decodes an integer gtsam::Key */
|
||||
LabeledSymbol(gtsam::Key key);
|
||||
/// Constructor that decodes an integer Key
|
||||
LabeledSymbol(Key key);
|
||||
|
||||
/** Cast to integer */
|
||||
operator gtsam::Key() const;
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
// Testable Requirements
|
||||
/// Prints the LabeledSymbol with an optional prefix string.
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/// Checks if this LabeledSymbol is equal to another, tolerance is ignored.
|
||||
bool equals(const LabeledSymbol& expected, double tol = 0.0) const {
|
||||
return (*this) == expected;
|
||||
}
|
||||
|
||||
/** return the integer version */
|
||||
gtsam::Key key() const { return (gtsam::Key) *this; }
|
||||
/// @}
|
||||
/// @name Standard API
|
||||
/// @{
|
||||
|
||||
/** Retrieve label character */
|
||||
/// Cast to Key
|
||||
operator Key() const;
|
||||
|
||||
/// return the integer version
|
||||
Key key() const { return (Key) * this; }
|
||||
|
||||
/// Retrieve label character
|
||||
inline unsigned char label() const { return label_; }
|
||||
|
||||
/** Retrieve key character */
|
||||
/// Retrieve key character
|
||||
inline unsigned char chr() const { return c_; }
|
||||
|
||||
/** Retrieve key index */
|
||||
/// Retrieve key index
|
||||
inline size_t index() const { return j_; }
|
||||
|
||||
/** Create a string from the key */
|
||||
/// Create a string from the key
|
||||
operator std::string() const;
|
||||
|
||||
/** Comparison for use in maps */
|
||||
/// Output stream operator that can be used with key_formatter (see Key.h).
|
||||
friend GTSAM_EXPORT std::ostream& operator<<(std::ostream&,
|
||||
const LabeledSymbol&);
|
||||
|
||||
/// @}
|
||||
/// @name Comparison
|
||||
/// @{
|
||||
|
||||
bool operator<(const LabeledSymbol& comp) const;
|
||||
bool operator==(const LabeledSymbol& comp) const;
|
||||
bool operator==(gtsam::Key comp) const;
|
||||
bool operator==(Key comp) const;
|
||||
bool operator!=(const LabeledSymbol& comp) const;
|
||||
bool operator!=(gtsam::Key comp) const;
|
||||
bool operator!=(Key comp) const;
|
||||
|
||||
/** Return a filter function that returns true when evaluated on a gtsam::Key whose
|
||||
* character (when converted to a LabeledSymbol) matches \c c. Use this with the
|
||||
* Values::filter() function to retrieve all key-value pairs with the
|
||||
* requested character.
|
||||
*/
|
||||
/// @}
|
||||
/// @name Filtering
|
||||
/// @{
|
||||
/// Return a filter function that returns true when evaluated on a Key whose
|
||||
/// character (when converted to a LabeledSymbol) matches \c c. Use this with
|
||||
/// the Values::filter() function to retrieve all key-value pairs with the
|
||||
/// requested character.
|
||||
|
||||
// Checks only the type
|
||||
static std::function<bool(gtsam::Key)> TypeTest(unsigned char c);
|
||||
/// Checks only the type
|
||||
static std::function<bool(Key)> TypeTest(unsigned char c);
|
||||
|
||||
// Checks only the robot ID (label_)
|
||||
static std::function<bool(gtsam::Key)> LabelTest(unsigned char label);
|
||||
/// Checks only the robot ID (label_)
|
||||
static std::function<bool(Key)> LabelTest(unsigned char label);
|
||||
|
||||
// Checks both type and the robot ID
|
||||
static std::function<bool(gtsam::Key)> TypeLabelTest(unsigned char c, unsigned char label);
|
||||
/// Checks both type and the robot ID
|
||||
static std::function<bool(Key)> TypeLabelTest(unsigned char c,
|
||||
unsigned char label);
|
||||
|
||||
// Converts to upper/lower versions of labels
|
||||
/// @}
|
||||
/// @name Advanced API
|
||||
/// @{
|
||||
|
||||
/// Converts to upper/lower versions of labels
|
||||
LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); }
|
||||
LabeledSymbol lower() const { return LabeledSymbol(c_, tolower(label_), j_); }
|
||||
|
||||
// Create a new symbol with a different character.
|
||||
LabeledSymbol newChr(unsigned char c) const { return LabeledSymbol(c, label_, j_); }
|
||||
/// Create a new symbol with a different character.
|
||||
LabeledSymbol newChr(unsigned char c) const {
|
||||
return LabeledSymbol(c, label_, j_);
|
||||
}
|
||||
|
||||
// Create a new symbol with a different label.
|
||||
LabeledSymbol newLabel(unsigned char label) const { return LabeledSymbol(c_, label, j_); }
|
||||
/// Create a new symbol with a different label.
|
||||
LabeledSymbol newLabel(unsigned char label) const {
|
||||
return LabeledSymbol(c_, label, j_);
|
||||
}
|
||||
|
||||
/// Output stream operator that can be used with key_formatter (see Key.h).
|
||||
friend GTSAM_EXPORT std::ostream &operator<<(std::ostream &, const LabeledSymbol &);
|
||||
|
||||
private:
|
||||
/// @}
|
||||
|
||||
private:
|
||||
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
/** Serialization function */
|
||||
/// Serialization function
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_NVP(c_);
|
||||
ar & BOOST_SERIALIZATION_NVP(label_);
|
||||
ar & BOOST_SERIALIZATION_NVP(j_);
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& BOOST_SERIALIZATION_NVP(c_);
|
||||
ar& BOOST_SERIALIZATION_NVP(label_);
|
||||
ar& BOOST_SERIALIZATION_NVP(j_);
|
||||
}
|
||||
#endif
|
||||
}; // \class LabeledSymbol
|
||||
}; // \class LabeledSymbol
|
||||
|
||||
/** Create a symbol key from a character, label and index, i.e. xA5. */
|
||||
/// Create a symbol key from a character, label and index, i.e. xA5.
|
||||
inline Key mrsymbol(unsigned char c, unsigned char label, size_t j) {
|
||||
return (Key)LabeledSymbol(c,label,j);
|
||||
return (Key)LabeledSymbol(c, label, j);
|
||||
}
|
||||
|
||||
/** Return the character portion of a symbol key. */
|
||||
/// Return the character portion of a symbol key.
|
||||
inline unsigned char mrsymbolChr(Key key) { return LabeledSymbol(key).chr(); }
|
||||
|
||||
/** Return the label portion of a symbol key. */
|
||||
inline unsigned char mrsymbolLabel(Key key) { return LabeledSymbol(key).label(); }
|
||||
/// Return the label portion of a symbol key.
|
||||
inline unsigned char mrsymbolLabel(Key key) {
|
||||
return LabeledSymbol(key).label();
|
||||
}
|
||||
|
||||
/** Return the index portion of a symbol key. */
|
||||
/// Return the index portion of a symbol key.
|
||||
inline size_t mrsymbolIndex(Key key) { return LabeledSymbol(key).index(); }
|
||||
|
||||
/// traits
|
||||
template<> struct traits<LabeledSymbol> : public Testable<LabeledSymbol> {};
|
||||
|
||||
} // \namespace gtsam
|
||||
template <>
|
||||
struct traits<LabeledSymbol> : public Testable<LabeledSymbol> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -0,0 +1,57 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file testEdgeKey.cpp
|
||||
* @date Oct 24, 2024
|
||||
* @author: Frank Dellaert
|
||||
* @author: Akshay Krishnan
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/inference/EdgeKey.h>
|
||||
|
||||
#include <sstream>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(EdgeKey, Construction) {
|
||||
EdgeKey edge(1, 2);
|
||||
EXPECT(edge.i() == 1);
|
||||
EXPECT(edge.j() == 2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(EdgeKey, Equality) {
|
||||
EdgeKey edge1(1, 2);
|
||||
EdgeKey edge2(1, 2);
|
||||
EdgeKey edge3(2, 3);
|
||||
|
||||
EXPECT(assert_equal(edge1, edge2));
|
||||
EXPECT(!edge1.equals(edge3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(EdgeKey, StreamOutput) {
|
||||
EdgeKey edge(1, 2);
|
||||
std::ostringstream oss;
|
||||
oss << edge;
|
||||
EXPECT("{1, 2}" == oss.str());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,118 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
* GTSAM Copyright 2010-2024, 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
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file TransferFactor.h
|
||||
* @brief TransferFactor class
|
||||
* @author Frank Dellaert
|
||||
* @date October 24, 2024
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/FundamentalMatrix.h>
|
||||
#include <gtsam/inference/EdgeKey.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Binary factor in the context of Structure from Motion (SfM).
|
||||
* It is used to transfer transfer corresponding points from two views to a
|
||||
* third based on two fundamental matrices. The factor computes the error
|
||||
* between the transferred points `pa` and `pb`, and the actual point `pc` in
|
||||
* the target view. Jacobians are done using numerical differentiation.
|
||||
*/
|
||||
template <typename F>
|
||||
class TransferFactor : public NoiseModelFactorN<F, F> {
|
||||
EdgeKey key1_, key2_; ///< the two EdgeKeys
|
||||
std::vector<std::tuple<Point2, Point2, Point2>>
|
||||
triplets_; ///< Point triplets
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for a single triplet of points
|
||||
*
|
||||
* @note: batching all points for the same transfer will be much faster.
|
||||
*
|
||||
* @param key1 First EdgeKey specifying F1: (a, c) or (c, a).
|
||||
* @param key2 Second EdgeKey specifying F2: (b, c) or (c, b).
|
||||
* @param pa The point in the first view (a).
|
||||
* @param pb The point in the second view (b).
|
||||
* @param pc The point in the third (and transfer target) view (c).
|
||||
* @param model An optional SharedNoiseModel that defines the noise model
|
||||
* for this factor. Defaults to nullptr.
|
||||
*/
|
||||
TransferFactor(EdgeKey key1, EdgeKey key2, const Point2& pa, const Point2& pb,
|
||||
const Point2& pc, const SharedNoiseModel& model = nullptr)
|
||||
: NoiseModelFactorN<F, F>(model, key1, key2),
|
||||
key1_(key1),
|
||||
key2_(key2),
|
||||
triplets_({std::make_tuple(pa, pb, pc)}) {}
|
||||
|
||||
/**
|
||||
* @brief Constructor that accepts a vector of point triplets.
|
||||
*
|
||||
* @param key1 First EdgeKey specifying F1: (a, c) or (c, a).
|
||||
* @param key2 Second EdgeKey specifying F2: (b, c) or (c, b).
|
||||
* @param triplets A vector of triplets containing (pa, pb, pc).
|
||||
* @param model An optional SharedNoiseModel that defines the noise model
|
||||
* for this factor. Defaults to nullptr.
|
||||
*/
|
||||
TransferFactor(
|
||||
EdgeKey key1, EdgeKey key2,
|
||||
const std::vector<std::tuple<Point2, Point2, Point2>>& triplets,
|
||||
const SharedNoiseModel& model = nullptr)
|
||||
: NoiseModelFactorN<F, F>(model, key1, key2),
|
||||
key1_(key1),
|
||||
key2_(key2),
|
||||
triplets_(triplets) {}
|
||||
|
||||
// Create Matrix3 objects based on EdgeKey configurations
|
||||
std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const {
|
||||
// Fill Fca and Fcb based on EdgeKey configurations
|
||||
if (key1_.i() == key2_.i()) {
|
||||
return {F1.matrix(), F2.matrix()};
|
||||
} else if (key1_.i() == key2_.j()) {
|
||||
return {F1.matrix(), F2.matrix().transpose()};
|
||||
} else if (key1_.j() == key2_.i()) {
|
||||
return {F1.matrix().transpose(), F2.matrix()};
|
||||
} else if (key1_.j() == key2_.j()) {
|
||||
return {F1.matrix().transpose(), F2.matrix().transpose()};
|
||||
} else {
|
||||
throw std::runtime_error(
|
||||
"TransferFactor: invalid EdgeKey configuration.");
|
||||
}
|
||||
}
|
||||
|
||||
/// vector of errors returns 2*N vector
|
||||
Vector evaluateError(const F& F1, const F& F2,
|
||||
OptionalMatrixType H1 = nullptr,
|
||||
OptionalMatrixType H2 = nullptr) const override {
|
||||
std::function<Vector(const F&, const F&)> transfer = [&](const F& F1,
|
||||
const F& F2) {
|
||||
Vector errors(2 * triplets_.size());
|
||||
size_t idx = 0;
|
||||
auto [Fca, Fcb] = getMatrices(F1, F2);
|
||||
for (const auto& tuple : triplets_) {
|
||||
const auto& [pa, pb, pc] = tuple;
|
||||
Point2 transferredPoint = EpipolarTransfer(Fca, pa, Fcb, pb);
|
||||
Vector2 error = transferredPoint - pc;
|
||||
errors.segment<2>(idx) = error;
|
||||
idx += 2;
|
||||
}
|
||||
return errors;
|
||||
};
|
||||
if (H1) *H1 = numericalDerivative21<Vector, F, F>(transfer, F1, F2);
|
||||
if (H2) *H2 = numericalDerivative22<Vector, F, F>(transfer, F1, F2);
|
||||
return transfer(F1, F2);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -55,7 +55,7 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
|
|||
: Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {}
|
||||
|
||||
/**
|
||||
* @brief Caclulate error: (norm(Tb - Ta) - measurement)
|
||||
* @brief Calculate error: (norm(Tb - Ta) - measurement)
|
||||
* where Tb and Ta are Point3 translations and measurement is
|
||||
* the Unit3 translation direction from a to b.
|
||||
*
|
||||
|
|
@ -120,7 +120,7 @@ class BilinearAngleTranslationFactor
|
|||
using NoiseModelFactor2<Point3, Point3, Vector1>::evaluateError;
|
||||
|
||||
/**
|
||||
* @brief Caclulate error: (scale * (Tb - Ta) - measurement)
|
||||
* @brief Calculate error: (scale * (Tb - Ta) - measurement)
|
||||
* where Tb and Ta are Point3 translations and measurement is
|
||||
* the Unit3 translation direction from a to b.
|
||||
*
|
||||
|
|
|
|||
|
|
@ -119,7 +119,7 @@ class GTSAM_EXPORT TranslationRecovery {
|
|||
* @param betweenTranslations relative translations (with scale) between 2
|
||||
* points in world coordinate frame known a priori.
|
||||
* @param rng random number generator
|
||||
* @param intialValues (optional) initial values from a prior
|
||||
* @param initialValues (optional) initial values from a prior
|
||||
* @return Values
|
||||
*/
|
||||
Values initializeRandomly(
|
||||
|
|
@ -156,7 +156,7 @@ class GTSAM_EXPORT TranslationRecovery {
|
|||
* points in world coordinate frame known a priori. Unlike
|
||||
* relativeTranslations, zero-magnitude betweenTranslations are not treated as
|
||||
* hard constraints.
|
||||
* @param initialValues intial values for optimization. Initializes randomly
|
||||
* @param initialValues initial values for optimization. Initializes randomly
|
||||
* if not provided.
|
||||
* @return Values
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -0,0 +1,161 @@
|
|||
/*
|
||||
* @file testTransferFactor.cpp
|
||||
* @brief Test TransferFactor class
|
||||
* @author Your Name
|
||||
* @date October 23, 2024
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
#include <gtsam/nonlinear/factorTesting.h>
|
||||
#include <gtsam/sfm/TransferFactor.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
//*************************************************************************
|
||||
/// Generate three cameras on a circle, looking in
|
||||
std::array<Pose3, 3> generateCameraPoses() {
|
||||
std::array<Pose3, 3> cameraPoses;
|
||||
const double radius = 1.0;
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
double angle = i * 2.0 * M_PI / 3.0;
|
||||
double c = cos(angle), s = sin(angle);
|
||||
Rot3 aRb({-s, c, 0}, {0, 0, -1}, {-c, -s, 0});
|
||||
cameraPoses[i] = {aRb, Point3(radius * c, radius * s, 0)};
|
||||
}
|
||||
return cameraPoses;
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
// Function to generate a TripleF from camera poses
|
||||
TripleF<SimpleFundamentalMatrix> generateTripleF(
|
||||
const std::array<Pose3, 3>& cameraPoses) {
|
||||
std::array<SimpleFundamentalMatrix, 3> F;
|
||||
for (size_t i = 0; i < 3; ++i) {
|
||||
size_t j = (i + 1) % 3;
|
||||
const Pose3 iPj = cameraPoses[i].between(cameraPoses[j]);
|
||||
EssentialMatrix E(iPj.rotation(), Unit3(iPj.translation()));
|
||||
F[i] = {E, 1000.0, 1000.0, Point2(640 / 2, 480 / 2),
|
||||
Point2(640 / 2, 480 / 2)};
|
||||
}
|
||||
return {F[0], F[1], F[2]}; // Return a TripleF instance
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
namespace fixture {
|
||||
// Generate cameras on a circle
|
||||
std::array<Pose3, 3> cameraPoses = generateCameraPoses();
|
||||
auto triplet = generateTripleF(cameraPoses);
|
||||
double focalLength = 1000;
|
||||
Point2 principalPoint(640 / 2, 480 / 2);
|
||||
const Cal3_S2 K(focalLength, focalLength, 0.0, //
|
||||
principalPoint.x(), principalPoint.y());
|
||||
} // namespace fixture
|
||||
|
||||
//*************************************************************************
|
||||
// Test for getMatrices
|
||||
TEST(TransferFactor, GetMatrices) {
|
||||
using namespace fixture;
|
||||
TransferFactor<SimpleFundamentalMatrix> factor{{2, 0}, {1, 2}, {}};
|
||||
|
||||
// Check that getMatrices is correct
|
||||
auto [Fki, Fkj] = factor.getMatrices(triplet.Fca, triplet.Fbc);
|
||||
EXPECT(assert_equal<Matrix3>(triplet.Fca.matrix(), Fki));
|
||||
EXPECT(assert_equal<Matrix3>(triplet.Fbc.matrix().transpose(), Fkj));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
// Test for TransferFactor
|
||||
TEST(TransferFactor, Jacobians) {
|
||||
using namespace fixture;
|
||||
|
||||
// Now project a point into the three cameras
|
||||
const Point3 P(0.1, 0.2, 0.3);
|
||||
|
||||
std::array<Point2, 3> p;
|
||||
for (size_t i = 0; i < 3; ++i) {
|
||||
// Project the point into each camera
|
||||
PinholeCameraCal3_S2 camera(cameraPoses[i], K);
|
||||
p[i] = camera.project(P);
|
||||
}
|
||||
|
||||
// Create a TransferFactor
|
||||
EdgeKey key01(0, 1), key12(1, 2), key20(2, 0);
|
||||
TransferFactor<SimpleFundamentalMatrix> //
|
||||
factor0{key01, key20, p[1], p[2], p[0]},
|
||||
factor1{key12, key01, p[2], p[0], p[1]},
|
||||
factor2{key20, key12, p[0], p[1], p[2]};
|
||||
|
||||
// Create Values with edge keys
|
||||
Values values;
|
||||
values.insert(key01, triplet.Fab);
|
||||
values.insert(key12, triplet.Fbc);
|
||||
values.insert(key20, triplet.Fca);
|
||||
|
||||
// Check error and Jacobians
|
||||
for (auto&& f : {factor0, factor1, factor2}) {
|
||||
Vector error = f.unwhitenedError(values);
|
||||
EXPECT(assert_equal<Vector>(Z_2x1, error));
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-7);
|
||||
}
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
// Test for TransferFactor with multiple tuples
|
||||
TEST(TransferFactor, MultipleTuples) {
|
||||
using namespace fixture;
|
||||
|
||||
// Now project multiple points into the three cameras
|
||||
const size_t numPoints = 5; // Number of points to test
|
||||
std::vector<Point3> points3D;
|
||||
std::vector<std::array<Point2, 3>> projectedPoints;
|
||||
|
||||
// Generate random 3D points and project them
|
||||
for (size_t n = 0; n < numPoints; ++n) {
|
||||
Point3 P(0.1 * n, 0.2 * n, 0.3 + 0.1 * n);
|
||||
points3D.push_back(P);
|
||||
|
||||
std::array<Point2, 3> p;
|
||||
for (size_t i = 0; i < 3; ++i) {
|
||||
// Project the point into each camera
|
||||
PinholeCameraCal3_S2 camera(cameraPoses[i], K);
|
||||
p[i] = camera.project(P);
|
||||
}
|
||||
projectedPoints.push_back(p);
|
||||
}
|
||||
|
||||
// Create a vector of tuples for the TransferFactor
|
||||
EdgeKey key01(0, 1), key12(1, 2), key20(2, 0);
|
||||
std::vector<std::tuple<Point2, Point2, Point2>> tuples;
|
||||
|
||||
for (size_t n = 0; n < numPoints; ++n) {
|
||||
const auto& p = projectedPoints[n];
|
||||
tuples.emplace_back(p[1], p[2], p[0]);
|
||||
}
|
||||
|
||||
// Create TransferFactors using the new constructor
|
||||
TransferFactor<SimpleFundamentalMatrix> factor{key01, key20, tuples};
|
||||
|
||||
// Create Values with edge keys
|
||||
Values values;
|
||||
values.insert(key01, triplet.Fab);
|
||||
values.insert(key12, triplet.Fbc);
|
||||
values.insert(key20, triplet.Fca);
|
||||
|
||||
// Check error and Jacobians for multiple tuples
|
||||
Vector error = factor.unwhitenedError(values);
|
||||
// The error vector should be of size 2 * numPoints
|
||||
EXPECT_LONGS_EQUAL(2 * numPoints, error.size());
|
||||
// Since the points are perfectly matched, the error should be zero
|
||||
EXPECT(assert_equal<Vector>(Vector::Zero(2 * numPoints), error, 1e-9));
|
||||
|
||||
// Check the Jacobians
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
//*************************************************************************
|
||||
Loading…
Reference in New Issue