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

Put in dummy code so compiles and links - tests fail, obviously

release/4.3a0
dellaert 2015-02-10 19:04:25 +01:00
parent cb09af39a5
commit 7508aa1fec
1 changed files with 39 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
gtsam/geometry/Rot3Q.cpp
View File

@ -85,16 +85,17 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector3& w, double theta) { Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
return QuaternionChart::Expmap(theta,w); // TODO return QuaternionChart::Expmap(theta,w);
return Rot3();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2, // Rot3 Rot3::compose(const Rot3& R2,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { // OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = R2.transpose(); // if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3; // if (H2) *H2 = I3;
return Rot3(quaternion_ * R2.quaternion_); // return Rot3(quaternion_ * R2.quaternion_);
} // }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const { Rot3 Rot3::operator*(const Rot3& R2) const {
@ -102,10 +103,10 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const { // Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const {
if (H1) *H1 = -matrix(); // if (H1) *H1 = -matrix();
return Rot3(quaternion_.inverse()); // return Rot3(quaternion_.inverse());
} // }
/* ************************************************************************* */ /* ************************************************************************* */
// TODO: Could we do this? It works in Rot3M but not here, probably because // TODO: Could we do this? It works in Rot3M but not here, probably because
@ -116,12 +117,12 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::between(const Rot3& R2, // Rot3 Rot3::between(const Rot3& R2,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { // OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = -(R2.transpose()*matrix()); // if (H1) *H1 = -(R2.transpose()*matrix());
if (H2) *H2 = I3; // if (H2) *H2 = I3;
return inverse() * R2; // return inverse() * R2;
} // }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p, Point3 Rot3::rotate(const Point3& p,
@ -135,20 +136,35 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) { Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
if(H) *H = Rot3::LogmapDerivative(thetaR); // if(H) *H = Rot3::LogmapDerivative(thetaR);
return QuaternionChart::Logmap(R.quaternion_); // return QuaternionChart::Logmap(R.quaternion_);
return Vector3(0,0,0);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
return compose(Expmap(omega)); static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
if (mode == Rot3::EXPMAP) return Expmap(omega, H);
else throw std::runtime_error("Rot3::Retract: unknown mode");
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const { Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
return Logmap(between(t2)); static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
if (mode == Rot3::EXPMAP) return Logmap(R, H);
else throw std::runtime_error("Rot3::Local: unknown mode");
} }
/* ************************************************************************* */
// Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
// return compose(Expmap(omega));
// }
/* ************************************************************************* */
// Vector3 Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const {
// return Logmap(between(t2));
// }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();} Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();}