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

Constructor from Pose2

release/4.3a0
Frank Dellaert 2012-01-10 05:05:36 +00:00
parent 8242fdadb4
commit 659e524fa0
4 changed files with 27 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam.h
View File

@ -172,6 +172,7 @@ class Pose3 {
Pose3(const Rot3& r, const Point3& t);
Pose3(Vector v);
Pose3(Matrix t);
Pose3(const Pose2& pose2);
static Pose3 Expmap(Vector v);
static Vector Logmap(const Pose3& p);
void print(string s) const;

9
gtsam/geometry/Pose3.cpp
View File

@ -14,9 +14,10 @@
* @brief 3D Pose
*/
#include <gtsam/base/Lie-inl.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/base/Lie-inl.h>
#include <iostream>
#include <cmath>
@ -32,6 +33,12 @@ namespace gtsam {
static const Matrix I3 = eye(3), Z3 = zeros(3, 3), _I3=-I3, I6 = eye(6);
/* ************************************************************************* */
Pose3::Pose3(const Pose2& pose2) :
R_(Rot3::rodriguez(0, 0, pose2.theta())),
t_(Point3(pose2.x(), pose2.y(), 0)) {
}
/* ************************************************************************* */
// Calculate Adjoint map
// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)

5
gtsam/geometry/Pose3.h
View File

@ -27,6 +27,8 @@
namespace gtsam {
class Pose2; // forward declare
/**
* A 3D pose (R,t) : (Rot3,Point3)
* @ingroup geometry
@ -54,6 +56,9 @@ namespace gtsam {
/** Construct from R,t */
Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {}
/** Construct from Pose2 */
Pose3(const Pose2& pose2);
/** Constructor from 4*4 matrix */
Pose3(const Matrix &T) :
R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0),

17
gtsam/geometry/tests/testPose3.cpp
View File

@ -14,12 +14,13 @@
* @brief Unit tests for Pose3 class
*/
#include <cmath>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/geometry/Pose3.h>
#include <CppUnitLite/TestHarness.h>
#include <cmath>
using namespace std;
using namespace gtsam;
@ -43,6 +44,14 @@ TEST( Pose3, equals)
EXPECT(!T3.equals(origin));
}
/* ************************************************************************* */
TEST( Pose3, constructors)
{
Pose3 expected(Rot3::rodriguez(0,0,3),Point3(1,2,0));
Pose2 pose2(1,2,3);
EXPECT(assert_equal(expected,Pose3(pose2)));
}
/* ************************************************************************* */
TEST( Pose3, retract_first_order)
{