120 lines
3.5 KiB
C++
120 lines
3.5 KiB
C++
/*
|
|
* @file RotateFactor.cpp
|
|
* @brief RotateFactor class
|
|
* @author Frank Dellaert
|
|
* @date December 17, 2013
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
|
#include <gtsam/geometry/Rot3.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* Factor on unknown rotation iRC that relates two incremental rotations
|
|
* c1Rc2 = iRc' * i1Ri2 * iRc
|
|
* Which we can write (see doc/math.lyx)
|
|
* e^[z] = iRc' * e^[p] * iRc = e^([iRc'*p])
|
|
* with z and p measured and predicted angular velocities, and hence
|
|
* p = iRc * z
|
|
*/
|
|
class RotateFactor: public NoiseModelFactorN<Rot3> {
|
|
|
|
Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z
|
|
|
|
typedef NoiseModelFactorN<Rot3> Base;
|
|
typedef RotateFactor This;
|
|
|
|
public:
|
|
|
|
/// Constructor
|
|
RotateFactor(Key key, const Rot3& P, const Rot3& Z,
|
|
const SharedNoiseModel& model) :
|
|
Base(model, key), p_(Rot3::Logmap(P)), z_(Rot3::Logmap(Z)) {
|
|
}
|
|
|
|
/// @return a deep copy of this factor
|
|
gtsam::NonlinearFactor::shared_ptr clone() const override {
|
|
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
|
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
|
|
|
/// print
|
|
void print(const std::string& s = "",
|
|
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
|
|
Base::print(s);
|
|
std::cout << "RotateFactor:]\n";
|
|
std::cout << "p: " << p_.transpose() << std::endl;
|
|
std::cout << "z: " << z_.transpose() << std::endl;
|
|
}
|
|
|
|
/// vector of errors returns 2D vector
|
|
Vector evaluateError(const Rot3& R,
|
|
OptionalMatrixType H = OptionalNone) const override {
|
|
// predict p_ as q = R*z_, derivative H will be filled if not none
|
|
Point3 q = R.rotate(z_,H);
|
|
// error is just difference, and note derivative of that wrpt q is I3
|
|
return (Vector(3) << q.x()-p_.x(), q.y()-p_.y(), q.z()-p_.z()).finished();
|
|
}
|
|
|
|
};
|
|
|
|
/**
|
|
* Factor on unknown rotation iRc that relates two directions c
|
|
* Directions provide less constraints than a full rotation
|
|
*/
|
|
class RotateDirectionsFactor: public NoiseModelFactorN<Rot3> {
|
|
|
|
Unit3 i_p_, c_z_; ///< Predicted and measured directions, i_p = iRc * c_z
|
|
|
|
typedef NoiseModelFactorN<Rot3> Base;
|
|
typedef RotateDirectionsFactor This;
|
|
|
|
public:
|
|
|
|
/// Constructor
|
|
RotateDirectionsFactor(Key key, const Unit3& i_p, const Unit3& c_z,
|
|
const SharedNoiseModel& model) :
|
|
Base(model, key), i_p_(i_p), c_z_(c_z) {
|
|
}
|
|
|
|
/// Initialize rotation iRc such that i_p = iRc * c_z
|
|
static Rot3 Initialize(const Unit3& i_p, const Unit3& c_z) {
|
|
gtsam::Quaternion iRc;
|
|
// setFromTwoVectors sets iRc to (a) quaternion which transform c_z into i_p
|
|
iRc.setFromTwoVectors(c_z.unitVector(), i_p.unitVector());
|
|
return Rot3(iRc);
|
|
}
|
|
|
|
/// @return a deep copy of this factor
|
|
gtsam::NonlinearFactor::shared_ptr clone() const override {
|
|
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
|
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
|
|
|
/// print
|
|
void print(const std::string& s = "",
|
|
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
|
|
Base::print(s);
|
|
std::cout << "RotateDirectionsFactor:" << std::endl;
|
|
i_p_.print("p");
|
|
c_z_.print("z");
|
|
}
|
|
|
|
/// vector of errors returns 2D vector
|
|
Vector evaluateError(const Rot3& iRc, OptionalMatrixType H = OptionalNone) const override {
|
|
Unit3 i_q = iRc * c_z_;
|
|
Vector error = i_p_.error(i_q, H);
|
|
if (H) {
|
|
Matrix DR;
|
|
iRc.rotate(c_z_, DR);
|
|
*H = (*H) * DR;
|
|
}
|
|
return error;
|
|
}
|
|
|
|
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
|
|
};
|
|
} // namespace gtsam
|
|
|