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

intialize values that may have between factors

release/4.3a0
Akshay Krishnan 2022-05-14 00:18:04 -07:00
parent 06bc78175c
commit fa743ae0ac
2 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/sfm/TranslationRecovery.cpp
View File

@ -134,6 +134,7 @@ void TranslationRecovery::addPrior(
Values TranslationRecovery::initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
std::mt19937 *rng, const Values &initialValues) const {
uniform_real_distribution<double> randomVal(-1, 1);
// Create a lambda expression that checks whether value exists and randomly
@ -155,14 +156,19 @@ Values TranslationRecovery::initializeRandomly(
insert(edge.key1());
insert(edge.key2());
}
for (auto edge : betweenTranslations) {
insert(edge.key1());
insert(edge.key2());
}
return initial;
}
Values TranslationRecovery::initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
const Values &initialValues) const {
return initializeRandomly(relativeTranslations, &kRandomNumberGenerator,
initialValues);
return initializeRandomly(relativeTranslations, betweenTranslations,
&kRandomNumberGenerator, initialValues);
}
Values TranslationRecovery::run(
@ -187,8 +193,8 @@ Values TranslationRecovery::run(
&graph);
// Uses initial values from params if provided.
Values initial =
initializeRandomly(nonzeroRelativeTranslations, initialValues);
Values initial = initializeRandomly(
nonzeroRelativeTranslations, nonzeroBetweenTranslations, initialValues);
// If there are no valid edges, but zero-distance edges exist, initialize one
// of the nodes in a connected component of zero-distance edges.

6
gtsam/sfm/TranslationRecovery.h
View File

@ -112,12 +112,15 @@ class TranslationRecovery {
*
* @param relativeTranslations unit translation directions between
* translations to be estimated
* @param betweenTranslations relative translations (with scale) between 2
* points in world coordinate frame known a priori.
* @param rng random number generator
* @param intialValues (optional) initial values from a prior
* @return Values
*/
Values initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
std::mt19937 *rng, const Values &initialValues = Values()) const;
/**
@ -125,11 +128,14 @@ class TranslationRecovery {
*
* @param relativeTranslations unit translation directions between
* translations to be estimated
* @param betweenTranslations relative translations (with scale) between 2
* points in world coordinate frame known a priori.
* @param initialValues (optional) initial values from a prior
* @return Values
*/
Values initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
const Values &initialValues = Values()) const;
/**