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

hasUniqueCameras to camel case

release/4.3a0
senselessdev1 2022-07-24 02:54:07 -04:00
parent 51fb3750e8
commit e6ccf97712
2 changed files with 99 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

186
gtsam/sfm/DsfTrackGenerator.h
View File

@ -17,7 +17,6 @@
*/
#pragma once
#include <pybind11/stl.h>
#include <Eigen/Core>
#include <gtsam/base/DSFMap.h>
@ -39,8 +38,7 @@ typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array
using KeypointCoordinates = Eigen::MatrixX2d;
struct Keypoints
{
struct Keypoints {
KeypointCoordinates coordinates;
// typedef'd for Eigen::VectorXd
boost::optional<gtsam::Vector> scales;
@ -57,8 +55,7 @@ using MatchIndicesMap = std::map<ImagePair, CorrespondenceIndices>;
// @param camera index
// @param 2d measurement
// Implemented as named tuple, instead of std::pair (like SfmMeasurement in SfmTrack.h)
struct NamedSfmMeasurement
{
struct NamedSfmMeasurement {
size_t i;
gtsam::Point2 uv;
@ -71,24 +68,23 @@ struct NamedSfmMeasurement
* 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_;
class SfmTrack2d {
private:
std::vector<NamedSfmMeasurement> measurements_;
public:
void addMeasurement(const NamedSfmMeasurement &m) {
measurements_.emplace_back(m);
}
std::vector<NamedSfmMeasurement> measurements() {return measurements_; }
public:
void addMeasurement(const NamedSfmMeasurement &m) {
measurements_.emplace_back(m);
}
std::vector<NamedSfmMeasurement> measurements() {return measurements_; }
// @brief Validates the track by checking that no two measurements are from the same camera.
//
// returns boolean result of the validation.
bool has_unique_cameras()
bool hasUniqueCameras()
{
std::vector<int> track_cam_idxs;
for (auto & measurement: measurements_)
for (auto & measurement : measurements_)
{
track_cam_idxs.emplace_back(measurement.i);
}
@ -101,93 +97,91 @@ class SfmTrack2d
/**
* @brief Generates point tracks from connected components in the keypoint matches graph.
*/
class DsfTrackGenerator
{
class DsfTrackGenerator {
public:
/** Default constructor. */
DsfTrackGenerator() {}
public:
/** Default constructor. */
DsfTrackGenerator() {}
// 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> generate_tracks_from_pairwise_matches(
const MatchIndicesMap& matches_dict,
const KeypointsList& keypoints_list)
{
std::vector<SfmTrack2d> track_2d_list;
// 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> generate_tracks_from_pairwise_matches(
const MatchIndicesMap& matches_dict,
const KeypointsList& keypoints_list) {
std::vector<SfmTrack2d> track_2d_list;
std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl;
// Generate the DSF to form tracks.
DSFMapIndexPair dsf;
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;
for (const auto& kv : matches_dict) {
const auto pair_idxs = kv.first;
const auto corr_idxs = 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++)
{
// 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.
dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2));
}
// 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++)
{
// 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.
dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2));
}
std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl;
const std::map<IndexPair, std::set<IndexPair> > key_sets = dsf.sets();
// Return immediately if no sets were found.
if (key_sets.empty()) return track_2d_list;
size_t erroneous_track_count = 0;
// Create a list of tracks.
// Each track will be represented as a list of (camera_idx, measurements).
for (const auto& kv: key_sets) {
const auto set_id = kv.first;
const auto index_pair_set = kv.second;
// 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.
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)));
}
// 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.has_unique_cameras())
{
track_2d_list.emplace_back(track_2d);
} else {
erroneous_track_count++;
}
}
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;
// TODO(johnwlambert): return the Transitivity failure percentage here.
return track_2d_list;
}
std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl;
const std::map<IndexPair, std::set<IndexPair> > key_sets = dsf.sets();
// Return immediately if no sets were found.
if (key_sets.empty()) return track_2d_list;
size_t erroneous_track_count = 0;
// Create a list of tracks.
// Each track will be represented as a list of (camera_idx, measurements).
for (const auto& kv : key_sets) {
const auto set_id = kv.first;
const auto index_pair_set = kv.second;
// 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.
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)));
}
// 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++;
}
}
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;
// TODO(johnwlambert): return the Transitivity failure percentage here.
return track_2d_list;
}
};
} // namespace gtsam

16
gtsam/sfm/sfm.i
View File

@ -17,6 +17,13 @@ class MatchIndicesMap {
};
class Keypoints
{
Keypoints(const gtsam::KeypointCoordinates& coordinates);
gtsam::KeypointCoordinates coordinates;
}; // check if this should be a method
class KeypointsList {
KeypointsList();
KeypointsList(const gtsam::KeypointsList& other);
@ -28,13 +35,6 @@ class KeypointsList {
};
class Keypoints
{
Keypoints(const gtsam::KeypointCoordinates& coordinates);
gtsam::KeypointCoordinates coordinates;
}; // check if this should be a method
class NamedSfmMeasurement
{
size_t i;
@ -46,7 +46,7 @@ class SfmTrack2d
{
void addMeasurement(const gtsam::NamedSfmMeasurement &m);
std::vector<gtsam::NamedSfmMeasurement> measurements();
bool has_unique_cameras();
bool hasUniqueCameras();
};