Frank Dellaert
GitHub
commit
67495babae
|
|
@ -56,7 +56,7 @@ std::vector<Point3> createPoints() {
|
|||
|
||||
/**
|
||||
* Create a set of ground-truth poses
|
||||
* Default values give a circular trajectory, radius 30 at pi/4 intervals,
|
||||
* Default values give a circular trajectory, radius 30 at pi/4 intervals,
|
||||
* always facing the circle center
|
||||
*/
|
||||
std::vector<Pose3> createPoses(
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ 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);
|
||||
Cal3_S2 cal(50.0, 50.0, 0.0, 50.0, 50.0);
|
||||
|
||||
// Create the set of 8 ground-truth landmarks
|
||||
vector<Point3> points = createPoints();
|
||||
|
|
@ -47,13 +47,13 @@ int main(int argc, char* argv[]) {
|
|||
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);
|
||||
auto F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K());
|
||||
auto F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.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);
|
||||
PinholeCamera<Cal3_S2> camera(poses[i], cal);
|
||||
for (size_t j = 0; j < 8; ++j) {
|
||||
p[i][j] = camera.project(points[j]);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,10 +2,12 @@
|
|||
* @file FundamentalMatrix.cpp
|
||||
* @brief FundamentalMatrix classes
|
||||
* @author Frank Dellaert
|
||||
* @date Oct 23, 2024
|
||||
* @date October 2024
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/geometry/FundamentalMatrix.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -26,6 +28,11 @@ Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa, //
|
|||
}
|
||||
|
||||
//*************************************************************************
|
||||
FundamentalMatrix::FundamentalMatrix(const Matrix3& U, double s,
|
||||
const Matrix3& V) {
|
||||
initialize(U, s, V);
|
||||
}
|
||||
|
||||
FundamentalMatrix::FundamentalMatrix(const Matrix3& F) {
|
||||
// Perform SVD
|
||||
Eigen::JacobiSVD<Matrix3> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
|
||||
|
|
@ -47,28 +54,24 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) {
|
|||
"The input matrix does not represent a valid fundamental matrix.");
|
||||
}
|
||||
|
||||
// Ensure the second singular value is recorded as s
|
||||
s_ = singularValues(1);
|
||||
initialize(U, singularValues(1), V);
|
||||
}
|
||||
|
||||
// Check if U is a reflection
|
||||
if (U.determinant() < 0) {
|
||||
U = -U;
|
||||
s_ = -s_; // Change sign of scalar if U is a reflection
|
||||
}
|
||||
void FundamentalMatrix::initialize(Matrix3 U, double s, Matrix3 V) {
|
||||
// Check if U is a reflection and flip third column if so
|
||||
if (U.determinant() < 0) U.col(2) *= -1;
|
||||
|
||||
// Check if V is a reflection
|
||||
if (V.determinant() < 0) {
|
||||
V = -V;
|
||||
s_ = -s_; // Change sign of scalar if U is a reflection
|
||||
}
|
||||
// Same check for V
|
||||
if (V.determinant() < 0) V.col(2) *= -1;
|
||||
|
||||
// Assign the rotations
|
||||
U_ = Rot3(U);
|
||||
s_ = s;
|
||||
V_ = Rot3(V);
|
||||
}
|
||||
|
||||
Matrix3 FundamentalMatrix::matrix() const {
|
||||
return U_.matrix() * Vector3(1, s_, 0).asDiagonal() * V_.transpose().matrix();
|
||||
return U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() *
|
||||
V_.transpose().matrix();
|
||||
}
|
||||
|
||||
void FundamentalMatrix::print(const std::string& s) const {
|
||||
|
|
@ -90,9 +93,9 @@ Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const {
|
|||
}
|
||||
|
||||
FundamentalMatrix FundamentalMatrix::retract(const Vector& delta) const {
|
||||
Rot3 newU = U_.retract(delta.head<3>());
|
||||
double newS = s_ + delta(3); // Update scalar
|
||||
Rot3 newV = V_.retract(delta.tail<3>());
|
||||
const Rot3 newU = U_.retract(delta.head<3>());
|
||||
const double newS = s_ + delta(3);
|
||||
const Rot3 newV = V_.retract(delta.tail<3>());
|
||||
return FundamentalMatrix(newU, newS, newV);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@
|
|||
* @file FundamentalMatrix.h
|
||||
* @brief FundamentalMatrix classes
|
||||
* @author Frank Dellaert
|
||||
* @date Oct 23, 2024
|
||||
* @date October 2024
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -15,11 +15,15 @@ namespace gtsam {
|
|||
|
||||
/**
|
||||
* @class FundamentalMatrix
|
||||
* @brief Represents a general fundamental matrix.
|
||||
* @brief Represents a fundamental matrix in computer vision, which encodes the
|
||||
* epipolar geometry between two views.
|
||||
*
|
||||
* This class represents a general fundamental matrix, which is a 3x3 matrix
|
||||
* that describes the relationship between two images. It is parameterized by a
|
||||
* left rotation U, a scalar s, and a right rotation V.
|
||||
* The FundamentalMatrix class encapsulates the fundamental matrix, which
|
||||
* relates corresponding points in stereo images. It is parameterized by two
|
||||
* rotation matrices (U and V) and a scalar parameter (s).
|
||||
* Using these values, the fundamental matrix is represented as
|
||||
*
|
||||
* F = U * diag(1, s, 0) * V^T
|
||||
*/
|
||||
class GTSAM_EXPORT FundamentalMatrix {
|
||||
private:
|
||||
|
|
@ -34,15 +38,10 @@ class GTSAM_EXPORT FundamentalMatrix {
|
|||
/**
|
||||
* @brief Construct from U, V, and scalar s
|
||||
*
|
||||
* Initializes the FundamentalMatrix with the given left rotation U,
|
||||
* scalar s, and right rotation V.
|
||||
*
|
||||
* @param U Left rotation matrix
|
||||
* @param s Scalar parameter for the fundamental matrix
|
||||
* @param V Right rotation matrix
|
||||
* Initializes the FundamentalMatrix From the SVD representation
|
||||
* U*diag(1,s,0)*V^T. It will internally convert to using SO(3).
|
||||
*/
|
||||
FundamentalMatrix(const Rot3& U, double s, const Rot3& V)
|
||||
: U_(U), s_(s), V_(V) {}
|
||||
FundamentalMatrix(const Matrix3& U, double s, const Matrix3& V);
|
||||
|
||||
/**
|
||||
* @brief Construct from a 3x3 matrix using SVD
|
||||
|
|
@ -54,22 +53,35 @@ class GTSAM_EXPORT FundamentalMatrix {
|
|||
*/
|
||||
FundamentalMatrix(const Matrix3& F);
|
||||
|
||||
/**
|
||||
* @brief Construct from essential matrix and calibration matrices
|
||||
*
|
||||
* Initializes the FundamentalMatrix from the given essential matrix E
|
||||
* and calibration matrices Ka and Kb, using
|
||||
* F = Ka^(-T) * E * Kb^(-1)
|
||||
* and then calls constructor that decomposes F via SVD.
|
||||
*
|
||||
* @param E Essential matrix
|
||||
* @param Ka Calibration matrix for the left camera
|
||||
* @param Kb Calibration matrix for the right camera
|
||||
*/
|
||||
FundamentalMatrix(const Matrix3& Ka, const EssentialMatrix& E,
|
||||
const Matrix3& Kb)
|
||||
: FundamentalMatrix(Ka.transpose().inverse() * E.matrix() *
|
||||
Kb.inverse()) {}
|
||||
|
||||
/**
|
||||
* @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()) {}
|
||||
FundamentalMatrix(const Matrix3& Ka, const Pose3& aPb, const Matrix3& Kb)
|
||||
: FundamentalMatrix(Ka, EssentialMatrix::FromPose3(aPb), Kb) {}
|
||||
|
||||
/// Return the fundamental matrix representation
|
||||
Matrix3 matrix() const;
|
||||
|
|
@ -96,6 +108,13 @@ class GTSAM_EXPORT FundamentalMatrix {
|
|||
/// Retract the given vector to get a new FundamentalMatrix
|
||||
FundamentalMatrix retract(const Vector& delta) const;
|
||||
/// @}
|
||||
private:
|
||||
/// Private constructor for internal use
|
||||
FundamentalMatrix(const Rot3& U, double s, const Rot3& V)
|
||||
: U_(U), s_(s), V_(V) {}
|
||||
|
||||
/// Initialize SO(3) matrices from general O(3) matrices
|
||||
void initialize(Matrix3 U, double s, Matrix3 V);
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -578,6 +578,8 @@ class Unit3 {
|
|||
// Standard Constructors
|
||||
Unit3();
|
||||
Unit3(const gtsam::Point3& pose);
|
||||
Unit3(double x, double y, double z);
|
||||
Unit3(const gtsam::Point2& p, double f);
|
||||
|
||||
// Testable
|
||||
void print(string s = "") const;
|
||||
|
|
@ -620,10 +622,10 @@ class EssentialMatrix {
|
|||
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
|
||||
|
||||
// Constructors from Pose3
|
||||
gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_);
|
||||
static gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_);
|
||||
|
||||
gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_,
|
||||
Eigen::Ref<Eigen::MatrixXd> H);
|
||||
static gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_,
|
||||
Eigen::Ref<Eigen::MatrixXd> H);
|
||||
|
||||
// Testable
|
||||
void print(string s = "") const;
|
||||
|
|
@ -903,6 +905,59 @@ class Cal3Bundler {
|
|||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/FundamentalMatrix.h>
|
||||
|
||||
// FundamentalMatrix class
|
||||
class FundamentalMatrix {
|
||||
// Constructors
|
||||
FundamentalMatrix();
|
||||
FundamentalMatrix(const gtsam::Matrix3& U, double s, const gtsam::Matrix3& V);
|
||||
FundamentalMatrix(const gtsam::Matrix3& F);
|
||||
|
||||
// Overloaded constructors for specific calibration types
|
||||
FundamentalMatrix(const gtsam::Matrix3& Ka, const gtsam::EssentialMatrix& E,
|
||||
const gtsam::Matrix3& Kb);
|
||||
FundamentalMatrix(const gtsam::Matrix3& Ka, const gtsam::Pose3& aPb,
|
||||
const gtsam::Matrix3& Kb);
|
||||
|
||||
// Methods
|
||||
gtsam::Matrix3 matrix() const;
|
||||
|
||||
// Testable
|
||||
void print(const std::string& s = "") const;
|
||||
bool equals(const gtsam::FundamentalMatrix& other, double tol = 1e-9) const;
|
||||
|
||||
// Manifold
|
||||
static size_t Dim();
|
||||
size_t dim() const;
|
||||
gtsam::Vector localCoordinates(const gtsam::FundamentalMatrix& F) const;
|
||||
gtsam::FundamentalMatrix retract(const gtsam::Vector& delta) const;
|
||||
};
|
||||
|
||||
// SimpleFundamentalMatrix class
|
||||
class SimpleFundamentalMatrix {
|
||||
// Constructors
|
||||
SimpleFundamentalMatrix();
|
||||
SimpleFundamentalMatrix(const gtsam::EssentialMatrix& E, double fa, double fb,
|
||||
const gtsam::Point2& ca, const gtsam::Point2& cb);
|
||||
|
||||
// Methods
|
||||
gtsam::Matrix3 matrix() const;
|
||||
|
||||
// Testable
|
||||
void print(const std::string& s = "") const;
|
||||
bool equals(const gtsam::SimpleFundamentalMatrix& other, double tol = 1e-9) const;
|
||||
|
||||
// Manifold
|
||||
static size_t Dim();
|
||||
size_t dim() const;
|
||||
gtsam::Vector localCoordinates(const gtsam::SimpleFundamentalMatrix& F) const;
|
||||
gtsam::SimpleFundamentalMatrix retract(const gtsam::Vector& delta) const;
|
||||
};
|
||||
|
||||
gtsam::Point2 EpipolarTransfer(const gtsam::Matrix3& Fca, const gtsam::Point2& pa,
|
||||
const gtsam::Matrix3& Fcb, const gtsam::Point2& pb);
|
||||
|
||||
#include <gtsam/geometry/CalibratedCamera.h>
|
||||
class CalibratedCamera {
|
||||
// Standard Constructors and Named Constructors
|
||||
|
|
|
|||
|
|
@ -6,12 +6,15 @@
|
|||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/geometry/FundamentalMatrix.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace std::placeholders;
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -24,21 +27,38 @@ GTSAM_CONCEPT_MANIFOLD_INST(FundamentalMatrix)
|
|||
Rot3 trueU = Rot3::Yaw(M_PI_2);
|
||||
Rot3 trueV = Rot3::Yaw(M_PI_4);
|
||||
double trueS = 0.5;
|
||||
FundamentalMatrix trueF(trueU, trueS, trueV);
|
||||
FundamentalMatrix trueF(trueU.matrix(), trueS, trueV.matrix());
|
||||
|
||||
//*************************************************************************
|
||||
TEST(FundamentalMatrix, localCoordinates) {
|
||||
TEST(FundamentalMatrix, LocalCoordinates) {
|
||||
Vector expected = Z_7x1; // Assuming 7 dimensions for U, V, and s
|
||||
Vector actual = trueF.localCoordinates(trueF);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST(FundamentalMatrix, retract) {
|
||||
TEST(FundamentalMatrix, Retract) {
|
||||
FundamentalMatrix actual = trueF.retract(Z_7x1);
|
||||
EXPECT(assert_equal(trueF, actual));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
// Test conversion from F matrices, including non-rotations
|
||||
TEST(FundamentalMatrix, Conversion) {
|
||||
Matrix3 U = trueU.matrix();
|
||||
Matrix3 V = trueV.matrix();
|
||||
std::vector<FundamentalMatrix> Fs = {
|
||||
FundamentalMatrix(U, trueS, V), FundamentalMatrix(U, trueS, -V),
|
||||
FundamentalMatrix(-U, trueS, V), FundamentalMatrix(-U, trueS, -V)};
|
||||
|
||||
for (const auto& trueF : Fs) {
|
||||
const Matrix3 F = trueF.matrix();
|
||||
FundamentalMatrix actual(F);
|
||||
// We check the matrices as the underlying representation is not unique
|
||||
CHECK(assert_equal(F, actual.matrix()));
|
||||
}
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST(FundamentalMatrix, RoundTrip) {
|
||||
Vector7 d;
|
||||
|
|
@ -61,14 +81,14 @@ TEST(SimpleStereo, Conversion) {
|
|||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST(SimpleStereo, localCoordinates) {
|
||||
TEST(SimpleStereo, LocalCoordinates) {
|
||||
Vector expected = Z_7x1;
|
||||
Vector actual = stereoF.localCoordinates(stereoF);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST(SimpleStereo, retract) {
|
||||
TEST(SimpleStereo, Retract) {
|
||||
SimpleFundamentalMatrix actual = stereoF.retract(Z_9x1);
|
||||
EXPECT(assert_equal(stereoF, actual));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -11,6 +11,7 @@ namespace gtsam {
|
|||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/CalibratedCamera.h>
|
||||
#include <gtsam/geometry/EssentialMatrix.h>
|
||||
#include <gtsam/geometry/FundamentalMatrix.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
|
|
@ -537,6 +538,8 @@ class ISAM2 {
|
|||
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
|
||||
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
|
||||
gtsam::Cal3Bundler, gtsam::EssentialMatrix,
|
||||
gtsam::Cal3Bundler, gtsam::FundamentalMatrix,
|
||||
gtsam::Cal3Bundler, gtsam::SimpleFundamentalMatrix,
|
||||
gtsam::PinholeCamera<gtsam::Cal3_S2>,
|
||||
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
|
||||
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@ def main():
|
|||
points = SFMdata.createPoints()
|
||||
|
||||
# Create the set of ground-truth poses
|
||||
poses = SFMdata.createPoses(K)
|
||||
poses = SFMdata.createPoses()
|
||||
|
||||
# Create a factor graph
|
||||
graph = NonlinearFactorGraph()
|
||||
|
|
|
|||
|
|
@ -3,14 +3,14 @@ A structure-from-motion example with landmarks
|
|||
- The landmarks form a 10 meter cube
|
||||
- The robot rotates around the landmarks, always facing towards the cube
|
||||
"""
|
||||
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from typing import List
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam import Cal3_S2, Point3, Pose3
|
||||
from gtsam import Point3, Pose3, Rot3
|
||||
|
||||
|
||||
def createPoints() -> List[Point3]:
|
||||
|
|
@ -28,16 +28,49 @@ def createPoints() -> List[Point3]:
|
|||
return points
|
||||
|
||||
|
||||
def createPoses(K: Cal3_S2) -> List[Pose3]:
|
||||
"""Generate a set of ground-truth camera poses arranged in a circle about the origin."""
|
||||
radius = 40.0
|
||||
height = 10.0
|
||||
angles = np.linspace(0, 2 * np.pi, 8, endpoint=False)
|
||||
up = gtsam.Point3(0, 0, 1)
|
||||
target = gtsam.Point3(0, 0, 0)
|
||||
poses = []
|
||||
for theta in angles:
|
||||
position = gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta), height)
|
||||
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
|
||||
poses.append(camera.pose())
|
||||
_M_PI_2 = np.pi / 2
|
||||
_M_PI_4 = np.pi / 4
|
||||
|
||||
|
||||
def createPoses(
|
||||
init: Pose3 = Pose3(Rot3.Ypr(_M_PI_2, 0, -_M_PI_2), Point3(30, 0, 0)),
|
||||
delta: Pose3 = Pose3(
|
||||
Rot3.Ypr(0, -_M_PI_4, 0),
|
||||
Point3(np.sin(_M_PI_4) * 30, 0, 30 * (1 - np.sin(_M_PI_4))),
|
||||
),
|
||||
steps: int = 8,
|
||||
) -> List[Pose3]:
|
||||
"""
|
||||
Create a set of ground-truth poses
|
||||
Default values give a circular trajectory, radius 30 at pi/4 intervals,
|
||||
always facing the circle center
|
||||
"""
|
||||
poses = [init]
|
||||
for _ in range(1, steps):
|
||||
poses.append(poses[-1].compose(delta))
|
||||
return poses
|
||||
|
||||
|
||||
def posesOnCircle(num_cameras=8, R=30):
|
||||
"""Create regularly spaced poses with specified radius and number of cameras."""
|
||||
theta = 2 * np.pi / num_cameras
|
||||
|
||||
# Initial pose at angle 0, position (R, 0, 0), facing the center with Y-axis pointing down
|
||||
init_rotation = Rot3.Ypr(_M_PI_2, 0, -_M_PI_2)
|
||||
init_position = np.array([R, 0, 0])
|
||||
init = Pose3(init_rotation, init_position)
|
||||
|
||||
# Delta rotation: rotate by -theta around Z-axis (counterclockwise movement)
|
||||
delta_rotation = Rot3.Ypr(0, -theta, 0)
|
||||
|
||||
# Delta translation in world frame
|
||||
delta_translation_world = np.array([R * (np.cos(theta) - 1), R * np.sin(theta), 0])
|
||||
|
||||
# Transform delta translation to local frame of the camera
|
||||
delta_translation_local = init.rotation().unrotate(delta_translation_world)
|
||||
|
||||
# Define delta pose
|
||||
delta = Pose3(delta_rotation, delta_translation_local)
|
||||
|
||||
# Generate poses
|
||||
return createPoses(init, delta, num_cameras)
|
||||
|
|
|
|||
|
|
@ -31,8 +31,7 @@ def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]:
|
|||
that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata
|
||||
and the unit translations directions between some camera pairs are computed from their
|
||||
global translations. """
|
||||
fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0
|
||||
wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(fx, fy, s, u0, v0))
|
||||
wTc_list = SFMdata.createPoses()
|
||||
# Rotations of the cameras in the world frame.
|
||||
wRc_values = gtsam.Values()
|
||||
# Normalized translation directions from camera i to camera j
|
||||
|
|
|
|||
|
|
@ -73,7 +73,7 @@ def visual_ISAM2_example():
|
|||
points = SFMdata.createPoints()
|
||||
|
||||
# Create the set of ground-truth poses
|
||||
poses = SFMdata.createPoses(K)
|
||||
poses = SFMdata.createPoses()
|
||||
|
||||
# Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps
|
||||
# to maintain proper linearization and efficient variable ordering, iSAM2
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@ def main():
|
|||
# Create the set of ground-truth landmarks
|
||||
points = SFMdata.createPoints()
|
||||
# Create the set of ground-truth poses
|
||||
poses = SFMdata.createPoses(K)
|
||||
poses = SFMdata.createPoses()
|
||||
|
||||
# Create a NonlinearISAM object which will relinearize and reorder the variables
|
||||
# every "reorderInterval" updates
|
||||
|
|
|
|||
|
|
@ -0,0 +1,285 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
FundamentalMatrix unit tests.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
|
||||
# pylint: disable=no-name-in-module
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
from gtsam.examples import SFMdata
|
||||
from numpy.testing import assert_almost_equal
|
||||
|
||||
import gtsam
|
||||
from gtsam import (Cal3_S2, EssentialMatrix, FundamentalMatrix,
|
||||
PinholeCameraCal3_S2, Point2, Point3, Rot3,
|
||||
SimpleFundamentalMatrix, Unit3)
|
||||
|
||||
|
||||
class TestFundamentalMatrix(unittest.TestCase):
|
||||
|
||||
def setUp(self):
|
||||
# Create two rotations and corresponding fundamental matrix F
|
||||
self.trueU = Rot3.Yaw(np.pi / 2)
|
||||
self.trueV = Rot3.Yaw(np.pi / 4)
|
||||
self.trueS = 0.5
|
||||
self.trueF = FundamentalMatrix(self.trueU.matrix(), self.trueS, self.trueV.matrix())
|
||||
|
||||
def test_localCoordinates(self):
|
||||
expected = np.zeros(7) # Assuming 7 dimensions for U, V, and s
|
||||
actual = self.trueF.localCoordinates(self.trueF)
|
||||
assert_almost_equal(expected, actual, decimal=8)
|
||||
|
||||
def test_retract(self):
|
||||
actual = self.trueF.retract(np.zeros(7))
|
||||
self.assertTrue(self.trueF.equals(actual, 1e-9))
|
||||
|
||||
def test_RoundTrip(self):
|
||||
d = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
|
||||
hx = self.trueF.retract(d)
|
||||
actual = self.trueF.localCoordinates(hx)
|
||||
assert_almost_equal(d, actual, decimal=8)
|
||||
|
||||
|
||||
class TestSimpleStereo(unittest.TestCase):
|
||||
|
||||
def setUp(self):
|
||||
# Create the simplest SimpleFundamentalMatrix, a stereo pair
|
||||
self.defaultE = EssentialMatrix(Rot3(), Unit3(1, 0, 0))
|
||||
self.zero = Point2(0.0, 0.0)
|
||||
self.stereoF = SimpleFundamentalMatrix(self.defaultE, 1.0, 1.0, self.zero, self.zero)
|
||||
|
||||
def test_Conversion(self):
|
||||
convertedF = FundamentalMatrix(self.stereoF.matrix())
|
||||
assert_almost_equal(self.stereoF.matrix(), convertedF.matrix(), decimal=8)
|
||||
|
||||
def test_localCoordinates(self):
|
||||
expected = np.zeros(7)
|
||||
actual = self.stereoF.localCoordinates(self.stereoF)
|
||||
assert_almost_equal(expected, actual, decimal=8)
|
||||
|
||||
def test_retract(self):
|
||||
actual = self.stereoF.retract(np.zeros(9))
|
||||
self.assertTrue(self.stereoF.equals(actual, 1e-9))
|
||||
|
||||
def test_RoundTrip(self):
|
||||
d = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
|
||||
hx = self.stereoF.retract(d)
|
||||
actual = self.stereoF.localCoordinates(hx)
|
||||
assert_almost_equal(d, actual, decimal=8)
|
||||
|
||||
def test_EpipolarLine(self):
|
||||
# Create a point in b
|
||||
p_b = np.array([0, 2, 1])
|
||||
# Convert the point to a horizontal line in a
|
||||
l_a = self.stereoF.matrix() @ p_b
|
||||
# Check if the line is horizontal at height 2
|
||||
expected = np.array([0, -1, 2])
|
||||
assert_almost_equal(l_a, expected, decimal=8)
|
||||
|
||||
|
||||
class TestPixelStereo(unittest.TestCase):
|
||||
|
||||
def setUp(self):
|
||||
# Create a stereo pair in pixels, zero principal points
|
||||
self.focalLength = 1000.0
|
||||
self.defaultE = EssentialMatrix(Rot3(), Unit3(1, 0, 0))
|
||||
self.zero = Point2(0.0, 0.0)
|
||||
self.pixelStereo = SimpleFundamentalMatrix(
|
||||
self.defaultE, self.focalLength, self.focalLength, self.zero, self.zero
|
||||
)
|
||||
|
||||
def test_Conversion(self):
|
||||
expected = self.pixelStereo.matrix()
|
||||
convertedF = FundamentalMatrix(self.pixelStereo.matrix())
|
||||
# Check equality of F-matrices up to a scale
|
||||
actual = convertedF.matrix()
|
||||
scale = expected[1, 2] / actual[1, 2]
|
||||
actual *= scale
|
||||
assert_almost_equal(expected, actual, decimal=5)
|
||||
|
||||
def test_PointInBToHorizontalLineInA(self):
|
||||
# Create a point in b
|
||||
p_b = np.array([0, 300, 1])
|
||||
# Convert the point to a horizontal line in a
|
||||
l_a = self.pixelStereo.matrix() @ p_b
|
||||
# Check if the line is horizontal at height 0.3
|
||||
expected = np.array([0, -0.001, 0.3])
|
||||
assert_almost_equal(l_a, expected, decimal=8)
|
||||
|
||||
|
||||
class TestRotatedPixelStereo(unittest.TestCase):
|
||||
|
||||
def setUp(self):
|
||||
# Create a stereo pair with the right camera rotated 90 degrees
|
||||
self.focalLength = 1000.0
|
||||
self.zero = Point2(0.0, 0.0)
|
||||
self.aRb = Rot3.Rz(np.pi / 2) # Rotate 90 degrees around the Z-axis
|
||||
self.rotatedE = EssentialMatrix(self.aRb, Unit3(1, 0, 0))
|
||||
self.rotatedPixelStereo = SimpleFundamentalMatrix(
|
||||
self.rotatedE, self.focalLength, self.focalLength, self.zero, self.zero
|
||||
)
|
||||
|
||||
def test_Conversion(self):
|
||||
expected = self.rotatedPixelStereo.matrix()
|
||||
convertedF = FundamentalMatrix(self.rotatedPixelStereo.matrix())
|
||||
# Check equality of F-matrices up to a scale
|
||||
actual = convertedF.matrix()
|
||||
scale = expected[1, 2] / actual[1, 2]
|
||||
actual *= scale
|
||||
assert_almost_equal(expected, actual, decimal=4)
|
||||
|
||||
def test_PointInBToHorizontalLineInA(self):
|
||||
# Create a point in b
|
||||
p_b = np.array([300, 0, 1])
|
||||
# Convert the point to a horizontal line in a
|
||||
l_a = self.rotatedPixelStereo.matrix() @ p_b
|
||||
# Check if the line is horizontal at height 0.3
|
||||
expected = np.array([0, -0.001, 0.3])
|
||||
assert_almost_equal(l_a, expected, decimal=8)
|
||||
|
||||
|
||||
class TestStereoWithPrincipalPoints(unittest.TestCase):
|
||||
|
||||
def setUp(self):
|
||||
# Now check that principal points also survive conversion
|
||||
self.focalLength = 1000.0
|
||||
self.principalPoint = Point2(640 / 2, 480 / 2)
|
||||
self.aRb = Rot3.Rz(np.pi / 2)
|
||||
self.rotatedE = EssentialMatrix(self.aRb, Unit3(1, 0, 0))
|
||||
self.stereoWithPrincipalPoints = SimpleFundamentalMatrix(
|
||||
self.rotatedE, self.focalLength, self.focalLength, self.principalPoint, self.principalPoint
|
||||
)
|
||||
|
||||
def test_Conversion(self):
|
||||
expected = self.stereoWithPrincipalPoints.matrix()
|
||||
convertedF = FundamentalMatrix(self.stereoWithPrincipalPoints.matrix())
|
||||
# Check equality of F-matrices up to a scale
|
||||
actual = convertedF.matrix()
|
||||
scale = expected[1, 2] / actual[1, 2]
|
||||
actual *= scale
|
||||
assert_almost_equal(expected, actual, decimal=4)
|
||||
|
||||
|
||||
class TestTripleF(unittest.TestCase):
|
||||
|
||||
def setUp(self):
|
||||
# Generate three cameras on a circle, looking in
|
||||
self.cameraPoses = SFMdata.posesOnCircle(3, 1.0)
|
||||
self.focalLength = 1000.0
|
||||
self.principalPoint = Point2(640 / 2, 480 / 2)
|
||||
self.triplet = self.generateTripleF(self.cameraPoses)
|
||||
|
||||
def generateTripleF(self, cameraPoses):
|
||||
F = []
|
||||
for i in range(3):
|
||||
j = (i + 1) % 3
|
||||
iPj = cameraPoses[i].between(cameraPoses[j])
|
||||
E = EssentialMatrix(iPj.rotation(), Unit3(iPj.translation()))
|
||||
F_ij = SimpleFundamentalMatrix(
|
||||
E, self.focalLength, self.focalLength, self.principalPoint, self.principalPoint
|
||||
)
|
||||
F.append(F_ij)
|
||||
return {"Fab": F[0], "Fbc": F[1], "Fca": F[2]}
|
||||
|
||||
def transferToA(self, pb, pc):
|
||||
return gtsam.EpipolarTransfer(self.triplet["Fab"].matrix(), pb, self.triplet["Fca"].matrix().transpose(), pc)
|
||||
|
||||
def transferToB(self, pa, pc):
|
||||
return gtsam.EpipolarTransfer(self.triplet["Fab"].matrix().transpose(), pa, self.triplet["Fbc"].matrix(), pc)
|
||||
|
||||
def transferToC(self, pa, pb):
|
||||
return gtsam.EpipolarTransfer(self.triplet["Fca"].matrix(), pa, self.triplet["Fbc"].matrix().transpose(), pb)
|
||||
|
||||
def test_Transfer(self):
|
||||
triplet = self.triplet
|
||||
# Check that they are all equal
|
||||
self.assertTrue(triplet["Fab"].equals(triplet["Fbc"], 1e-9))
|
||||
self.assertTrue(triplet["Fbc"].equals(triplet["Fca"], 1e-9))
|
||||
self.assertTrue(triplet["Fca"].equals(triplet["Fab"], 1e-9))
|
||||
|
||||
# Now project a point into the three cameras
|
||||
P = Point3(0.1, 0.2, 0.3)
|
||||
K = Cal3_S2(self.focalLength, self.focalLength, 0.0, self.principalPoint[0], self.principalPoint[1])
|
||||
|
||||
p = []
|
||||
for i in range(3):
|
||||
# Project the point into each camera
|
||||
camera = PinholeCameraCal3_S2(self.cameraPoses[i], K)
|
||||
p_i = camera.project(P)
|
||||
p.append(p_i)
|
||||
|
||||
# Check that transfer works
|
||||
assert_almost_equal(p[0], self.transferToA(p[1], p[2]), decimal=9)
|
||||
assert_almost_equal(p[1], self.transferToB(p[0], p[2]), decimal=9)
|
||||
assert_almost_equal(p[2], self.transferToC(p[0], p[1]), decimal=9)
|
||||
|
||||
|
||||
class TestManyCamerasCircle(unittest.TestCase):
|
||||
N = 6
|
||||
|
||||
def setUp(self):
|
||||
# Generate six cameras on a circle, looking in
|
||||
self.cameraPoses = SFMdata.posesOnCircle(self.N, 1.0)
|
||||
self.focalLength = 1000.0
|
||||
self.principalPoint = Point2(640 / 2, 480 / 2)
|
||||
self.manyFs = self.generateManyFs(self.cameraPoses)
|
||||
|
||||
def generateManyFs(self, cameraPoses):
|
||||
F = []
|
||||
for i in range(self.N):
|
||||
j = (i + 1) % self.N
|
||||
iPj = cameraPoses[i].between(cameraPoses[j])
|
||||
E = EssentialMatrix(iPj.rotation(), Unit3(iPj.translation()))
|
||||
F_ij = SimpleFundamentalMatrix(
|
||||
E, self.focalLength, self.focalLength, self.principalPoint, self.principalPoint
|
||||
)
|
||||
F.append(F_ij)
|
||||
return F
|
||||
|
||||
def test_Conversion(self):
|
||||
for i in range(self.N):
|
||||
expected = self.manyFs[i].matrix()
|
||||
convertedF = FundamentalMatrix(self.manyFs[i].matrix())
|
||||
# Check equality of F-matrices up to a scale
|
||||
actual = convertedF.matrix()
|
||||
scale = expected[1, 2] / actual[1, 2]
|
||||
actual *= scale
|
||||
# print(f"\n{np.round(expected, 3)}", f"\n{np.round(actual, 3)}")
|
||||
assert_almost_equal(expected, actual, decimal=4)
|
||||
|
||||
def test_Transfer(self):
|
||||
# Now project a point into the six cameras
|
||||
P = Point3(0.1, 0.2, 0.3)
|
||||
K = Cal3_S2(self.focalLength, self.focalLength, 0.0, self.principalPoint[0], self.principalPoint[1])
|
||||
|
||||
p = []
|
||||
for i in range(self.N):
|
||||
# Project the point into each camera
|
||||
camera = PinholeCameraCal3_S2(self.cameraPoses[i], K)
|
||||
p_i = camera.project(P)
|
||||
p.append(p_i)
|
||||
|
||||
# Check that transfer works
|
||||
for a in range(self.N):
|
||||
b = (a + 1) % self.N
|
||||
c = (a + 2) % self.N
|
||||
# We transfer from a to b and from c to b,
|
||||
# and check that the result lines up with the projected point in b.
|
||||
transferred = gtsam.EpipolarTransfer(
|
||||
self.manyFs[a].matrix().transpose(), # need to transpose for a->b
|
||||
p[a],
|
||||
self.manyFs[c].matrix(),
|
||||
p[c],
|
||||
)
|
||||
assert_almost_equal(p[b], transferred, decimal=9)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Loading…
Reference in New Issue