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

139
gtsam/sfm/DsfTrackGenerator.h
View File

@ -17,11 +17,11 @@
*/
#pragma once
#include <Eigen/Core>
#include <gtsam/base/DSFMap.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/sfm/SfmTrack.h>
#include <Eigen/Core>
#include <algorithm>
#include <iostream>
#include <map>
@ -33,19 +33,18 @@ namespace gtsam {
namespace gtsfm {
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;
// Output of detections in an image.
// 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).
// 3. Origin is at the top left corner of the image.
struct Keypoints {
// The (x, y) coordinates of the features, of shape Nx2.
KeypointCoordinates coordinates;
@ -56,80 +55,20 @@ struct Keypoints {
/// Optional confidences/responses for each detection, of shape N.
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>;
// 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>;
// @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.
* Note: Equivalent to gtsam.SfmTrack, but without the 3d measurement.
* 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.
* @brief Generates point tracks from connected components in the keypoint
* matches graph.
*/
class DsfTrackGenerator {
public:
/** Default constructor. */
DsfTrackGenerator() {}
@ -144,28 +83,28 @@ class DsfTrackGenerator {
// correspondence indices, from each image.
// @param Length-N list of keypoints, for N images/cameras.
std::vector<SfmTrack2d> generate_tracks_from_pairwise_matches(
const MatchIndicesMap& matches_dict,
const KeypointsVector& keypoints_list,
bool verbose = false) {
const MatchIndicesMap& matches_dict,
const KeypointsVector& keypoints_list, bool verbose = false) {
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.
DSFMapIndexPair dsf;
for (const auto& kv : matches_dict) {
const auto pair_idxs = kv.first;
const auto corr_idxs = kv.second;
const auto pair_indices = kv.first;
const auto corr_indices = kv.second;
// Image pair is (i1,i2).
size_t i1 = pair_idxs.first;
size_t i2 = pair_idxs.second;
for (size_t k = 0; k < corr_idxs.rows(); k++)
{
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_idxs(k, 0);
size_t k2 = corr_idxs(k, 1);
// Unique keys for the DSF are (i,k), representing keypoint index in an image.
size_t k1 = corr_indices(k, 0);
size_t k2 = corr_indices(k, 1);
// Unique keys for the DSF are (i,k), representing keypoint index in an
// image.
dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2));
}
}
@ -186,19 +125,19 @@ class DsfTrackGenerator {
// Initialize track from measurements.
SfmTrack2d track_2d;
for (const auto& index_pair : index_pair_set)
{
// Camera index is represented by i, and measurement index is represented by k.
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.
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.
// This is an expected result from an incorrect correspondence slipping through.
if (track_2d.hasUniqueCameras())
{
// Skip erroneous track that had repeated measurements within the same
// image. This is an expected result from an incorrect correspondence
// slipping through.
if (track_2d.hasUniqueCameras()) {
track_2d_list.emplace_back(track_2d);
} else {
erroneous_track_count++;
@ -206,19 +145,19 @@ class DsfTrackGenerator {
}
if (verbose) {
double erroneous_track_pct = static_cast<float>(erroneous_track_count)
/ static_cast<float>(key_sets.size()) * 100;
double erroneous_track_pct = static_cast<float>(erroneous_track_count) /
static_cast<float>(key_sets.size()) * 100;
// TODO(johnwlambert): restrict decimal places to 2 decimals.
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.
return track_2d_list;
}
};
}///\namespace gtsfm
} // namespace gtsam
} // namespace gtsfm
} // 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
* @returns boolean result of the validation.
*/
bool hasUniqueCameras() {
bool hasUniqueCameras() const {
std::vector<int> track_cam_indices;
for (auto& measurement : measurements) {
track_cam_indices.emplace_back(measurement.first);

139
gtsam/sfm/sfm.i
View File

@ -4,91 +4,22 @@
namespace gtsam {
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 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;
};
#include <gtsam/sfm/SfmTrack.h>
class SfmTrack2d
{
std::vector<pair<size_t, gtsam::Point2>> measurements;
SfmTrack2d();
SfmTrack2d(std::vector<gtsam::gtsfm::NamedSfmMeasurement> &measurements);
SfmTrack2d(std::vector<gtsam::SfmMeasurement> &measurements);
size_t numberMeasurements() const;
void addMeasurement(const gtsam::gtsfm::NamedSfmMeasurement &m);
std::vector<gtsam::gtsfm::NamedSfmMeasurement> measurements();
gtsam::gtsfm::NamedSfmMeasurement measurement(size_t idx) 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();
};
class SfmTrack2dVector {
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 {
virtual class SfmTrack : gtsam::SfmTrack2d {
SfmTrack();
SfmTrack(const gtsam::Point3& pt);
const Point3& point3() const;
@ -99,13 +30,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;
@ -113,6 +37,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();
@ -196,7 +122,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 {
@ -391,4 +317,45 @@ 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 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

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

@ -15,10 +15,9 @@
// #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(
@ -26,5 +25,3 @@ PYBIND11_MAKE_OPAQUE(
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::gtsfm::Keypoints>);
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> > >(
m_, "BinaryMeasurementsRot3");
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
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::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::gtsfm::SfmTrack2d> >(
m_, "SfmTrack2dVector");
py::bind_vector<
std::vector<gtsam::gtsfm::NamedSfmMeasurement> >(
m_, "NamedSfmMeasurementVector");
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");

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

@ -6,15 +6,9 @@ Authors: John Lambert
import unittest
import numpy as np
import gtsam
from gtsam import IndexPair, KeypointsVector, MatchIndicesMap, NamedSfmMeasurementVector
from gtsam.gtsfm import (
DsfTrackGenerator,
Keypoints,
NamedSfmMeasurement,
SfmTrack2d,
)
from gtsam import (IndexPair, KeypointsVector, MatchIndicesMap, Point2,
SfmMeasurementVector, SfmTrack2d)
from gtsam.gtsfm import DsfTrackGenerator, Keypoints
from gtsam.utils.test_case import GtsamTestCase
@ -22,48 +16,55 @@ 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(coordinates=np.array([[10.0, 20], [30, 40]]))
kps_i1 = Keypoints(coordinates=np.array([[50.0, 60], [70, 80], [90, 100]]))
kps_i2 = Keypoints(coordinates=np.array([[110.0, 120], [130, 140]]))
"""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).
# 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 = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict,
keypoints_list,
verbose=True)
tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(
matches_dict,
keypoints_list,
verbose=False,
)
assert len(tracks) == 3
# Verify track 0.
assert np.allclose(tracks[0].measurements()[0].uv, np.array([10.0, 20.0]))
assert np.allclose(tracks[0].measurements()[1].uv, np.array([50.0, 60.0]))
assert tracks[0].measurements()[0].i == 0
assert tracks[0].measurements()[1].i == 1
assert tracks[0].numberMeasurements() == 2
track0 = tracks[0]
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
assert track0.numberMeasurements() == 2
# Verify track 1.
assert np.allclose(tracks[1].measurements()[0].uv, np.array([30.0, 40.0]))
assert np.allclose(tracks[1].measurements()[1].uv, np.array([70.0, 80.0]))
assert np.allclose(tracks[1].measurements()[2].uv, np.array([130.0, 140.0]))
assert tracks[1].measurements()[0].i == 0
assert tracks[1].measurements()[1].i == 1
assert tracks[1].measurements()[2].i == 2
assert tracks[1].numberMeasurements() == 3
track1 = tracks[1]
np.testing.assert_allclose(track1.measurements[0][1], Point2(30, 40))
np.testing.assert_allclose(track1.measurements[1][1], Point2(70, 80))
np.testing.assert_allclose(track1.measurements[2][1], Point2(130, 140))
assert track1.measurements[0][0] == 0
assert track1.measurements[1][0] == 1
assert track1.measurements[2][0] == 2
assert track1.numberMeasurements() == 3
# Verify track 2.
assert np.allclose(tracks[2].measurements()[0].uv, np.array([90.0, 100.0]))
assert np.allclose(tracks[2].measurements()[1].uv, np.array([110.0, 120.0]))
assert tracks[2].measurements()[0].i == 1
assert tracks[2].measurements()[1].i == 2
assert tracks[2].numberMeasurements() == 2
track2 = tracks[2]
np.testing.assert_allclose(track2.measurements[0][1], Point2(90, 100))
np.testing.assert_allclose(track2.measurements[1][1], Point2(110, 120))
assert track2.measurements[0][0] == 1
assert track2.measurements[1][0] == 2
assert track2.numberMeasurements() == 2
class TestSfmTrack2d(GtsamTestCase):
@ -71,8 +72,12 @@ class TestSfmTrack2d(GtsamTestCase):
def test_sfm_track_2d_constructor(self) -> None:
""" """
measurements = NamedSfmMeasurementVector()
measurements.append(NamedSfmMeasurement(i=0, uv=np.array([10.0, 20.0])))
measurements = SfmMeasurementVector()
measurements.append((0, Point2(10, 20)))
track = SfmTrack2d(measurements=measurements)
track.measurement(0)
track.numberMeasurements() == 1
if __name__ == "__main__":
unittest.main()