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

Fixed examples compiling on windows

release/4.3a0
Richard Roberts 2012-05-28 20:48:32 +00:00
parent 363b44057b
commit 069be2b143
3 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -83,7 +83,7 @@ int main(int argc, char** argv) {
Rot2 bearing11 = Rot2::fromDegrees(45), Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90), bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90); bearing32 = Rot2::fromDegrees(90);
double range11 = sqrt(4+4), double range11 = sqrt(4.0+4.0),
range21 = 2.0, range21 = 2.0,
range32 = 2.0; range32 = 2.0;

14
examples/Pose2SLAMExample_easy.cpp
View File

@ -17,8 +17,8 @@
*/ */
// pull in the Pose2 SLAM domain with all typedefs and helper functions defined // pull in the Pose2 SLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/pose2SLAM.h>
#include <cmath> #include <cmath>
#include <gtsam/slam/pose2SLAM.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -36,13 +36,13 @@ int main(int argc, char** argv) {
// 2b. Add odometry factors // 2b. Add odometry factors
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1));
graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI/2.0), odometryNoise);
graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI/2.0), odometryNoise);
graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI/2.0), odometryNoise);
// 2c. Add pose constraint // 2c. Add pose constraint
SharedDiagonal constraintUncertainty(Vector_(3, 0.2, 0.2, 0.1)); SharedDiagonal constraintUncertainty(Vector_(3, 0.2, 0.2, 0.1));
graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI/2.0), constraintUncertainty);
// print // print
graph.print("\nFactor graph:\n"); graph.print("\nFactor graph:\n");
@ -51,9 +51,9 @@ int main(int argc, char** argv) {
pose2SLAM::Values initialEstimate; pose2SLAM::Values initialEstimate;
Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1); Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1);
Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2); Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2);
Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insertPose(3, x3); Pose2 x3(4.1, 0.1, M_PI/2.0); initialEstimate.insertPose(3, x3);
Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insertPose(4, x4); Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insertPose(4, x4);
Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insertPose(5, x5); Pose2 x5(2.1, 2.1,-M_PI/2.0); initialEstimate.insertPose(5, x5);
initialEstimate.print("\nInitial estimate:\n "); initialEstimate.print("\nInitial estimate:\n ");
// 4. Single Step Optimization using Levenberg-Marquardt // 4. Single Step Optimization using Levenberg-Marquardt

10
examples/vSLAMexample/vISAMexample.cpp
View File

@ -76,11 +76,11 @@ void readAllDataISAM() {
/** /**
* Setup newFactors and initialValues for each new pose and set of measurements at each frame. * Setup newFactors and initialValues for each new pose and set of measurements at each frame.
*/ */
void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues, void createNewFactors(boost::shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
// Create a graph of newFactors with new measurements // Create a graph of newFactors with new measurements
newFactors = shared_ptr<Graph> (new Graph()); newFactors = boost::shared_ptr<Graph> (new Graph());
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
newFactors->addMeasurement( newFactors->addMeasurement(
measurements[i].m_p, measurements[i].m_p,
@ -97,7 +97,7 @@ void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>&
} }
// Create initial values for all nodes in the newFactors // Create initial values for all nodes in the newFactors
initialValues = shared_ptr<Values> (new Values()); initialValues = boost::shared_ptr<Values> (new Values());
initialValues->insert(PoseKey(pose_id), pose); initialValues->insert(PoseKey(pose_id), pose);
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
initialValues->insert(PointKey(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]); initialValues->insert(PointKey(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
@ -127,8 +127,8 @@ int main(int argc, char* argv[]) {
typedef std::map<int, std::vector<Feature2D> > FeatureMap; typedef std::map<int, std::vector<Feature2D> > FeatureMap;
BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) { BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) {
const int poseId = features.first; const int poseId = features.first;
shared_ptr<Graph> newFactors; boost::shared_ptr<Graph> newFactors;
shared_ptr<Values> initialValues; boost::shared_ptr<Values> initialValues;
createNewFactors(newFactors, initialValues, poseId, g_poses[poseId], createNewFactors(newFactors, initialValues, poseId, g_poses[poseId],
features.second, measurementSigma, g_calib); features.second, measurementSigma, g_calib);