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

fix warning in FrobeniusWormholeFactor

release/4.3a0
Varun Agrawal 2020-08-02 20:55:50 -05:00
parent 269dea3a24
commit 00a9564299
1 changed files with 18 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/slam/FrobeniusFactor.cpp
View File

@ -32,7 +32,7 @@ namespace gtsam {
//****************************************************************************** //******************************************************************************
boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel( boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
const SharedNoiseModel& model, size_t d, bool defaultToUnit) { const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
double sigma = 1.0; double sigma = 1.0;
if (model != nullptr) { if (model != nullptr) {
if (model->dim() != 6) { if (model->dim() != 6) {
@ -67,15 +67,18 @@ FrobeniusWormholeFactor::FrobeniusWormholeFactor(
G_ = boost::make_shared<Matrix>(); G_ = boost::make_shared<Matrix>();
*G_ = SOn::VectorizedGenerators(p); // expensive! *G_ = SOn::VectorizedGenerators(p); // expensive!
} }
if (G_->rows() != pp_ || G_->cols() != SOn::Dimension(p)) if (static_cast<size_t>(G_->rows()) != pp_ ||
throw std::invalid_argument("FrobeniusWormholeFactor: passed in generators " static_cast<size_t>(G_->cols()) != SOn::Dimension(p))
throw std::invalid_argument(
"FrobeniusWormholeFactor: passed in generators "
"of incorrect dimension."); "of incorrect dimension.");
} }
//****************************************************************************** //******************************************************************************
void FrobeniusWormholeFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const { void FrobeniusWormholeFactor::print(const std::string &s,
std::cout << s << "FrobeniusWormholeFactor<" << p_ << ">(" << keyFormatter(key1()) << "," const KeyFormatter &keyFormatter) const {
<< keyFormatter(key2()) << ")\n"; std::cout << s << "FrobeniusWormholeFactor<" << p_ << ">("
<< keyFormatter(key1()) << "," << keyFormatter(key2()) << ")\n";
traits<Matrix>::Print(M_, " M: "); traits<Matrix>::Print(M_, " M: ");
noiseModel_->print(" noise model: "); noiseModel_->print(" noise model: ");
} }
@ -90,12 +93,12 @@ bool FrobeniusWormholeFactor::equals(const NonlinearFactor &expected,
//****************************************************************************** //******************************************************************************
Vector FrobeniusWormholeFactor::evaluateError( Vector FrobeniusWormholeFactor::evaluateError(
const SOn& Q1, const SOn& Q2, boost::optional<Matrix&> H1, const SOn &Q1, const SOn &Q2, boost::optional<Matrix &> H1,
boost::optional<Matrix&> H2) const { boost::optional<Matrix &> H2) const {
gttic(FrobeniusWormholeFactorP_evaluateError); gttic(FrobeniusWormholeFactorP_evaluateError);
const Matrix& M1 = Q1.matrix(); const Matrix &M1 = Q1.matrix();
const Matrix& M2 = Q2.matrix(); const Matrix &M2 = Q2.matrix();
assert(M1.rows() == p_ && M2.rows() == p_); assert(M1.rows() == p_ && M2.rows() == p_);
const size_t dim = 3 * p_; // Stiefel manifold dimension const size_t dim = 3 * p_; // Stiefel manifold dimension