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) {
return QuaternionChart::Expmap(theta,w);
// TODO return QuaternionChart::Expmap(theta,w);
return Rot3();
}
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return Rot3(quaternion_ * R2.quaternion_);
}
// Rot3 Rot3::compose(const Rot3& R2,
// OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
// if (H1) *H1 = R2.transpose();
// if (H2) *H2 = I3;
// return Rot3(quaternion_ * R2.quaternion_);
// }
/* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const {
@ -102,10 +103,10 @@ namespace gtsam {
}
/* ************************************************************************* */
Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const {
if (H1) *H1 = -matrix();
return Rot3(quaternion_.inverse());
}
// Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const {
// if (H1) *H1 = -matrix();
// return Rot3(quaternion_.inverse());
// }
/* ************************************************************************* */
// 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,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = -(R2.transpose()*matrix());
if (H2) *H2 = I3;
return inverse() * R2;
}
// Rot3 Rot3::between(const Rot3& R2,
// OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
// if (H1) *H1 = -(R2.transpose()*matrix());
// if (H2) *H2 = I3;
// return inverse() * R2;
// }
/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
@ -135,20 +136,35 @@ namespace gtsam {
/* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
if(H) *H = Rot3::LogmapDerivative(thetaR);
return QuaternionChart::Logmap(R.quaternion_);
// if(H) *H = Rot3::LogmapDerivative(thetaR);
// return QuaternionChart::Logmap(R.quaternion_);
return Vector3(0,0,0);
}
/* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
return compose(Expmap(omega));
Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
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 {
return Logmap(between(t2));
Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
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();}