Merge pull request #535 from borglab/feature/1dsfm_example
Wrapping MFAS and 1dsfm python examplerelease/4.3a0
Akshay Krishnan
GitHub
commit
627c015727
|
|
@ -2885,6 +2885,7 @@ class BinaryMeasurement {
|
|||
size_t key1() const;
|
||||
size_t key2() const;
|
||||
T measured() const;
|
||||
gtsam::noiseModel::Base* noiseModel() const;
|
||||
};
|
||||
|
||||
typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
|
||||
|
|
@ -3018,6 +3019,26 @@ class ShonanAveraging3 {
|
|||
pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p, size_t max_p) const;
|
||||
};
|
||||
|
||||
#include <gtsam/sfm/MFAS.h>
|
||||
|
||||
class KeyPairDoubleMap {
|
||||
KeyPairDoubleMap();
|
||||
KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other);
|
||||
|
||||
size_t size() const;
|
||||
bool empty() const;
|
||||
void clear();
|
||||
size_t at(const pair<size_t, size_t>& keypair) const;
|
||||
};
|
||||
|
||||
class MFAS {
|
||||
MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
|
||||
const gtsam::Unit3& projectionDirection);
|
||||
|
||||
gtsam::KeyPairDoubleMap computeOutlierWeights() const;
|
||||
gtsam::KeyVector computeOrdering() const;
|
||||
};
|
||||
|
||||
#include <gtsam/sfm/TranslationRecovery.h>
|
||||
class TranslationRecovery {
|
||||
TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations,
|
||||
|
|
|
|||
|
|
@ -111,10 +111,8 @@ void removeNodeFromGraph(const Key node,
|
|||
graph.erase(node);
|
||||
}
|
||||
|
||||
MFAS::MFAS(const std::shared_ptr<vector<Key>>& nodes,
|
||||
const TranslationEdges& relativeTranslations,
|
||||
const Unit3& projectionDirection)
|
||||
: nodes_(nodes) {
|
||||
MFAS::MFAS(const TranslationEdges& relativeTranslations,
|
||||
const Unit3& projectionDirection) {
|
||||
// Iterate over edges, obtain weights by projecting
|
||||
// their relativeTranslations along the projection direction
|
||||
for (const auto& measurement : relativeTranslations) {
|
||||
|
|
|
|||
|
|
@ -56,40 +56,28 @@ class MFAS {
|
|||
using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
|
||||
|
||||
private:
|
||||
// pointer to nodes in the graph
|
||||
const std::shared_ptr<std::vector<Key>> nodes_;
|
||||
|
||||
// edges with a direction such that all weights are positive
|
||||
// i.e, edges that originally had negative weights are flipped
|
||||
std::map<KeyPair, double> edgeWeights_;
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Construct from the nodes in a graph and weighted directed edges
|
||||
* @brief Construct from the weighted directed edges
|
||||
* between the nodes. Each node is identified by a Key.
|
||||
* A shared pointer to the nodes is used as input parameter
|
||||
* because, MFAS ordering is usually used to compute the ordering of a
|
||||
* large graph that is already stored in memory. It is unnecessary to make a
|
||||
* copy of the nodes in this class.
|
||||
* @param nodes: Nodes in the graph
|
||||
* @param edgeWeights: weights of edges in the graph
|
||||
*/
|
||||
MFAS(const std::shared_ptr<std::vector<Key>> &nodes,
|
||||
const std::map<KeyPair, double> &edgeWeights)
|
||||
: nodes_(nodes), edgeWeights_(edgeWeights) {}
|
||||
MFAS(const std::map<KeyPair, double> &edgeWeights)
|
||||
: edgeWeights_(edgeWeights) {}
|
||||
|
||||
/**
|
||||
* @brief Constructor to be used in the context of translation averaging.
|
||||
* Here, the nodes of the graph are cameras in 3D and the edges have a unit
|
||||
* translation direction between them. The weights of the edges is computed by
|
||||
* projecting them along a projection direction.
|
||||
* @param nodes cameras in the epipolar graph (each camera is identified by a
|
||||
* Key)
|
||||
* @param relativeTranslations translation directions between the cameras
|
||||
* @param projectionDirection direction in which edges are to be projected
|
||||
*/
|
||||
MFAS(const std::shared_ptr<std::vector<Key>> &nodes,
|
||||
const TranslationEdges &relativeTranslations,
|
||||
MFAS(const TranslationEdges &relativeTranslations,
|
||||
const Unit3 &projectionDirection);
|
||||
|
||||
/**
|
||||
|
|
@ -99,7 +87,7 @@ class MFAS {
|
|||
std::vector<Key> computeOrdering() const;
|
||||
|
||||
/**
|
||||
* @brief Computes the "outlier weights" of the graph. We define the outlier
|
||||
* @brief Computes the outlier weights of the graph. We define the outlier
|
||||
* weight of a edge to be zero if the edge is an inlier and the magnitude of
|
||||
* its edgeWeight if it is an outlier. This function internally calls
|
||||
* computeOrdering and uses the obtained ordering to identify outlier edges.
|
||||
|
|
@ -108,4 +96,6 @@ class MFAS {
|
|||
std::map<KeyPair, double> computeOutlierWeights() const;
|
||||
};
|
||||
|
||||
typedef std::map<std::pair<Key, Key>, double> KeyPairDoubleMap;
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/sfm/MFAS.h>
|
||||
#include <iostream>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -39,14 +39,13 @@ map<MFAS::KeyPair, double> getEdgeWeights(const vector<MFAS::KeyPair> &edges,
|
|||
for (size_t i = 0; i < edges.size(); i++) {
|
||||
edgeWeights[edges[i]] = weights[i];
|
||||
}
|
||||
cout << "returning edge weights " << edgeWeights.size() << endl;
|
||||
return edgeWeights;
|
||||
}
|
||||
|
||||
// test the ordering and the outlierWeights function using weights2 - outlier
|
||||
// edge is rejected when projected in a direction that gives weights2
|
||||
TEST(MFAS, OrderingWeights2) {
|
||||
MFAS mfas_obj(make_shared<vector<Key>>(nodes), getEdgeWeights(edges, weights2));
|
||||
MFAS mfas_obj(getEdgeWeights(edges, weights2));
|
||||
|
||||
vector<Key> ordered_nodes = mfas_obj.computeOrdering();
|
||||
|
||||
|
|
@ -76,7 +75,7 @@ TEST(MFAS, OrderingWeights2) {
|
|||
// weights1 (outlier edge is accepted when projected in a direction that
|
||||
// produces weights1)
|
||||
TEST(MFAS, OrderingWeights1) {
|
||||
MFAS mfas_obj(make_shared<vector<Key>>(nodes), getEdgeWeights(edges, weights1));
|
||||
MFAS mfas_obj(getEdgeWeights(edges, weights1));
|
||||
|
||||
vector<Key> ordered_nodes = mfas_obj.computeOrdering();
|
||||
|
||||
|
|
|
|||
|
|
@ -37,7 +37,8 @@ set(ignore
|
|||
gtsam::Point2Vector
|
||||
gtsam::Pose3Vector
|
||||
gtsam::KeyVector
|
||||
gtsam::BinaryMeasurementsUnit3)
|
||||
gtsam::BinaryMeasurementsUnit3
|
||||
gtsam::KeyPairDoubleMap)
|
||||
|
||||
pybind_wrap(gtsam_py # target
|
||||
${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header
|
||||
|
|
@ -80,7 +81,9 @@ set(ignore
|
|||
gtsam::Pose3Vector
|
||||
gtsam::KeyVector
|
||||
gtsam::FixedLagSmootherKeyTimestampMapValue
|
||||
gtsam::BinaryMeasurementsUnit3)
|
||||
gtsam::BinaryMeasurementsUnit3
|
||||
gtsam::KeyPairDoubleMap)
|
||||
|
||||
pybind_wrap(gtsam_unstable_py # target
|
||||
${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header
|
||||
"gtsam_unstable.cpp" # generated_cpp
|
||||
|
|
|
|||
|
|
@ -0,0 +1,144 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2018, 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
|
||||
|
||||
This example shows how 1dsfm uses outlier rejection (MFAS) and optimization (translation recovery)
|
||||
together for estimating global translations from relative translation directions and global rotations.
|
||||
The purpose of this example is to illustrate the connection between these two classes using a small SfM dataset.
|
||||
|
||||
Author: Akshay Krishnan
|
||||
Date: September 2020
|
||||
"""
|
||||
|
||||
from collections import defaultdict
|
||||
from typing import List, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam.examples import SFMdata
|
||||
|
||||
# Hyperparameters for 1dsfm, values used from Kyle Wilson's code.
|
||||
MAX_1DSFM_PROJECTION_DIRECTIONS = 48
|
||||
OUTLIER_WEIGHT_THRESHOLD = 0.1
|
||||
|
||||
|
||||
def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]:
|
||||
""""Returns global rotations and unit translation directions between 8 cameras
|
||||
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))
|
||||
# Rotations of the cameras in the world frame.
|
||||
wRc_values = gtsam.Values()
|
||||
# Normalized translation directions from camera i to camera j
|
||||
# in the coordinate frame of camera i.
|
||||
i_iZj_list = []
|
||||
for i in range(0, len(wTc_list) - 2):
|
||||
# Add the rotation.
|
||||
wRi = wTc_list[i].rotation()
|
||||
wRc_values.insert(i, wRi)
|
||||
# Create unit translation measurements with next two poses.
|
||||
for j in range(i + 1, i + 3):
|
||||
# Compute the translation from pose i to pose j, in the world reference frame.
|
||||
w_itj = wTc_list[j].translation() - wTc_list[i].translation()
|
||||
# Obtain the translation in the camera i's reference frame.
|
||||
i_itj = wRi.unrotate(w_itj)
|
||||
# Compute the normalized unit translation direction.
|
||||
i_iZj = gtsam.Unit3(i_itj)
|
||||
i_iZj_list.append(gtsam.BinaryMeasurementUnit3(
|
||||
i, j, i_iZj, gtsam.noiseModel.Isotropic.Sigma(3, 0.01)))
|
||||
# Add the last two rotations.
|
||||
wRc_values.insert(len(wTc_list) - 1, wTc_list[-1].rotation())
|
||||
wRc_values.insert(len(wTc_list) - 2, wTc_list[-2].rotation())
|
||||
return wRc_values, i_iZj_list
|
||||
|
||||
|
||||
def filter_outliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryMeasurementsUnit3:
|
||||
"""Removes outliers from a list of Unit3 measurements that are the
|
||||
translation directions from camera i to camera j in the world frame."""
|
||||
|
||||
# Indices of measurements that are to be used as projection directions.
|
||||
# These are randomly chosen. All sampled directions must be unique.
|
||||
num_directions_to_sample = min(
|
||||
MAX_1DSFM_PROJECTION_DIRECTIONS, len(w_iZj_list))
|
||||
sampled_indices = np.random.choice(
|
||||
len(w_iZj_list), num_directions_to_sample, replace=False)
|
||||
|
||||
# Sample projection directions from the measurements.
|
||||
projection_directions = [w_iZj_list[idx].measured()
|
||||
for idx in sampled_indices]
|
||||
|
||||
outlier_weights = []
|
||||
# Find the outlier weights for each direction using MFAS.
|
||||
for direction in projection_directions:
|
||||
algorithm = gtsam.MFAS(w_iZj_list, direction)
|
||||
outlier_weights.append(algorithm.computeOutlierWeights())
|
||||
|
||||
# Compute average of outlier weights. Each outlier weight is a map from a pair of Keys
|
||||
# (camera IDs) to a weight, where weights are proportional to the probability of the edge
|
||||
# being an outlier.
|
||||
avg_outlier_weights = defaultdict(float)
|
||||
for outlier_weight_dict in outlier_weights:
|
||||
for keypair, weight in outlier_weight_dict.items():
|
||||
avg_outlier_weights[keypair] += weight / len(outlier_weights)
|
||||
|
||||
# Remove w_iZj that have weight greater than threshold, these are outliers.
|
||||
w_iZj_inliers = gtsam.BinaryMeasurementsUnit3()
|
||||
[w_iZj_inliers.append(w_iZj) for w_iZj in w_iZj_list if avg_outlier_weights[(
|
||||
w_iZj.key1(), w_iZj.key2())] < OUTLIER_WEIGHT_THRESHOLD]
|
||||
|
||||
return w_iZj_inliers
|
||||
|
||||
|
||||
def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3,
|
||||
wRc_values: gtsam.Values) -> gtsam.Values:
|
||||
"""Estimate poses given rotations and normalized translation directions between cameras.
|
||||
|
||||
Args:
|
||||
i_iZj_list: List of normalized translation direction measurements between camera pairs,
|
||||
Z here refers to measurements. The measurements are of camera j with reference
|
||||
to camera i (iZj), in camera i's coordinate frame (i_). iZj represents a unit
|
||||
vector to j in i's frame and is not a transformation.
|
||||
wRc_values: Rotations of the cameras in the world frame.
|
||||
|
||||
Returns:
|
||||
gtsam.Values: Estimated poses.
|
||||
"""
|
||||
|
||||
# Convert the translation direction measurements to world frame using the rotations.
|
||||
w_iZj_list = gtsam.BinaryMeasurementsUnit3()
|
||||
for i_iZj in i_iZj_list:
|
||||
w_iZj = gtsam.Unit3(wRc_values.atRot3(i_iZj.key1())
|
||||
.rotate(i_iZj.measured().point3()))
|
||||
w_iZj_list.append(gtsam.BinaryMeasurementUnit3(
|
||||
i_iZj.key1(), i_iZj.key2(), w_iZj, i_iZj.noiseModel()))
|
||||
|
||||
# Remove the outliers in the unit translation directions.
|
||||
w_iZj_inliers = filter_outliers(w_iZj_list)
|
||||
|
||||
# Run the optimizer to obtain translations for normalized directions.
|
||||
wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run()
|
||||
|
||||
wTc_values = gtsam.Values()
|
||||
for key in wRc_values.keys():
|
||||
wTc_values.insert(key, gtsam.Pose3(
|
||||
wRc_values.atRot3(key), wtc_values.atPoint3(key)))
|
||||
return wTc_values
|
||||
|
||||
|
||||
def main():
|
||||
wRc_values, i_iZj_list = get_data()
|
||||
wTc_values = estimate_poses(i_iZj_list, wRc_values)
|
||||
print("**** Translation averaging output ****")
|
||||
print(wTc_values)
|
||||
print("**************************************")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -12,3 +12,4 @@ py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>
|
|||
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(m_, "BinaryMeasurementsUnit3");
|
||||
py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
|
||||
py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
|
||||
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
|
||||
|
|
|
|||
Loading…
Reference in New Issue