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

Use SfmMeasurement and SfmTrack

release/4.3a0
Frank Dellaert 2022-10-22 18:37:44 -07:00
parent cafa3c556c
commit 38be12eaf4
6 changed files with 144 additions and 250 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

133
gtsam/sfm/DsfTrackGenerator.h
View File

@ -17,11 +17,11 @@
*/ */
#pragma once #pragma once
#include <Eigen/Core>
#include <gtsam/base/DSFMap.h> #include <gtsam/base/DSFMap.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/sfm/SfmTrack.h>
#include <Eigen/Core>
#include <algorithm> #include <algorithm>
#include <iostream> #include <iostream>
#include <map> #include <map>
@ -35,17 +35,16 @@ namespace gtsfm {
typedef DSFMap<IndexPair> DSFMapIndexPair; typedef DSFMap<IndexPair> DSFMapIndexPair;
typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array
//struct Keypoints; // struct Keypoints;
using KeypointCoordinates = Eigen::MatrixX2d; using KeypointCoordinates = Eigen::MatrixX2d;
// Output of detections in an image. // Output of detections in an image.
// Coordinate system convention: // Coordinate system convention:
// 1. The x coordinate denotes the horizontal direction (+ve direction towards the right). // 1. The x coordinate denotes the horizontal direction (+ve direction towards
// the right).
// 2. The y coordinate denotes the vertical direction (+ve direction downwards). // 2. The y coordinate denotes the vertical direction (+ve direction downwards).
// 3. Origin is at the top left corner of the image. // 3. Origin is at the top left corner of the image.
struct Keypoints { struct Keypoints {
// The (x, y) coordinates of the features, of shape Nx2. // The (x, y) coordinates of the features, of shape Nx2.
KeypointCoordinates coordinates; KeypointCoordinates coordinates;
@ -56,80 +55,20 @@ struct Keypoints {
/// Optional confidences/responses for each detection, of shape N. /// Optional confidences/responses for each detection, of shape N.
boost::optional<gtsam::Vector> responses; boost::optional<gtsam::Vector> responses;
Keypoints(const gtsam::gtsfm::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none Keypoints(const KeypointCoordinates& coordinates)
: coordinates(coordinates){}; // boost::none
}; };
using KeypointsVector = std::vector<Keypoints>; using KeypointsVector = std::vector<Keypoints>;
// Mapping from each image pair to (N,2) array representing indices of matching keypoints. // Mapping from each image pair to (N,2) array representing indices of matching
// keypoints.
using MatchIndicesMap = std::map<IndexPair, CorrespondenceIndices>; using MatchIndicesMap = std::map<IndexPair, CorrespondenceIndices>;
// @param camera index
// @param 2d measurement
// Implemented as named tuple, instead of std::pair (like SfmMeasurement in SfmTrack.h)
struct NamedSfmMeasurement {
size_t i;
gtsam::Point2 uv;
NamedSfmMeasurement(size_t i, gtsam::Point2 uv) : i(i), uv(uv) {}
};
/** /**
* @brief Track containing 2D measurements associated with a single 3D point. * @brief Generates point tracks from connected components in the keypoint
* Note: Equivalent to gtsam.SfmTrack, but without the 3d measurement. * matches graph.
* This class holds data temporarily before 3D point is initialized.
*/
class SfmTrack2d {
private:
std::vector<NamedSfmMeasurement> measurements_;
public:
// Default constructor.
SfmTrack2d() = default;
// Constructor from measurements.
SfmTrack2d(std::vector<NamedSfmMeasurement> &measurements) : measurements_(measurements) {}
// Add a measurement to the track.
void addMeasurement(const NamedSfmMeasurement &m) {
measurements_.emplace_back(m);
}
/// The measurement at index `idx`
NamedSfmMeasurement measurement(size_t idx) const { return measurements_[idx]; }
// Return all measurements in the track.
std::vector<NamedSfmMeasurement> measurements() {return measurements_; }
/// Total number of measurements in this track.
size_t numberMeasurements() const { return measurements_.size(); }
// @brief Validates the track by checking that no two measurements are from the same camera.
//
// returns boolean result of the validation.
bool hasUniqueCameras()
{
std::vector<int> track_cam_idxs;
for (auto & measurement : measurements_)
{
track_cam_idxs.emplace_back(measurement.i);
}
auto i = std::adjacent_find(track_cam_idxs.begin(), track_cam_idxs.end());
bool all_cameras_unique = (i == track_cam_idxs.end());
return all_cameras_unique;
}
};
using SfmTrack2dVector = std::vector<gtsam::gtsfm::SfmTrack2d>;
using NamedSfmMeasurementVector = std::vector<NamedSfmMeasurement>;
/**
* @brief Generates point tracks from connected components in the keypoint matches graph.
*/ */
class DsfTrackGenerator { class DsfTrackGenerator {
public: public:
/** Default constructor. */ /** Default constructor. */
DsfTrackGenerator() {} DsfTrackGenerator() {}
@ -145,27 +84,27 @@ class DsfTrackGenerator {
// @param Length-N list of keypoints, for N images/cameras. // @param Length-N list of keypoints, for N images/cameras.
std::vector<SfmTrack2d> generate_tracks_from_pairwise_matches( std::vector<SfmTrack2d> generate_tracks_from_pairwise_matches(
const MatchIndicesMap& matches_dict, const MatchIndicesMap& matches_dict,
const KeypointsVector& keypoints_list, const KeypointsVector& keypoints_list, bool verbose = false) {
bool verbose = false) {
std::vector<SfmTrack2d> track_2d_list; std::vector<SfmTrack2d> track_2d_list;
if (verbose) std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; if (verbose)
std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl;
// Generate the DSF to form tracks. // Generate the DSF to form tracks.
DSFMapIndexPair dsf; DSFMapIndexPair dsf;
for (const auto& kv : matches_dict) { for (const auto& kv : matches_dict) {
const auto pair_idxs = kv.first; const auto pair_indices = kv.first;
const auto corr_idxs = kv.second; const auto corr_indices = kv.second;
// Image pair is (i1,i2). // Image pair is (i1,i2).
size_t i1 = pair_idxs.first; size_t i1 = pair_indices.first;
size_t i2 = pair_idxs.second; size_t i2 = pair_indices.second;
for (size_t k = 0; k < corr_idxs.rows(); k++) for (size_t k = 0; k < corr_indices.rows(); k++) {
{
// Measurement indices are found in a single matrix row, as (k1,k2). // Measurement indices are found in a single matrix row, as (k1,k2).
size_t k1 = corr_idxs(k, 0); size_t k1 = corr_indices(k, 0);
size_t k2 = corr_idxs(k, 1); size_t k2 = corr_indices(k, 1);
// Unique keys for the DSF are (i,k), representing keypoint index in an image. // Unique keys for the DSF are (i,k), representing keypoint index in an
// image.
dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2)); dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2));
} }
} }
@ -186,19 +125,19 @@ class DsfTrackGenerator {
// Initialize track from measurements. // Initialize track from measurements.
SfmTrack2d track_2d; SfmTrack2d track_2d;
for (const auto& index_pair : index_pair_set) for (const auto& index_pair : index_pair_set) {
{ // Camera index is represented by i, and measurement index is
// Camera index is represented by i, and measurement index is represented by k. // represented by k.
size_t i = index_pair.i(); size_t i = index_pair.i();
size_t k = index_pair.j(); size_t k = index_pair.j();
// Add measurement to this track. // Add measurement to this track.
track_2d.addMeasurement(NamedSfmMeasurement(i, keypoints_list[i].coordinates.row(k))); track_2d.addMeasurement(i, keypoints_list[i].coordinates.row(k));
} }
// Skip erroneous track that had repeated measurements within the same image. // Skip erroneous track that had repeated measurements within the same
// This is an expected result from an incorrect correspondence slipping through. // image. This is an expected result from an incorrect correspondence
if (track_2d.hasUniqueCameras()) // slipping through.
{ if (track_2d.hasUniqueCameras()) {
track_2d_list.emplace_back(track_2d); track_2d_list.emplace_back(track_2d);
} else { } else {
erroneous_track_count++; erroneous_track_count++;
@ -206,19 +145,19 @@ class DsfTrackGenerator {
} }
if (verbose) { if (verbose) {
double erroneous_track_pct = static_cast<float>(erroneous_track_count) double erroneous_track_pct = static_cast<float>(erroneous_track_count) /
/ static_cast<float>(key_sets.size()) * 100; static_cast<float>(key_sets.size()) * 100;
// TODO(johnwlambert): restrict decimal places to 2 decimals. // TODO(johnwlambert): restrict decimal places to 2 decimals.
std::cout << "DSF Union-Find: " << erroneous_track_pct; std::cout << "DSF Union-Find: " << erroneous_track_pct;
std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; std::cout << "% of tracks discarded from multiple obs. in a single image."
<< std::endl;
} }
// TODO(johnwlambert): return the Transitivity failure percentage here. // TODO(johnwlambert): return the Transitivity failure percentage here.
return track_2d_list; return track_2d_list;
} }
}; };
}///\namespace gtsfm } // namespace gtsfm
} // namespace gtsam } // namespace gtsam

