Merge pull request #1312 from borglab/dsf-gtsfm-refactor
Frank Dellaert
GitHub
commit
05d4d9187d
|
|
@ -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 DsfTrackGenerator.cpp
|
||||
* @date October 2022
|
||||
* @author John Lambert
|
||||
* @brief Identifies connected components in the keypoint matches graph.
|
||||
*/
|
||||
|
||||
#include <gtsam/sfm/DsfTrackGenerator.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
namespace gtsfm {
|
||||
|
||||
typedef DSFMap<IndexPair> DSFMapIndexPair;
|
||||
|
||||
/// Generate the DSF to form tracks.
|
||||
static DSFMapIndexPair generateDSF(const MatchIndicesMap& matches) {
|
||||
DSFMapIndexPair dsf;
|
||||
|
||||
for (const auto& kv : matches) {
|
||||
const auto pair_indices = kv.first;
|
||||
const auto corr_indices = kv.second;
|
||||
|
||||
// Image pair is (i1,i2).
|
||||
size_t i1 = pair_indices.first;
|
||||
size_t i2 = pair_indices.second;
|
||||
for (size_t k = 0; k < corr_indices.rows(); k++) {
|
||||
// Measurement indices are found in a single matrix row, as (k1,k2).
|
||||
size_t k1 = corr_indices(k, 0), k2 = corr_indices(k, 1);
|
||||
// Unique key for DSF is (i,k), representing keypoint index in an image.
|
||||
dsf.merge({i1, k1}, {i2, k2});
|
||||
}
|
||||
}
|
||||
|
||||
return dsf;
|
||||
}
|
||||
|
||||
/// Generate a single track from a set of index pairs
|
||||
static SfmTrack2d trackFromIndexPairs(const std::set<IndexPair>& index_pair_set,
|
||||
const KeypointsVector& keypoints) {
|
||||
// Initialize track from measurements.
|
||||
SfmTrack2d track2d;
|
||||
|
||||
for (const auto& index_pair : index_pair_set) {
|
||||
// Camera index is represented by i, and measurement index is
|
||||
// represented by k.
|
||||
size_t i = index_pair.i();
|
||||
size_t k = index_pair.j();
|
||||
// Add measurement to this track.
|
||||
track2d.addMeasurement(i, keypoints[i].coordinates.row(k));
|
||||
}
|
||||
|
||||
return track2d;
|
||||
}
|
||||
|
||||
/// Generate tracks from the DSF.
|
||||
static std::vector<SfmTrack2d> tracksFromDSF(const DSFMapIndexPair& dsf,
|
||||
const KeypointsVector& keypoints) {
|
||||
const std::map<IndexPair, std::set<IndexPair> > key_sets = dsf.sets();
|
||||
|
||||
// Return immediately if no sets were found.
|
||||
if (key_sets.empty()) return {};
|
||||
|
||||
// Create a list of tracks.
|
||||
// Each track will be represented as a list of (camera_idx, measurements).
|
||||
std::vector<SfmTrack2d> tracks2d;
|
||||
for (const auto& kv : key_sets) {
|
||||
// Initialize track from measurements.
|
||||
SfmTrack2d track2d = trackFromIndexPairs(kv.second, keypoints);
|
||||
tracks2d.emplace_back(track2d);
|
||||
}
|
||||
return tracks2d;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Creates a list of tracks from 2d point correspondences.
|
||||
*
|
||||
* Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches.
|
||||
* We create a singleton for union-find set elements from camera index of a
|
||||
* detection and the index of that detection in that camera's keypoint list,
|
||||
* i.e. (i,k).
|
||||
*
|
||||
* @param Map from (i1,i2) image pair indices to (K,2) matrix, for K
|
||||
* correspondence indices, from each image.
|
||||
* @param Length-N list of keypoints, for N images/cameras.
|
||||
*/
|
||||
std::vector<SfmTrack2d> tracksFromPairwiseMatches(
|
||||
const MatchIndicesMap& matches, const KeypointsVector& keypoints,
|
||||
bool verbose) {
|
||||
// Generate the DSF to form tracks.
|
||||
if (verbose) std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl;
|
||||
DSFMapIndexPair dsf = generateDSF(matches);
|
||||
if (verbose) std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl;
|
||||
|
||||
std::vector<SfmTrack2d> tracks2d = tracksFromDSF(dsf, keypoints);
|
||||
|
||||
// Filter out erroneous tracks that had repeated measurements within the
|
||||
// same image. This is an expected result from an incorrect correspondence
|
||||
// slipping through.
|
||||
std::vector<SfmTrack2d> validTracks;
|
||||
std::copy_if(
|
||||
tracks2d.begin(), tracks2d.end(), std::back_inserter(validTracks),
|
||||
[](const SfmTrack2d& track2d) { return track2d.hasUniqueCameras(); });
|
||||
|
||||
if (verbose) {
|
||||
size_t erroneous_track_count = tracks2d.size() - validTracks.size();
|
||||
double erroneous_percentage = static_cast<float>(erroneous_track_count) /
|
||||
static_cast<float>(tracks2d.size()) * 100;
|
||||
|
||||
std::cout << std::fixed << std::setprecision(2);
|
||||
std::cout << "DSF Union-Find: " << erroneous_percentage;
|
||||
std::cout << "% of tracks discarded from multiple obs. in a single image."
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
// TODO(johnwlambert): return the Transitivity failure percentage here.
|
||||
return tracks2d;
|
||||
}
|
||||
|
||||
} // namespace gtsfm
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,78 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 DsfTrackGenerator.h
|
||||
* @date July 2022
|
||||
* @author John Lambert
|
||||
* @brief Identifies connected components in the keypoint matches graph.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <gtsam/base/DSFMap.h>
|
||||
#include <gtsam/sfm/SfmTrack.h>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <map>
|
||||
#include <optional>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
namespace gtsfm {
|
||||
|
||||
typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array
|
||||
|
||||
// Output of detections in an image.
|
||||
// Coordinate system convention:
|
||||
// 1. The x coordinate denotes the horizontal direction (+ve direction towards
|
||||
// the right).
|
||||
// 2. The y coordinate denotes the vertical direction (+ve direction downwards).
|
||||
// 3. Origin is at the top left corner of the image.
|
||||
struct Keypoints {
|
||||
// The (x, y) coordinates of the features, of shape Nx2.
|
||||
Eigen::MatrixX2d coordinates;
|
||||
|
||||
// Optional scale of the detections, of shape N.
|
||||
// Note: gtsam::Vector is typedef'd for Eigen::VectorXd.
|
||||
boost::optional<gtsam::Vector> scales;
|
||||
|
||||
/// Optional confidences/responses for each detection, of shape N.
|
||||
boost::optional<gtsam::Vector> responses;
|
||||
|
||||
Keypoints(const Eigen::MatrixX2d& coordinates)
|
||||
: coordinates(coordinates){}; // boost::none
|
||||
};
|
||||
|
||||
using KeypointsVector = std::vector<Keypoints>;
|
||||
// Mapping from each image pair to (N,2) array representing indices of matching
|
||||
// keypoints.
|
||||
using MatchIndicesMap = std::map<IndexPair, CorrespondenceIndices>;
|
||||
|
||||
/**
|
||||
* @brief Creates a list of tracks from 2d point correspondences.
|
||||
*
|
||||
* Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches.
|
||||
* We create a singleton for union-find set elements from camera index of a
|
||||
* detection and the index of that detection in that camera's keypoint list,
|
||||
* i.e. (i,k).
|
||||
*
|
||||
* @param Map from (i1,i2) image pair indices to (K,2) matrix, for K
|
||||
* correspondence indices, from each image.
|
||||
* @param Length-N list of keypoints, for N images/cameras.
|
||||
*/
|
||||
std::vector<SfmTrack2d> tracksFromPairwiseMatches(
|
||||
const MatchIndicesMap& matches, const KeypointsVector& keypoints,
|
||||
bool verbose = false);
|
||||
|
||||
} // namespace gtsfm
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -22,6 +22,7 @@
|
|||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
|
@ -35,28 +36,26 @@ typedef std::pair<size_t, Point2> SfmMeasurement;
|
|||
typedef std::pair<size_t, size_t> SiftIndex;
|
||||
|
||||
/**
|
||||
* @brief An SfmTrack stores SfM measurements grouped in a track
|
||||
* @addtogroup sfm
|
||||
* @brief Track containing 2D measurements associated with a single 3D point.
|
||||
* Note: Equivalent to gtsam.SfmTrack, but without the 3d measurement.
|
||||
* This class holds data temporarily before 3D point is initialized.
|
||||
*/
|
||||
struct GTSAM_EXPORT SfmTrack {
|
||||
Point3 p; ///< 3D position of the point
|
||||
float r, g, b; ///< RGB color of the 3D point
|
||||
|
||||
struct GTSAM_EXPORT SfmTrack2d {
|
||||
/// The 2D image projections (id,(u,v))
|
||||
std::vector<SfmMeasurement> measurements;
|
||||
|
||||
/// The feature descriptors
|
||||
/// The feature descriptors (optional)
|
||||
std::vector<SiftIndex> siftIndices;
|
||||
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
explicit SfmTrack(float r = 0, float g = 0, float b = 0)
|
||||
: p(0, 0, 0), r(r), g(g), b(b) {}
|
||||
// Default constructor.
|
||||
SfmTrack2d() = default;
|
||||
|
||||
explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0,
|
||||
float b = 0)
|
||||
: p(pt), r(r), g(g), b(b) {}
|
||||
// Constructor from measurements.
|
||||
explicit SfmTrack2d(const std::vector<SfmMeasurement>& measurements)
|
||||
: measurements(measurements) {}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
|
|
@ -78,6 +77,70 @@ struct GTSAM_EXPORT SfmTrack {
|
|||
/// Get the SIFT feature index corresponding to the measurement at `idx`
|
||||
const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; }
|
||||
|
||||
/**
|
||||
* @brief Check that no two measurements are from the same camera.
|
||||
* @returns boolean result of the validation.
|
||||
*/
|
||||
bool hasUniqueCameras() const {
|
||||
std::vector<int> track_cam_indices;
|
||||
for (auto& measurement : measurements) {
|
||||
track_cam_indices.emplace_back(measurement.first);
|
||||
}
|
||||
auto i =
|
||||
std::adjacent_find(track_cam_indices.begin(), track_cam_indices.end());
|
||||
bool all_cameras_unique = (i == track_cam_indices.end());
|
||||
return all_cameras_unique;
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Vectorized Interface
|
||||
/// @{
|
||||
|
||||
/// @brief Return the measurements as a 2D matrix
|
||||
Eigen::MatrixX2d measurementMatrix() const {
|
||||
Eigen::MatrixX2d m(numberMeasurements(), 2);
|
||||
for (size_t i = 0; i < numberMeasurements(); i++) {
|
||||
m.row(i) = measurement(i).second;
|
||||
}
|
||||
return m;
|
||||
}
|
||||
|
||||
/// @brief Return the camera indices of the measurements
|
||||
Eigen::VectorXi indexVector() const {
|
||||
Eigen::VectorXi v(numberMeasurements());
|
||||
for (size_t i = 0; i < numberMeasurements(); i++) {
|
||||
v(i) = measurement(i).first;
|
||||
}
|
||||
return v;
|
||||
}
|
||||
|
||||
/// @}
|
||||
};
|
||||
|
||||
using SfmTrack2dVector = std::vector<SfmTrack2d>;
|
||||
|
||||
/**
|
||||
* @brief An SfmTrack stores SfM measurements grouped in a track
|
||||
* @addtogroup sfm
|
||||
*/
|
||||
struct GTSAM_EXPORT SfmTrack : SfmTrack2d {
|
||||
Point3 p; ///< 3D position of the point
|
||||
float r, g, b; ///< RGB color of the 3D point
|
||||
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
explicit SfmTrack(float r = 0, float g = 0, float b = 0)
|
||||
: p(0, 0, 0), r(r), g(g), b(b) {}
|
||||
|
||||
explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0,
|
||||
float b = 0)
|
||||
: p(pt), r(r), g(g), b(b) {}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// Get 3D point
|
||||
const Point3& point3() const { return p; }
|
||||
|
||||
|
|
|
|||
|
|
@ -4,10 +4,23 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/sfm/SfmTrack.h>
|
||||
class SfmTrack {
|
||||
class SfmTrack2d {
|
||||
std::vector<pair<size_t, gtsam::Point2>> measurements;
|
||||
|
||||
SfmTrack2d();
|
||||
SfmTrack2d(const std::vector<gtsam::SfmMeasurement>& measurements);
|
||||
size_t numberMeasurements() const;
|
||||
pair<size_t, gtsam::Point2> measurement(size_t idx) const;
|
||||
pair<size_t, size_t> siftIndex(size_t idx) const;
|
||||
void addMeasurement(size_t idx, const gtsam::Point2& m);
|
||||
gtsam::SfmMeasurement measurement(size_t idx) const;
|
||||
bool hasUniqueCameras() const;
|
||||
Eigen::MatrixX2d measurementMatrix() const;
|
||||
Eigen::VectorXi indexVector() const;
|
||||
};
|
||||
|
||||
virtual class SfmTrack : gtsam::SfmTrack2d {
|
||||
SfmTrack();
|
||||
SfmTrack(const gtsam::Point3& pt);
|
||||
const Point3& point3() const;
|
||||
|
|
@ -18,13 +31,6 @@ class SfmTrack {
|
|||
double g;
|
||||
double b;
|
||||
|
||||
std::vector<pair<size_t, gtsam::Point2>> measurements;
|
||||
|
||||
size_t numberMeasurements() const;
|
||||
pair<size_t, gtsam::Point2> measurement(size_t idx) const;
|
||||
pair<size_t, size_t> siftIndex(size_t idx) const;
|
||||
void addMeasurement(size_t idx, const gtsam::Point2& m);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
|
||||
|
|
@ -32,6 +38,8 @@ class SfmTrack {
|
|||
bool equals(const gtsam::SfmTrack& expected, double tol) const;
|
||||
};
|
||||
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/sfm/SfmData.h>
|
||||
class SfmData {
|
||||
SfmData();
|
||||
|
|
@ -115,7 +123,7 @@ class BinaryMeasurementsRot3 {
|
|||
|
||||
#include <gtsam/sfm/ShonanAveraging.h>
|
||||
|
||||
// TODO(frank): copy/pasta below until we have integer template paremeters in
|
||||
// TODO(frank): copy/pasta below until we have integer template parameters in
|
||||
// wrap!
|
||||
|
||||
class ShonanAveragingParameters2 {
|
||||
|
|
@ -310,4 +318,38 @@ class TranslationRecovery {
|
|||
const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const;
|
||||
};
|
||||
|
||||
namespace gtsfm {
|
||||
|
||||
#include <gtsam/sfm/DsfTrackGenerator.h>
|
||||
|
||||
class MatchIndicesMap {
|
||||
MatchIndicesMap();
|
||||
MatchIndicesMap(const gtsam::gtsfm::MatchIndicesMap& other);
|
||||
|
||||
size_t size() const;
|
||||
bool empty() const;
|
||||
void clear();
|
||||
gtsam::gtsfm::CorrespondenceIndices at(const pair<size_t, size_t>& keypair) const;
|
||||
};
|
||||
|
||||
class Keypoints {
|
||||
Keypoints(const Eigen::MatrixX2d& coordinates);
|
||||
Eigen::MatrixX2d coordinates;
|
||||
};
|
||||
|
||||
class KeypointsVector {
|
||||
KeypointsVector();
|
||||
KeypointsVector(const gtsam::gtsfm::KeypointsVector& other);
|
||||
void push_back(const gtsam::gtsfm::Keypoints& keypoints);
|
||||
size_t size() const;
|
||||
bool empty() const;
|
||||
void clear();
|
||||
gtsam::gtsfm::Keypoints at(const size_t& index) const;
|
||||
};
|
||||
|
||||
gtsam::SfmTrack2dVector tracksFromPairwiseMatches(
|
||||
const gtsam::gtsfm::MatchIndicesMap& matches_dict,
|
||||
const gtsam::gtsfm::KeypointsVector& keypoints_list, bool verbose = false);
|
||||
} // namespace gtsfm
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -0,0 +1,53 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 TestSfmTrack.cpp
|
||||
* @date October 2022
|
||||
* @author Frank Dellaert
|
||||
* @brief tests for SfmTrack class
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/sfm/SfmTrack.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SfmTrack2d, defaultConstructor) {
|
||||
SfmTrack2d track;
|
||||
EXPECT_LONGS_EQUAL(0, track.measurements.size());
|
||||
EXPECT_LONGS_EQUAL(0, track.siftIndices.size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SfmTrack2d, measurementConstructor) {
|
||||
SfmTrack2d track({{0, Point2(1, 2)}, {1, Point2(3, 4)}});
|
||||
EXPECT_LONGS_EQUAL(2, track.measurements.size());
|
||||
EXPECT_LONGS_EQUAL(0, track.siftIndices.size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SfmTrack, construction) {
|
||||
SfmTrack track(Point3(1, 2, 3), 4, 5, 6);
|
||||
EXPECT(assert_equal(Point3(1, 2, 3), track.point3()));
|
||||
EXPECT(assert_equal(Point3(4, 5, 6), track.rgb()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -51,7 +51,10 @@ set(ignore
|
|||
gtsam::BinaryMeasurementsUnit3
|
||||
gtsam::BinaryMeasurementsRot3
|
||||
gtsam::DiscreteKey
|
||||
gtsam::KeyPairDoubleMap)
|
||||
gtsam::KeyPairDoubleMap
|
||||
gtsam::gtsfm::MatchIndicesMap
|
||||
gtsam::gtsfm::KeypointsVector
|
||||
gtsam::gtsfm::SfmTrack2dVector)
|
||||
|
||||
set(interface_headers
|
||||
${PROJECT_SOURCE_DIR}/gtsam/gtsam.i
|
||||
|
|
@ -148,8 +151,12 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
|
|||
gtsam::CameraSetCal3Bundler
|
||||
gtsam::CameraSetCal3Unified
|
||||
gtsam::CameraSetCal3Fisheye
|
||||
gtsam::KeyPairDoubleMap)
|
||||
|
||||
gtsam::KeyPairDoubleMap
|
||||
gtsam::gtsfm::MatchIndicesMap
|
||||
gtsam::gtsfm::KeypointsVector
|
||||
gtsam::gtsfm::SfmTrack2dVector)
|
||||
|
||||
|
||||
pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target
|
||||
${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header
|
||||
"gtsam_unstable.cpp" # generated_cpp
|
||||
|
|
|
|||
|
|
@ -0,0 +1,4 @@
|
|||
# This trick is to allow direct import of sub-modules
|
||||
# without this, we can only do `from gtsam.gtsam.gtsfm import X`
|
||||
# with this trick, we can do `from gtsam.gtsfm import X`
|
||||
from .gtsam.gtsfm import *
|
||||
|
|
@ -15,12 +15,13 @@
|
|||
// #include <pybind11/stl.h>
|
||||
#include <pybind11/stl_bind.h>
|
||||
|
||||
PYBIND11_MAKE_OPAQUE(
|
||||
std::vector<gtsam::SfmTrack>);
|
||||
|
||||
PYBIND11_MAKE_OPAQUE(
|
||||
std::vector<gtsam::SfmCamera>);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SfmMeasurement>);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SfmTrack>);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SfmCamera>);
|
||||
PYBIND11_MAKE_OPAQUE(
|
||||
std::vector<gtsam::BinaryMeasurement<gtsam::Unit3>>);
|
||||
PYBIND11_MAKE_OPAQUE(
|
||||
std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>);
|
||||
PYBIND11_MAKE_OPAQUE(
|
||||
std::vector<gtsam::gtsfm::Keypoints>);
|
||||
PYBIND11_MAKE_OPAQUE(gtsam::gtsfm::MatchIndicesMap);
|
||||
|
|
@ -18,16 +18,11 @@ py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
|
|||
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Rot3> > >(
|
||||
m_, "BinaryMeasurementsRot3");
|
||||
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
|
||||
py::bind_vector<std::vector<gtsam::SfmTrack2d>>(m_, "SfmTrack2dVector");
|
||||
py::bind_vector<std::vector<gtsam::SfmTrack>>(m_, "SfmTracks");
|
||||
py::bind_vector<std::vector<gtsam::SfmCamera>>(m_, "SfmCameras");
|
||||
py::bind_vector<std::vector<std::pair<size_t, gtsam::Point2>>>(
|
||||
m_, "SfmMeasurementVector");
|
||||
|
||||
py::bind_vector<
|
||||
std::vector<gtsam::SfmTrack> >(
|
||||
m_, "SfmTracks");
|
||||
|
||||
py::bind_vector<
|
||||
std::vector<gtsam::SfmCamera> >(
|
||||
m_, "SfmCameras");
|
||||
|
||||
py::bind_vector<
|
||||
std::vector<std::pair<size_t, gtsam::Point2>>>(
|
||||
m_, "SfmMeasurementVector"
|
||||
);
|
||||
py::bind_map<gtsam::gtsfm::MatchIndicesMap>(m_, "MatchIndicesMap");
|
||||
py::bind_vector<std::vector<gtsam::gtsfm::Keypoints>>(m_, "KeypointsVector");
|
||||
|
|
|
|||
|
|
@ -15,8 +15,7 @@ from __future__ import print_function
|
|||
import unittest
|
||||
from typing import Tuple
|
||||
|
||||
import gtsam
|
||||
from gtsam import IndexPair
|
||||
from gtsam import DSFMapIndexPair, IndexPair, IndexPairSetAsArray
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
|
|
@ -29,10 +28,10 @@ class TestDSFMap(GtsamTestCase):
|
|||
def key(index_pair) -> Tuple[int, int]:
|
||||
return index_pair.i(), index_pair.j()
|
||||
|
||||
dsf = gtsam.DSFMapIndexPair()
|
||||
pair1 = gtsam.IndexPair(1, 18)
|
||||
dsf = DSFMapIndexPair()
|
||||
pair1 = IndexPair(1, 18)
|
||||
self.assertEqual(key(dsf.find(pair1)), key(pair1))
|
||||
pair2 = gtsam.IndexPair(2, 2)
|
||||
pair2 = IndexPair(2, 2)
|
||||
|
||||
# testing the merge feature of dsf
|
||||
dsf.merge(pair1, pair2)
|
||||
|
|
@ -45,7 +44,7 @@ class TestDSFMap(GtsamTestCase):
|
|||
k'th detected keypoint in image i. For the data below, merging such
|
||||
measurements into feature tracks across frames should create 2 distinct sets.
|
||||
"""
|
||||
dsf = gtsam.DSFMapIndexPair()
|
||||
dsf = DSFMapIndexPair()
|
||||
dsf.merge(IndexPair(0, 1), IndexPair(1, 2))
|
||||
dsf.merge(IndexPair(0, 1), IndexPair(3, 4))
|
||||
dsf.merge(IndexPair(4, 5), IndexPair(6, 8))
|
||||
|
|
@ -56,7 +55,7 @@ class TestDSFMap(GtsamTestCase):
|
|||
for i in sets:
|
||||
set_keys = []
|
||||
s = sets[i]
|
||||
for val in gtsam.IndexPairSetAsArray(s):
|
||||
for val in IndexPairSetAsArray(s):
|
||||
set_keys.append((val.i(), val.j()))
|
||||
merged_sets.add(tuple(set_keys))
|
||||
|
||||
|
|
@ -0,0 +1,96 @@
|
|||
"""Unit tests for track generation using a Disjoint Set Forest data structure.
|
||||
|
||||
Authors: John Lambert
|
||||
"""
|
||||
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam import IndexPair, KeypointsVector, MatchIndicesMap, Point2, SfmMeasurementVector, SfmTrack2d
|
||||
from gtsam.gtsfm import Keypoints
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestDsfTrackGenerator(GtsamTestCase):
|
||||
"""Tests for DsfTrackGenerator."""
|
||||
|
||||
def test_track_generation(self) -> None:
|
||||
"""Ensures that DSF generates three tracks from measurements
|
||||
in 3 images (H=200,W=400)."""
|
||||
kps_i0 = Keypoints(np.array([[10.0, 20], [30, 40]]))
|
||||
kps_i1 = Keypoints(np.array([[50.0, 60], [70, 80], [90, 100]]))
|
||||
kps_i2 = Keypoints(np.array([[110.0, 120], [130, 140]]))
|
||||
|
||||
keypoints_list = KeypointsVector()
|
||||
keypoints_list.append(kps_i0)
|
||||
keypoints_list.append(kps_i1)
|
||||
keypoints_list.append(kps_i2)
|
||||
|
||||
# For each image pair (i1,i2), we provide a (K,2) matrix
|
||||
# of corresponding image indices (k1,k2).
|
||||
matches_dict = MatchIndicesMap()
|
||||
matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]])
|
||||
matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]])
|
||||
|
||||
tracks = gtsam.gtsfm.tracksFromPairwiseMatches(
|
||||
matches_dict,
|
||||
keypoints_list,
|
||||
verbose=False,
|
||||
)
|
||||
assert len(tracks) == 3
|
||||
|
||||
# Verify track 0.
|
||||
track0 = tracks[0]
|
||||
assert track0.numberMeasurements() == 2
|
||||
np.testing.assert_allclose(track0.measurements[0][1], Point2(10, 20))
|
||||
np.testing.assert_allclose(track0.measurements[1][1], Point2(50, 60))
|
||||
assert track0.measurements[0][0] == 0
|
||||
assert track0.measurements[1][0] == 1
|
||||
np.testing.assert_allclose(
|
||||
track0.measurementMatrix(),
|
||||
[
|
||||
[10, 20],
|
||||
[50, 60],
|
||||
],
|
||||
)
|
||||
np.testing.assert_allclose(track0.indexVector(), [0, 1])
|
||||
|
||||
# Verify track 1.
|
||||
track1 = tracks[1]
|
||||
np.testing.assert_allclose(
|
||||
track1.measurementMatrix(),
|
||||
[
|
||||
[30, 40],
|
||||
[70, 80],
|
||||
[130, 140],
|
||||
],
|
||||
)
|
||||
np.testing.assert_allclose(track1.indexVector(), [0, 1, 2])
|
||||
|
||||
# Verify track 2.
|
||||
track2 = tracks[2]
|
||||
np.testing.assert_allclose(
|
||||
track2.measurementMatrix(),
|
||||
[
|
||||
[90, 100],
|
||||
[110, 120],
|
||||
],
|
||||
)
|
||||
np.testing.assert_allclose(track2.indexVector(), [1, 2])
|
||||
|
||||
|
||||
class TestSfmTrack2d(GtsamTestCase):
|
||||
"""Tests for SfmTrack2d."""
|
||||
|
||||
def test_sfm_track_2d_constructor(self) -> None:
|
||||
""" """
|
||||
measurements = SfmMeasurementVector()
|
||||
measurements.append((0, Point2(10, 20)))
|
||||
track = SfmTrack2d(measurements=measurements)
|
||||
track.measurement(0)
|
||||
track.numberMeasurements() == 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Loading…
Reference in New Issue