fix warning in FrobeniusWormholeFactor
parent
269dea3a24
commit
00a9564299
|
@ -32,7 +32,7 @@ namespace gtsam {
|
|||
|
||||
//******************************************************************************
|
||||
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;
|
||||
if (model != nullptr) {
|
||||
if (model->dim() != 6) {
|
||||
|
@ -56,26 +56,29 @@ FrobeniusWormholeFactor::FrobeniusWormholeFactor(
|
|||
Key j1, Key j2, const Rot3 &R12, size_t p, const SharedNoiseModel &model,
|
||||
const boost::shared_ptr<Matrix> &G)
|
||||
: NoiseModelFactor2<SOn, SOn>(ConvertPose3NoiseModel(model, p * 3), j1, j2),
|
||||
M_(R12.matrix()), // 3*3 in all cases
|
||||
p_(p), // 4 for SO(4)
|
||||
pp_(p * p), // 16 for SO(4)
|
||||
M_(R12.matrix()), // 3*3 in all cases
|
||||
p_(p), // 4 for SO(4)
|
||||
pp_(p * p), // 16 for SO(4)
|
||||
G_(G) {
|
||||
if (noiseModel()->dim() != 3 * p_)
|
||||
throw std::invalid_argument(
|
||||
"FrobeniusWormholeFactor: model with incorrect dimension.");
|
||||
if (!G) {
|
||||
G_ = boost::make_shared<Matrix>();
|
||||
*G_ = SOn::VectorizedGenerators(p); // expensive!
|
||||
*G_ = SOn::VectorizedGenerators(p); // expensive!
|
||||
}
|
||||
if (G_->rows() != pp_ || G_->cols() != SOn::Dimension(p))
|
||||
throw std::invalid_argument("FrobeniusWormholeFactor: passed in generators "
|
||||
"of incorrect dimension.");
|
||||
if (static_cast<size_t>(G_->rows()) != pp_ ||
|
||||
static_cast<size_t>(G_->cols()) != SOn::Dimension(p))
|
||||
throw std::invalid_argument(
|
||||
"FrobeniusWormholeFactor: passed in generators "
|
||||
"of incorrect dimension.");
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
void FrobeniusWormholeFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const {
|
||||
std::cout << s << "FrobeniusWormholeFactor<" << p_ << ">(" << keyFormatter(key1()) << ","
|
||||
<< keyFormatter(key2()) << ")\n";
|
||||
void FrobeniusWormholeFactor::print(const std::string &s,
|
||||
const KeyFormatter &keyFormatter) const {
|
||||
std::cout << s << "FrobeniusWormholeFactor<" << p_ << ">("
|
||||
<< keyFormatter(key1()) << "," << keyFormatter(key2()) << ")\n";
|
||||
traits<Matrix>::Print(M_, " M: ");
|
||||
noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
@ -90,12 +93,12 @@ bool FrobeniusWormholeFactor::equals(const NonlinearFactor &expected,
|
|||
|
||||
//******************************************************************************
|
||||
Vector FrobeniusWormholeFactor::evaluateError(
|
||||
const SOn& Q1, const SOn& Q2, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
const SOn &Q1, const SOn &Q2, boost::optional<Matrix &> H1,
|
||||
boost::optional<Matrix &> H2) const {
|
||||
gttic(FrobeniusWormholeFactorP_evaluateError);
|
||||
|
||||
const Matrix& M1 = Q1.matrix();
|
||||
const Matrix& M2 = Q2.matrix();
|
||||
const Matrix &M1 = Q1.matrix();
|
||||
const Matrix &M2 = Q2.matrix();
|
||||
assert(M1.rows() == p_ && M2.rows() == p_);
|
||||
|
||||
const size_t dim = 3 * p_; // Stiefel manifold dimension
|
||||
|
|
Loading…
Reference in New Issue