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

add additional methods to TA + wrapper

release/4.3a0
Akshay Krishnan 2022-04-23 19:08:16 +05:30
parent 468f903e24
commit 3ea19f4bd0
4 changed files with 109 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/sfm/TranslationRecovery.cpp
View File

@ -21,6 +21,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -28,6 +29,7 @@
#include <gtsam/sfm/TranslationFactor.h>
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/expressions.h>
#include <set>
#include <utility>
@ -81,7 +83,7 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
}
void TranslationRecovery::addPrior(
const double scale, NonlinearFactorGraph *graph,
const double scale, const boost::shared_ptr<NonlinearFactorGraph> graph,
const SharedNoiseModel &priorNoiseModel) const {
auto edge = relativeTranslations_.begin();
if (edge == relativeTranslations_.end()) return;
@ -91,6 +93,32 @@ void TranslationRecovery::addPrior(
edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
}
void TranslationRecovery::addPrior(
Key i, const Point3 &prior,
const boost::shared_ptr<NonlinearFactorGraph> graph,
const SharedNoiseModel &priorNoiseModel) const {
graph->addPrior(getUniqueKey(i), prior, priorNoiseModel);
}
Key TranslationRecovery::getUniqueKey(const Key i) const {
for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
Key optimizedKey = optimizedAndDuplicateKeys.first;
std::set<Key> duplicateKeys = optimizedAndDuplicateKeys.second;
if (i == optimizedKey || duplicateKeys.count(i)) return optimizedKey;
}
// Unlikely case, when i is not in the graph.
return i;
}
void TranslationRecovery::addRelativeHardConstraint(
Key i, Key j, const Point3 &w_itj,
const boost::shared_ptr<NonlinearFactorGraph> graph) const {
Point3_ wti_(getUniqueKey(i)), wtj_(getUniqueKey(j));
Expression<Point3> w_itj_ = wtj_ - wti_;
graph->addExpressionFactor(noiseModel::Constrained::All(3, 1e9), w_itj,
w_itj_);
}
Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const {
uniform_real_distribution<double> randomVal(-1, 1);
// Create a lambda expression that checks whether value exists and randomly
@ -125,12 +153,17 @@ Values TranslationRecovery::initializeRandomly() const {
}
Values TranslationRecovery::run(const double scale) const {
auto graph = buildGraph();
addPrior(scale, &graph);
boost::shared_ptr<NonlinearFactorGraph> graph_ptr =
boost::make_shared<NonlinearFactorGraph>(buildGraph());
addPrior(scale, graph_ptr);
const Values initial = initializeRandomly();
LevenbergMarquardtOptimizer lm(graph, initial, params_);
LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_);
Values result = lm.optimize();
return addDuplicateNodes(result);
}
Values TranslationRecovery::addDuplicateNodes(const Values &result) const {
Values final_result = result;
// Nodes that were not optimized are stored in sameTranslationNodes_ as a map
// from a key that was optimized to keys that were not optimized. Iterate over
// map and add results for keys not optimized.
@ -139,11 +172,12 @@ Values TranslationRecovery::run(const double scale) const {
std::set<Key> duplicateKeys = optimizedAndDuplicateKeys.second;
// Add the result for the duplicate key if it does not already exist.
for (const Key duplicateKey : duplicateKeys) {
if (result.exists(duplicateKey)) continue;
result.insert<Point3>(duplicateKey, result.at<Point3>(optimizedKey));
if (final_result.exists(duplicateKey)) continue;
final_result.insert<Point3>(duplicateKey,
final_result.at<Point3>(optimizedKey));
}
}
return result;
return final_result;
}
TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(

17
gtsam/sfm/TranslationRecovery.h
View File

@ -93,10 +93,20 @@ class TranslationRecovery {
* @param graph factor graph to which prior is added.
* @param priorNoiseModel the noise model to use with the prior.
*/
void addPrior(const double scale, NonlinearFactorGraph *graph,
void addPrior(const double scale,
const boost::shared_ptr<NonlinearFactorGraph> graph,
const SharedNoiseModel &priorNoiseModel =
noiseModel::Isotropic::Sigma(3, 0.01)) const;
void addPrior(Key i, const Point3 &prior,
const boost::shared_ptr<NonlinearFactorGraph> graph,
const SharedNoiseModel &priorNoiseModel =
noiseModel::Isotropic::Sigma(3, 0.01)) const;
void addRelativeHardConstraint(
Key i, Key j, const Point3 &w_itj,
const boost::shared_ptr<NonlinearFactorGraph> graph) const;
/**
* @brief Create random initial translations.
*
@ -120,6 +130,8 @@ class TranslationRecovery {
*/
Values run(const double scale = 1.0) const;
Values addDuplicateNodes(const Values &result) const;
/**
* @brief Simulate translation direction measurements
*
@ -131,5 +143,8 @@ class TranslationRecovery {
*/
static TranslationEdges SimulateMeasurements(
const Values &poses, const std::vector<KeyPair> &edges);
private:
Key getUniqueKey(const Key i) const;
};
} // namespace gtsam

16
gtsam/sfm/sfm.i
View File

@ -4,6 +4,8 @@
namespace gtsam {
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/SfmTrack.h>
class SfmTrack {
SfmTrack();
@ -142,8 +144,8 @@ class ShonanAveraging2 {
ShonanAveraging2(string g2oFile);
ShonanAveraging2(string g2oFile,
const gtsam::ShonanAveragingParameters2& parameters);
ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors,
const gtsam::ShonanAveragingParameters2 &parameters);
ShonanAveraging2(const gtsam::BetweenFactorPose2s& factors,
const gtsam::ShonanAveragingParameters2& parameters);
// Query properties
size_t nrUnknowns() const;
@ -259,6 +261,16 @@ class TranslationRecovery {
TranslationRecovery(
const gtsam::BinaryMeasurementsUnit3&
relativeTranslations); // default LevenbergMarquardtParams
gtsam::NonlinearFactorGraph buildGraph() const;
gtsam::Values initializeRandomly() const;
void addPrior(gtsam::Key i, const gtsam::Point3& prior,
gtsam::NonlinearFactorGraph* graph,
const gtsam::SharedNoiseModel& model =
gtsam::noiseModel::Isotropic::Sigma(3, 0.01)) const;
void addRelativeHardConstraint(gtsam::Key i, gtsam::Key j,
const gtsam::Point3& w_itj,
gtsam::NonlinearFactorGraph* graph) const;
gtsam::Values addDuplicateNodes(const gtsam::Values& result) const;
gtsam::Values run(const double scale) const;
gtsam::Values run() const; // default scale = 1.0
};

39
tests/testTranslationRecovery.cpp
View File

@ -17,8 +17,8 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/sfm/SfmData.h>
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/slam/dataset.h>
using namespace std;
@ -265,6 +265,43 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(2), 1e-8));
}
TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) {
// Create a dataset with 3 poses.
// __ __
// \/ \/
// 0 _____ 1
// \ __ /
// \\//
// 3
//
// 0 and 1 face in the same direction but have a translation offset. 3 is in
// the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
Values poses;
poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
auto relativeTranslations =
TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {3, 0}});
TranslationRecovery algorithm(relativeTranslations);
boost::shared_ptr<NonlinearFactorGraph> graph_ptr =
boost::make_shared<NonlinearFactorGraph>(algorithm.buildGraph());
algorithm.addPrior(0, Point3(), graph_ptr);
algorithm.addRelativeHardConstraint(0, 1, Point3(2, 0, 0), graph_ptr);
const Values initial = algorithm.initializeRandomly();
LevenbergMarquardtParams params;
LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params);
auto result = algorithm.addDuplicateNodes(lm.optimize());
EXPECT_LONGS_EQUAL(4, graph_ptr->size());
// Check result
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
}
/* ************************************************************************* */
int main() {
TestResult tr;