-
Notifications
You must be signed in to change notification settings - Fork 764
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'develop' of github.com:borglab/gtsam into feature/essen…
…tial-mat-with-approx-k
- Loading branch information
Showing
30 changed files
with
3,466 additions
and
100 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,76 @@ | ||
/* ---------------------------------------------------------------------------- | ||
* GTSAM Copyright 2010, 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 | ||
* -------------------------------------------------------------------------- */ | ||
|
||
/** | ||
* @file CustomFactor.cpp | ||
* @brief Class to enable arbitrary factors with runtime swappable error function. | ||
* @author Fan Jiang | ||
*/ | ||
|
||
#include <gtsam/nonlinear/CustomFactor.h> | ||
|
||
namespace gtsam { | ||
|
||
/* | ||
* Calculates the unwhitened error by invoking the callback functor (i.e. from Python). | ||
*/ | ||
Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H) const { | ||
if(this->active(x)) { | ||
|
||
if(H) { | ||
/* | ||
* In this case, we pass the raw pointer to the `std::vector<Matrix>` object directly to pybind. | ||
* As the type `std::vector<Matrix>` has been marked as opaque in `preamble.h`, any changes in | ||
* Python will be immediately reflected on the C++ side. | ||
* | ||
* Example: | ||
* ``` | ||
* def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): | ||
* <calculated error> | ||
* if not H is None: | ||
* <calculate the Jacobian> | ||
* H[0] = J1 | ||
* H[1] = J2 | ||
* ... | ||
* return error | ||
* ``` | ||
*/ | ||
return this->error_function_(*this, x, H.get_ptr()); | ||
} else { | ||
/* | ||
* In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python. | ||
* Users can check for `None` in their callback to determine if the Jacobian is requested. | ||
*/ | ||
return this->error_function_(*this, x, nullptr); | ||
} | ||
} else { | ||
return Vector::Zero(this->dim()); | ||
} | ||
} | ||
|
||
void CustomFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const { | ||
std::cout << s << "CustomFactor on "; | ||
auto keys_ = this->keys(); | ||
bool f = false; | ||
for (const Key &k: keys_) { | ||
if (f) | ||
std::cout << ", "; | ||
std::cout << keyFormatter(k); | ||
f = true; | ||
} | ||
std::cout << "\n"; | ||
if (this->noiseModel_) | ||
this->noiseModel_->print(" noise model: "); | ||
else | ||
std::cout << "no noise model" << std::endl; | ||
} | ||
|
||
} |
Oops, something went wrong.