diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index 36f9ad8ca..102dcc315 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -83,7 +83,7 @@ int main(int argc, char** argv) { Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90), bearing32 = Rot2::fromDegrees(90); - double range11 = sqrt(4+4), + double range11 = sqrt(4.0+4.0), range21 = 2.0, range32 = 2.0; diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp index b6507d7fc..ef704c5fc 100644 --- a/examples/Pose2SLAMExample_easy.cpp +++ b/examples/Pose2SLAMExample_easy.cpp @@ -17,8 +17,8 @@ */ // pull in the Pose2 SLAM domain with all typedefs and helper functions defined -#include #include +#include using namespace std; using namespace gtsam; @@ -36,13 +36,13 @@ int main(int argc, char** argv) { // 2b. Add odometry factors SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); 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(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addOdometry(4, 5, 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.0), odometryNoise); + graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI/2.0), odometryNoise); // 2c. Add pose constraint 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 graph.print("\nFactor graph:\n"); @@ -51,9 +51,9 @@ int main(int argc, char** argv) { pose2SLAM::Values initialEstimate; 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 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 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 "); // 4. Single Step Optimization using Levenberg-Marquardt diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index 71d5516a2..7c16072d8 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -76,11 +76,11 @@ void readAllDataISAM() { /** * Setup newFactors and initialValues for each new pose and set of measurements at each frame. */ -void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, +void createNewFactors(boost::shared_ptr& newFactors, boost::shared_ptr& initialValues, int pose_id, const Pose3& pose, const std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { // Create a graph of newFactors with new measurements - newFactors = shared_ptr (new Graph()); + newFactors = boost::shared_ptr (new Graph()); for (size_t i = 0; i < measurements.size(); i++) { newFactors->addMeasurement( measurements[i].m_p, @@ -97,7 +97,7 @@ void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& } // Create initial values for all nodes in the newFactors - initialValues = shared_ptr (new Values()); + initialValues = boost::shared_ptr (new Values()); initialValues->insert(PoseKey(pose_id), pose); for (size_t i = 0; i < measurements.size(); i++) { 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 > FeatureMap; BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) { const int poseId = features.first; - shared_ptr newFactors; - shared_ptr initialValues; + boost::shared_ptr newFactors; + boost::shared_ptr initialValues; createNewFactors(newFactors, initialValues, poseId, g_poses[poseId], features.second, measurementSigma, g_calib);