Loading...
Searching...
No Matches
RotateFactor.h
/*
* @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 {
class RotateFactor: public NoiseModelFactorN<Rot3> {
Point3 p_, z_;
typedef NoiseModelFactorN<Rot3> Base;
typedef RotateFactor This;
public:
RotateFactor(Key key, const Rot3& P, const Rot3& Z,
const SharedNoiseModel& model) :
Base(model, key), p_(Rot3::Logmap(P)), z_(Rot3::Logmap(Z)) {
}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
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 evaluateError(const Rot3& R,
boost::optional<Matrix&> H = boost::none) 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();
}
};
class RotateDirectionsFactor: public NoiseModelFactorN<Rot3> {
Unit3 i_p_, c_z_;
typedef NoiseModelFactorN<Rot3> Base;
typedef RotateDirectionsFactor This;
public:
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) {
}
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);
}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
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 evaluateError(const Rot3& iRc, boost::optional<Matrix&> H = boost::none) 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
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Rot3.h
3D rotation represented as a rotation matrix or quaternion
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from rotated coordinate frame to world
Definition Rot3M.cpp:149
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
gtsam::Unit3::unitVector
Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Vector.
Definition Unit3.cpp:151
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
gtsam::NoiseModelFactorN< Rot3 >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::NoiseModelFactorN< Rot3 >::key
Key key() const
Definition NonlinearFactor.h:518
gtsam::RotateFactor::RotateFactor
RotateFactor(Key key, const Rot3 &P, const Rot3 &Z, const SharedNoiseModel &model)
Constructor.
Definition RotateFactor.h:33
gtsam::RotateFactor::evaluateError
Vector evaluateError(const Rot3 &R, boost::optional< Matrix & > H=boost::none) const override
vector of errors returns 2D vector
Definition RotateFactor.h:53
gtsam::RotateFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition RotateFactor.h:44
gtsam::RotateFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition RotateFactor.h:39
gtsam::RotateDirectionsFactor::evaluateError
Vector evaluateError(const Rot3 &iRc, boost::optional< Matrix & > H=boost::none) const override
vector of errors returns 2D vector
Definition RotateFactor.h:105
gtsam::RotateDirectionsFactor::RotateDirectionsFactor
RotateDirectionsFactor(Key key, const Unit3 &i_p, const Unit3 &c_z, const SharedNoiseModel &model)
Constructor.
Definition RotateFactor.h:77
gtsam::RotateDirectionsFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition RotateFactor.h:96
gtsam::RotateDirectionsFactor::Initialize
static Rot3 Initialize(const Unit3 &i_p, const Unit3 &c_z)
Initialize rotation iRc such that i_p = iRc * c_z.
Definition RotateFactor.h:83
gtsam::RotateDirectionsFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition RotateFactor.h:91