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( Values TranslationRecovery::initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations, const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
std::mt19937 *rng, const Values &initialValues) const { std::mt19937 *rng, const Values &initialValues) const {
uniform_real_distribution<double> randomVal(-1, 1); uniform_real_distribution<double> randomVal(-1, 1);
// Create a lambda expression that checks whether value exists and randomly // Create a lambda expression that checks whether value exists and randomly
@ -155,14 +156,19 @@ Values TranslationRecovery::initializeRandomly(
insert(edge.key1()); insert(edge.key1());
insert(edge.key2()); insert(edge.key2());
} }
for (auto edge : betweenTranslations) {
insert(edge.key1());
insert(edge.key2());
}
return initial; return initial;
} }
Values TranslationRecovery::initializeRandomly( Values TranslationRecovery::initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations, const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
const Values &initialValues) const { const Values &initialValues) const {
return initializeRandomly(relativeTranslations, &kRandomNumberGenerator, return initializeRandomly(relativeTranslations, betweenTranslations,
initialValues); &kRandomNumberGenerator, initialValues);
} }
Values TranslationRecovery::run( Values TranslationRecovery::run(
@ -187,8 +193,8 @@ Values TranslationRecovery::run(
&graph); &graph);
// Uses initial values from params if provided. // Uses initial values from params if provided.
Values initial = Values initial = initializeRandomly(
initializeRandomly(nonzeroRelativeTranslations, initialValues); nonzeroRelativeTranslations, nonzeroBetweenTranslations, initialValues);
// If there are no valid edges, but zero-distance edges exist, initialize one // If there are no valid edges, but zero-distance edges exist, initialize one
// of the nodes in a connected component of zero-distance edges. // 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 * @param relativeTranslations unit translation directions between
* translations to be estimated * 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 rng random number generator
* @param intialValues (optional) initial values from a prior * @param intialValues (optional) initial values from a prior
* @return Values * @return Values
*/ */
Values initializeRandomly( Values initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations, const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
std::mt19937 *rng, const Values &initialValues = Values()) const; std::mt19937 *rng, const Values &initialValues = Values()) const;
/** /**
@ -125,11 +128,14 @@ class TranslationRecovery {
* *
* @param relativeTranslations unit translation directions between * @param relativeTranslations unit translation directions between
* translations to be estimated * 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 * @param initialValues (optional) initial values from a prior
* @return Values * @return Values
*/ */
Values initializeRandomly( Values initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations, const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
const Values &initialValues = Values()) const; const Values &initialValues = Values()) const;
/** /**