2
gtsam/sfm/SfmTrack.h
View File

@ -80,7 +80,7 @@ struct GTSAM_EXPORT SfmTrack2d {
* @brief Validates the track by checking that no two measurements are from * @brief Validates the track by checking that no two measurements are from
* @returns boolean result of the validation. * @returns boolean result of the validation.
*/ */
bool hasUniqueCameras() { bool hasUniqueCameras() const {
std::vector<int> track_cam_indices; std::vector<int> track_cam_indices;
for (auto& measurement : measurements) { for (auto& measurement : measurements) {
track_cam_indices.emplace_back(measurement.first); track_cam_indices.emplace_back(measurement.first);

139
gtsam/sfm/sfm.i
View File

@ -4,91 +4,22 @@
namespace gtsam { namespace gtsam {
namespace gtsfm { #include <gtsam/sfm/SfmTrack.h>
#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 gtsam::gtsfm::KeypointCoordinates& coordinates);
gtsam::gtsfm::KeypointCoordinates coordinates;
}; // check if this should be a method
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;
};
class NamedSfmMeasurement
{
size_t i;
gtsam::Point2 uv;
NamedSfmMeasurement(size_t i, gtsam::Point2 uv);
};
class NamedSfmMeasurementVector {
NamedSfmMeasurementVector();
NamedSfmMeasurementVector(const gtsam::gtsfm::NamedSfmMeasurementVector& other);
void push_back(const gtsam::gtsfm::NamedSfmMeasurement& measurement);
size_t size() const;
bool empty() const;
void clear();
gtsam::gtsfm::NamedSfmMeasurement at(const size_t& index) const;
};
class SfmTrack2d class SfmTrack2d
{ {
std::vector<pair<size_t, gtsam::Point2>> measurements;
SfmTrack2d(); SfmTrack2d();
SfmTrack2d(std::vector<gtsam::gtsfm::NamedSfmMeasurement> &measurements); SfmTrack2d(std::vector<gtsam::SfmMeasurement> &measurements);
size_t numberMeasurements() const; size_t numberMeasurements() const;
void addMeasurement(const gtsam::gtsfm::NamedSfmMeasurement &m); pair<size_t, gtsam::Point2> measurement(size_t idx) const;
std::vector<gtsam::gtsfm::NamedSfmMeasurement> measurements(); pair<size_t, size_t> siftIndex(size_t idx) const;
gtsam::gtsfm::NamedSfmMeasurement measurement(size_t idx) const; void addMeasurement(size_t idx, const gtsam::Point2& m);
gtsam::SfmMeasurement measurement(size_t idx) const;
bool hasUniqueCameras(); bool hasUniqueCameras();
}; };
class SfmTrack2dVector { virtual class SfmTrack : gtsam::SfmTrack2d {
SfmTrack2dVector();
SfmTrack2dVector(const gtsam::gtsfm::SfmTrack2dVector& other);
void push_back(const gtsam::gtsfm::SfmTrack2d& track);
size_t size() const;
bool empty() const;
void clear();
gtsam::gtsfm::SfmTrack2d at(const size_t& index) const;
};
class DsfTrackGenerator {
DsfTrackGenerator();
const gtsam::gtsfm::SfmTrack2dVector generate_tracks_from_pairwise_matches(
const gtsam::gtsfm::MatchIndicesMap& matches_dict,
const gtsam::gtsfm::KeypointsVector& keypoints_list,
bool verbose = false);
};
}///\namespace gtsfm
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/SfmTrack.h>
class SfmTrack {
SfmTrack(); SfmTrack();
SfmTrack(const gtsam::Point3& pt); SfmTrack(const gtsam::Point3& pt);
const Point3& point3() const; const Point3& point3() const;
@ -99,13 +30,6 @@ class SfmTrack {
double g; double g;
double b; 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 // enabling serialization functionality
void serialize() const; void serialize() const;
@ -113,6 +37,8 @@ class SfmTrack {
bool equals(const gtsam::SfmTrack& expected, double tol) const; bool equals(const gtsam::SfmTrack& expected, double tol) const;
}; };
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/sfm/SfmData.h> #include <gtsam/sfm/SfmData.h>
class SfmData { class SfmData {
SfmData(); SfmData();
@ -196,7 +122,7 @@ class BinaryMeasurementsRot3 {
#include <gtsam/sfm/ShonanAveraging.h> #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! // wrap!
class ShonanAveragingParameters2 { class ShonanAveragingParameters2 {
@ -391,4 +317,45 @@ class TranslationRecovery {
const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const; 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 gtsam::gtsfm::KeypointCoordinates& coordinates);
gtsam::gtsfm::KeypointCoordinates coordinates;
}; // check if this should be a method
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;
};
class DsfTrackGenerator {
DsfTrackGenerator();
const gtsam::SfmTrack2dVector generate_tracks_from_pairwise_matches(
const gtsam::gtsfm::MatchIndicesMap& matches_dict,
const gtsam::gtsfm::KeypointsVector& keypoints_list,
bool verbose = false);
};
} // namespace gtsfm
} // namespace gtsam } // namespace gtsam

9
python/gtsam/preamble/sfm.h
View File

@ -15,10 +15,9 @@
// #include <pybind11/stl.h> // #include <pybind11/stl.h>
#include <pybind11/stl_bind.h> #include <pybind11/stl_bind.h>
PYBIND11_MAKE_OPAQUE( PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SfmMeasurement>);
std::vector<gtsam::SfmTrack>); PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SfmTrack>);
PYBIND11_MAKE_OPAQUE( PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SfmCamera>);
std::vector<gtsam::SfmCamera>);
PYBIND11_MAKE_OPAQUE( PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::BinaryMeasurement<gtsam::Unit3>>); std::vector<gtsam::BinaryMeasurement<gtsam::Unit3>>);
PYBIND11_MAKE_OPAQUE( PYBIND11_MAKE_OPAQUE(
@ -26,5 +25,3 @@ PYBIND11_MAKE_OPAQUE(
PYBIND11_MAKE_OPAQUE( PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::gtsfm::Keypoints>); std::vector<gtsam::gtsfm::Keypoints>);
PYBIND11_MAKE_OPAQUE(gtsam::gtsfm::MatchIndicesMap); PYBIND11_MAKE_OPAQUE(gtsam::gtsfm::MatchIndicesMap);
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::gtsfm::NamedSfmMeasurement>);

26
python/gtsam/specializations/sfm.h
View File

@ -18,25 +18,11 @@ py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Rot3> > >( py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Rot3> > >(
m_, "BinaryMeasurementsRot3"); m_, "BinaryMeasurementsRot3");
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap"); py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
py::bind_vector< py::bind_vector<std::vector<gtsam::SfmTrack2d>>(m_, "SfmTrack2dVector");
std::vector<gtsam::SfmTrack> >( py::bind_vector<std::vector<gtsam::SfmTrack>>(m_, "SfmTracks");
m_, "SfmTracks"); py::bind_vector<std::vector<gtsam::SfmCamera>>(m_, "SfmCameras");
py::bind_vector< py::bind_vector<std::vector<std::pair<size_t, gtsam::Point2>>>(
std::vector<gtsam::SfmCamera> >( m_, "SfmMeasurementVector");
m_, "SfmCameras");
py::bind_vector<
std::vector<std::pair<size_t, gtsam::Point2>>>(
m_, "SfmMeasurementVector"
);
py::bind_vector<
std::vector<gtsam::gtsfm::SfmTrack2d> >(
m_, "SfmTrack2dVector");
py::bind_vector<
std::vector<gtsam::gtsfm::NamedSfmMeasurement> >(
m_, "NamedSfmMeasurementVector");
py::bind_map<gtsam::gtsfm::MatchIndicesMap>(m_, "MatchIndicesMap"); py::bind_map<gtsam::gtsfm::MatchIndicesMap>(m_, "MatchIndicesMap");
py::bind_vector<std::vector<gtsam::gtsfm::Keypoints>>(m_, "KeypointsVector");
py::bind_vector<
std::vector<gtsam::gtsfm::Keypoints> >(
m_, "KeypointsVector");

75
python/gtsam/tests/test_DsfTrackGenerator.py
View File

@ -6,15 +6,9 @@ Authors: John Lambert
import unittest import unittest
import numpy as np import numpy as np
from gtsam import (IndexPair, KeypointsVector, MatchIndicesMap, Point2,
import gtsam SfmMeasurementVector, SfmTrack2d)
from gtsam import IndexPair, KeypointsVector, MatchIndicesMap, NamedSfmMeasurementVector from gtsam.gtsfm import DsfTrackGenerator, Keypoints
from gtsam.gtsfm import (
DsfTrackGenerator,
Keypoints,
NamedSfmMeasurement,
SfmTrack2d,
)
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
@ -22,48 +16,55 @@ class TestDsfTrackGenerator(GtsamTestCase):
"""Tests for DsfTrackGenerator.""" """Tests for DsfTrackGenerator."""
def test_track_generation(self) -> None: def test_track_generation(self) -> None:
"""Ensures that DSF generates three tracks from measurements in 3 images (H=200,W=400).""" """Ensures that DSF generates three tracks from measurements
kps_i0 = Keypoints(coordinates=np.array([[10.0, 20], [30, 40]])) in 3 images (H=200,W=400)."""
kps_i1 = Keypoints(coordinates=np.array([[50.0, 60], [70, 80], [90, 100]])) kps_i0 = Keypoints(np.array([[10.0, 20], [30, 40]]))
kps_i2 = Keypoints(coordinates=np.array([[110.0, 120], [130, 140]])) 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 = KeypointsVector()
keypoints_list.append(kps_i0) keypoints_list.append(kps_i0)
keypoints_list.append(kps_i1) keypoints_list.append(kps_i1)
keypoints_list.append(kps_i2) keypoints_list.append(kps_i2)
# For each image pair (i1,i2), we provide a (K,2) matrix of corresponding image indices (k1,k2). # For each image pair (i1,i2), we provide a (K,2) matrix
# of corresponding image indices (k1,k2).
matches_dict = MatchIndicesMap() matches_dict = MatchIndicesMap()
matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]])
matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]])
tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(
matches_dict,
keypoints_list, keypoints_list,
verbose=True) verbose=False,
)
assert len(tracks) == 3 assert len(tracks) == 3
# Verify track 0. # Verify track 0.
assert np.allclose(tracks[0].measurements()[0].uv, np.array([10.0, 20.0])) track0 = tracks[0]
assert np.allclose(tracks[0].measurements()[1].uv, np.array([50.0, 60.0])) np.testing.assert_allclose(track0.measurements[0][1], Point2(10, 20))
assert tracks[0].measurements()[0].i == 0 np.testing.assert_allclose(track0.measurements[1][1], Point2(50, 60))
assert tracks[0].measurements()[1].i == 1 assert track0.measurements[0][0] == 0
assert tracks[0].numberMeasurements() == 2 assert track0.measurements[1][0] == 1
assert track0.numberMeasurements() == 2
# Verify track 1. # Verify track 1.
assert np.allclose(tracks[1].measurements()[0].uv, np.array([30.0, 40.0])) track1 = tracks[1]
assert np.allclose(tracks[1].measurements()[1].uv, np.array([70.0, 80.0])) np.testing.assert_allclose(track1.measurements[0][1], Point2(30, 40))
assert np.allclose(tracks[1].measurements()[2].uv, np.array([130.0, 140.0])) np.testing.assert_allclose(track1.measurements[1][1], Point2(70, 80))
assert tracks[1].measurements()[0].i == 0 np.testing.assert_allclose(track1.measurements[2][1], Point2(130, 140))
assert tracks[1].measurements()[1].i == 1 assert track1.measurements[0][0] == 0
assert tracks[1].measurements()[2].i == 2 assert track1.measurements[1][0] == 1
assert tracks[1].numberMeasurements() == 3 assert track1.measurements[2][0] == 2
assert track1.numberMeasurements() == 3
# Verify track 2. # Verify track 2.
assert np.allclose(tracks[2].measurements()[0].uv, np.array([90.0, 100.0])) track2 = tracks[2]
assert np.allclose(tracks[2].measurements()[1].uv, np.array([110.0, 120.0])) np.testing.assert_allclose(track2.measurements[0][1], Point2(90, 100))
assert tracks[2].measurements()[0].i == 1 np.testing.assert_allclose(track2.measurements[1][1], Point2(110, 120))
assert tracks[2].measurements()[1].i == 2 assert track2.measurements[0][0] == 1
assert tracks[2].numberMeasurements() == 2 assert track2.measurements[1][0] == 2
assert track2.numberMeasurements() == 2
class TestSfmTrack2d(GtsamTestCase): class TestSfmTrack2d(GtsamTestCase):
@ -71,8 +72,12 @@ class TestSfmTrack2d(GtsamTestCase):
def test_sfm_track_2d_constructor(self) -> None: def test_sfm_track_2d_constructor(self) -> None:
""" """ """ """
measurements = NamedSfmMeasurementVector() measurements = SfmMeasurementVector()
measurements.append(NamedSfmMeasurement(i=0, uv=np.array([10.0, 20.0]))) measurements.append((0, Point2(10, 20)))
track = SfmTrack2d(measurements=measurements) track = SfmTrack2d(measurements=measurements)
track.measurement(0) track.measurement(0)
track.numberMeasurements() == 1 track.numberMeasurements() == 1
if __name__ == "__main__":
unittest.main()