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

new Pose2 class

release/4.3a0
Frank Dellaert 2009-08-29 01:24:26 +00:00
parent 6e23d61221
commit 3432b6c50f
5 changed files with 136 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
.cproject
View File

@ -300,7 +300,6 @@
<buildTargets>
<target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -308,6 +307,7 @@
</target>
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -315,6 +315,7 @@
</target>
<target name="testCal3_S2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testCal3_S2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -322,7 +323,6 @@
</target>
<target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testVSLAMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -330,6 +330,7 @@
</target>
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -337,15 +338,21 @@
</target>
<target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testConditionalGaussian.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -353,7 +360,6 @@
</target>
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -361,7 +367,6 @@
</target>
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>

6
cpp/Makefile.am
View File

@ -100,15 +100,17 @@ testConstrainedLinearFactorGraph_LDADD = libgtsam.la
testConstrainedChordalBayesNet_LDADD = libgtsam.la
# geometry
sources += Point2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
check_PROGRAMS += testPoint2 testPoint3 testRot3 testPose3 testCal3_S2
sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
check_PROGRAMS += testPoint2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2
testPoint2_SOURCES = testPoint2.cpp
testPose2_SOURCES = testPose2.cpp
testPoint3_SOURCES = testPoint3.cpp
testRot3_SOURCES = testRot3.cpp
testPose3_SOURCES = testPose3.cpp
testCal3_S2_SOURCES = testCal3_S2.cpp
testPoint2_LDADD = libgtsam.la
testPose2_LDADD = libgtsam.la
testPoint3_LDADD = libgtsam.la
testRot3_LDADD = libgtsam.la
testPose3_LDADD = libgtsam.la

39
cpp/Pose2.cpp Normal file
View File

@ -0,0 +1,39 @@
/**
* @file Pose2.cpp
* @brief 2D Pose
*/
#include "Pose2.h"
using namespace std;
namespace gtsam {
/* ************************************************************************* */
void Pose2::print(const string& s) const {
cout << s << "(" << x_ << ", " << y_ << ", " << theta_ << ")" << endl;
}
/* ************************************************************************* */
Pose2 Pose2::exmap(const Vector& v) const {
return Pose2(x_ + v(0), y_ + v(1), theta_ + v(2));
}
/* ************************************************************************* */
bool Pose2::equals(const Pose2& q, double tol) const {
return (fabs(x_ - q.x_) < tol && fabs(y_ - q.y_) < tol && fabs(theta_
- q.theta_) < tol);
}
/* ************************************************************************* */
bool assert_equal(const Pose2& A, const Pose2& B, double tol) {
if (A.equals(B, tol)) return true;
printf("not equal:\n");
A.print("A");
B.print("B");
return false;
}
/* ************************************************************************* */
} // namespace gtsam

57
cpp/Pose2.h Normal file
View File

@ -0,0 +1,57 @@
/**
* @file Pose2.h
* @brief 3D Pose
* @author: Frank Dellaert
*/
// \callgraph
#pragma once
#include "Point2.h"
namespace gtsam {
/**
* A 2D pose (x,y,theta)
*/
class Pose2 {
private:
double x_, y_, theta_;
public:
/** default constructor = origin */
Pose2() :
x_(0), y_(0), theta_(0) {
} // default is origin
/** copy constructor */
Pose2(const Pose2& pose) :
x_(pose.x_), y_(pose.y_), theta_(pose.theta_) {
}
/** construct from (x,y,theta) */
Pose2(double x, double y, double theta) :
x_(x), y_(y), theta_(theta) {
}
/** construct from rotation and translation */
Pose2(const Point2& t, double theta) :
x_(t.x()), y_(t.y()), theta_(theta) {
}
/** print with optional string */
void print(const std::string& s = "") const;
Pose2 exmap(const Vector& v) const;
/** assert equality up to a tolerance */
bool equals(const Pose2& pose, double tol = 1e-9) const;
};
/** assert equality up to a tolerance */
bool assert_equal(const Pose2& A, const Pose2& B, double tol = 1e-9);
} // namespace gtsam

25
cpp/testPose2.cpp Normal file
View File

@ -0,0 +1,25 @@
/**
* @file testPose2.cpp
* @brief Unit tests for Pose2 class
*/
#include <CppUnitLite/TestHarness.h>
#include "Pose2.h"
using namespace gtsam;
/* ************************************************************************* */
TEST(Pose2, constructors) {
Point2 p;
Pose2 pose(p,0);
Pose2 origin;
assert_equal(pose,origin);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */