From 60707450ca2a93b209d77643f63d0d8d09fa0657 Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Sun, 24 Nov 2019 23:01:43 -0500 Subject: [PATCH 01/55] Incorporated odometry measurements to the factor graph --- external/rj-ros-common | 2 +- external/velodyne | 1 + igvc_perception/CMakeLists.txt | 1 + igvc_perception/include/slam/slam.h | 51 ++++++++++ igvc_perception/launch/slam.launch | 7 ++ .../src/projection/.idea/workspace.xml | 94 +++++++++++++++++++ igvc_perception/src/slam/CMakeLists.txt | 25 +++++ igvc_perception/src/slam/main.cpp | 8 ++ igvc_perception/src/slam/slam.cpp | 48 ++++++++++ 9 files changed, 236 insertions(+), 1 deletion(-) create mode 160000 external/velodyne create mode 100644 igvc_perception/include/slam/slam.h create mode 100644 igvc_perception/launch/slam.launch create mode 100644 igvc_perception/src/projection/.idea/workspace.xml create mode 100644 igvc_perception/src/slam/CMakeLists.txt create mode 100644 igvc_perception/src/slam/main.cpp create mode 100644 igvc_perception/src/slam/slam.cpp diff --git a/external/rj-ros-common b/external/rj-ros-common index 4578f74eb..48e661eee 160000 --- a/external/rj-ros-common +++ b/external/rj-ros-common @@ -1 +1 @@ -Subproject commit 4578f74ebf294a470952ce190e3299176f6bdd86 +Subproject commit 48e661eee71057ca684c5a8ba62a1811a9285b23 diff --git a/external/velodyne b/external/velodyne new file mode 160000 index 000000000..720829573 --- /dev/null +++ b/external/velodyne @@ -0,0 +1 @@ +Subproject commit 720829573076bf7d7edd4b1de344924fad909049 diff --git a/igvc_perception/CMakeLists.txt b/igvc_perception/CMakeLists.txt index a180b88b3..648e54912 100644 --- a/igvc_perception/CMakeLists.txt +++ b/igvc_perception/CMakeLists.txt @@ -178,6 +178,7 @@ if (CATKIN_ENABLE_TESTING) add_subdirectory(src/tests) endif () +add_subdirectory(src/slam) add_subdirectory(src/filter_lidar) add_subdirectory(src/projection) add_subdirectory(src/pointcloud_filter) diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h new file mode 100644 index 000000000..b339d904c --- /dev/null +++ b/igvc_perception/include/slam/slam.h @@ -0,0 +1,51 @@ +#ifndef SRC_SLAM_H +#define SRC_SLAM_H + +#include +#include +// Pose2 == (x, y, theta) +#include +// PriorFactor == Initial Pose +#include +// BetweenFactor == Odom measurement +#include +// The factor graph we are creating. Nonlinear since angle measurements are nonlinear. +#include +// May want to choose a different optimizer later, but for now we use this one. +#include +#include +// Helps initialize initial guess +#include +#include +#include + +class Slam { +public: + Slam(); + +private: + ros::NodeHandle pnh_; + + ros::Subscriber odom_sub_; + ros::Subscriber imu_sub_; + //ros::Subscriber gps_sub_; + + ros::Publisher location_pub; + + + void ImuCallback(const sensor_msgs::Imu &msg); + void OdomCallback(const nav_msgs::Odometry &msg); + void Optimize(); + + // Defining some types + typedef gtsam::noiseModel::Diagonal noiseDiagonal; + + // Establishing global variables + gtsam::Values currEstimate; + gtsam::NonlinearFactorGraph graph; + gtsam::Pose2 previousPose; + long pose_index_; + double odomNoiseRatio; +}; + +#endif //SRC_SLAM_H diff --git a/igvc_perception/launch/slam.launch b/igvc_perception/launch/slam.launch new file mode 100644 index 000000000..a96d725ed --- /dev/null +++ b/igvc_perception/launch/slam.launch @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/igvc_perception/src/projection/.idea/workspace.xml b/igvc_perception/src/projection/.idea/workspace.xml new file mode 100644 index 000000000..4f1955cb6 --- /dev/null +++ b/igvc_perception/src/projection/.idea/workspace.xml @@ -0,0 +1,94 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1572483525476 + + + + + + + + + \ No newline at end of file diff --git a/igvc_perception/src/slam/CMakeLists.txt b/igvc_perception/src/slam/CMakeLists.txt new file mode 100644 index 000000000..3dcbc16a1 --- /dev/null +++ b/igvc_perception/src/slam/CMakeLists.txt @@ -0,0 +1,25 @@ +# Include GTSAM CMake tools +find_package(GTSAMCMakeTools) + +if (GTSAMCMakeTools_FOUND) + include(GtsamBuildTypes) # Load build type flags and default to Debug mode + include(GtsamTesting) # Easy functions for creating unit tests and scripts + + # Find GTSAM components + find_package(GTSAM REQUIRED) # Uses installed package + include_directories(${GTSAM_INCLUDE_DIR}) + if(GTSAM_FOUND) + message(STATUS "Found gtsam, compiling slam") + find_package(Eigen3 3.3 REQUIRED) + add_executable(slam + main.cpp + slam.cpp) + target_include_directories(slam PRIVATE ${catkin_INCLUDE_DIRS}) + target_link_libraries(slam gtsam ${catkin_LIBRARIES} Eigen3::Eigen) + add_dependencies(slam ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + else() + message(WARNING "Could not find gtsam, not compiling slam!") + endif() +else() + message(WARNING "Could not find GTSAMCMakeTools. Not compiling SLAM") +endif () \ No newline at end of file diff --git a/igvc_perception/src/slam/main.cpp b/igvc_perception/src/slam/main.cpp new file mode 100644 index 000000000..83c865520 --- /dev/null +++ b/igvc_perception/src/slam/main.cpp @@ -0,0 +1,8 @@ +#include +#include + +int main(int argc, char** argv) { + ros::init(argc, argv, "slam"); + Slam slam_node; + ros::spin(); +} \ No newline at end of file diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp new file mode 100644 index 000000000..0d64a3af8 --- /dev/null +++ b/igvc_perception/src/slam/slam.cpp @@ -0,0 +1,48 @@ +#include + + +Slam::Slam() : pnh_{ "~" } { + odom_sub_ = pnh_.subscribe("/wheel_odometry", 1, &Slam::OdomCallback, this); + imu_sub_ = pnh_.subscribe("/imu", 1, &Slam::ImuCallback, this); + location_pub = pnh_.advertise("/slam/position", 1); + + pose_index_ = 0; + odomNoiseRatio = pnh_.getParam("odomNoiseRatio", odomNoiseRatio); + + // Adding Initial Position (Pose + Covariance Matrix) + gtsam::Pose2 priorMean(0.0, 0.0, 0.0); + noiseDiagonal::shared_ptr priorNoise = noiseDiagonal::Sigmas(gtsam::Vector3(0.2, 0.2, 0.1)); + graph.emplace_shared>(0, priorMean, priorNoise); + currEstimate.insert(pose_index_, priorMean); +} + +void Slam::ImuCallback(const sensor_msgs::Imu &msg){ + +} + +void Slam::OdomCallback(const nav_msgs::Odometry &msg){ + gtsam::Pose2 currPose = gtsam::Pose2(msg.pose.pose.position.x, msg.pose.pose.position.y, tf::getYaw(msg.pose.pose.orientation)); + gtsam::Pose2 odometry = previousPose.between(currPose); + noiseDiagonal::shared_ptr odometryNoise = noiseDiagonal::Variances(gtsam::Vector3( + 0.02, 0.02, 0.01)); + graph.emplace_shared>(pose_index_, pose_index_+1, odometry, odometryNoise); + gtsam::Pose2 newPoseEstimate = currEstimate.at(pose_index_); + pose_index_++; + newPoseEstimate = odometry * newPoseEstimate; + currEstimate.insert(pose_index_, newPoseEstimate); + previousPose = currPose; + Optimize(); +} + +void Slam::Optimize() { + gtsam::LevenbergMarquardtOptimizer optimizer(graph, currEstimate); + currEstimate = optimizer.optimize(); + gtsam::Pose2 currPose = currEstimate.at(pose_index_); + nav_msgs::Odometry msg; + msg.child_frame_id = "/base_footprint"; + msg.header.frame_id = "/odom"; + msg.pose.pose.position.x = currPose.x(); + msg.pose.pose.position.y = currPose.y(); + msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(currPose.theta()); + location_pub.publish(msg); +} \ No newline at end of file From 3a3c7a7b861f288bc09b0b4023f5845173867d49 Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Sun, 1 Dec 2019 03:29:29 -0500 Subject: [PATCH 02/55] Added support for imu but it is not fully working yet. --- igvc_perception/include/slam/slam.h | 17 ++++- igvc_perception/launch/slam.launch | 3 +- igvc_perception/src/slam/slam.cpp | 112 +++++++++++++++++++++++----- 3 files changed, 110 insertions(+), 22 deletions(-) diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index b339d904c..2c5617388 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -18,6 +18,10 @@ #include #include #include +#include +#include +#include +#include class Slam { public: @@ -36,6 +40,8 @@ class Slam { void ImuCallback(const sensor_msgs::Imu &msg); void OdomCallback(const nav_msgs::Odometry &msg); void Optimize(); + void InitializeImuParams(); + void InitializePriors(); // Defining some types typedef gtsam::noiseModel::Diagonal noiseDiagonal; @@ -44,8 +50,15 @@ class Slam { gtsam::Values currEstimate; gtsam::NonlinearFactorGraph graph; gtsam::Pose2 previousPose; - long pose_index_; - double odomNoiseRatio; + unsigned long pose_index_; + int imu_bias_counter_; + int BIAS_UPDATE_RATE; + double BIAS_NOISE_CONST; + gtsam::ISAM2 isam; + const double KGRAVITY = 9.81; + gtsam::PreintegratedImuMeasurements accum; + ros::Time lastImuMeasurement; + bool imu_recieved_; }; #endif //SRC_SLAM_H diff --git a/igvc_perception/launch/slam.launch b/igvc_perception/launch/slam.launch index a96d725ed..2b3687e01 100644 --- a/igvc_perception/launch/slam.launch +++ b/igvc_perception/launch/slam.launch @@ -2,6 +2,7 @@ - + + \ No newline at end of file diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 0d64a3af8..ecbd7028b 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -1,5 +1,9 @@ #include +// Using statements +using gtsam::symbol_shorthand::V; +using gtsam::symbol_shorthand::X; +using gtsam::symbol_shorthand::B; Slam::Slam() : pnh_{ "~" } { odom_sub_ = pnh_.subscribe("/wheel_odometry", 1, &Slam::OdomCallback, this); @@ -7,37 +11,72 @@ Slam::Slam() : pnh_{ "~" } { location_pub = pnh_.advertise("/slam/position", 1); pose_index_ = 0; - odomNoiseRatio = pnh_.getParam("odomNoiseRatio", odomNoiseRatio); + imu_bias_counter_ = 0; + imu_recieved_ = false; + BIAS_UPDATE_RATE = pnh_.param("biasUpdateRatio", 20); + BIAS_NOISE_CONST = pnh_.param("biasNoiseConst", 0.001); - // Adding Initial Position (Pose + Covariance Matrix) - gtsam::Pose2 priorMean(0.0, 0.0, 0.0); - noiseDiagonal::shared_ptr priorNoise = noiseDiagonal::Sigmas(gtsam::Vector3(0.2, 0.2, 0.1)); - graph.emplace_shared>(0, priorMean, priorNoise); - currEstimate.insert(pose_index_, priorMean); + InitializeImuParams(); + InitializePriors(); } void Slam::ImuCallback(const sensor_msgs::Imu &msg){ - + ROS_INFO_STREAM("Imu called!"); + ros::Time currTime = ros::Time::now(); + gtsam::Vector3 measuredAcc = gtsam::Vector3(msg.linear_acceleration.x, msg.linear_acceleration.y, + msg.linear_acceleration.z); + gtsam::Vector3 measuredOmega = gtsam::Vector3(msg.angular_velocity.x, msg.angular_velocity.y, + msg.angular_velocity.z); + if (!imu_recieved_){ + imu_recieved_ = true; + accum.integrateMeasurement(measuredAcc, measuredOmega, 0.005); + } else { + double deltaT = (currTime-lastImuMeasurement).toSec(); + accum.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + } + lastImuMeasurement = currTime; } void Slam::OdomCallback(const nav_msgs::Odometry &msg){ - gtsam::Pose2 currPose = gtsam::Pose2(msg.pose.pose.position.x, msg.pose.pose.position.y, tf::getYaw(msg.pose.pose.orientation)); - gtsam::Pose2 odometry = previousPose.between(currPose); - noiseDiagonal::shared_ptr odometryNoise = noiseDiagonal::Variances(gtsam::Vector3( - 0.02, 0.02, 0.01)); - graph.emplace_shared>(pose_index_, pose_index_+1, odometry, odometryNoise); - gtsam::Pose2 newPoseEstimate = currEstimate.at(pose_index_); - pose_index_++; - newPoseEstimate = odometry * newPoseEstimate; - currEstimate.insert(pose_index_, newPoseEstimate); - previousPose = currPose; - Optimize(); + if (imu_recieved_) { + // Handle the odometry + gtsam::Pose2 currPose = gtsam::Pose2(msg.pose.pose.position.x, msg.pose.pose.position.y, + tf::getYaw(msg.pose.pose.orientation)); + gtsam::Pose2 odometry = previousPose.between(currPose); + noiseDiagonal::shared_ptr odometryNoise = noiseDiagonal::Variances(gtsam::Vector3( + 0.02, 0.02, 0.01)); + graph.emplace_shared>(pose_index_, pose_index_ + 1, odometry, odometryNoise); + auto newPoseEstimate = currEstimate.at(pose_index_); //gtsam::Pose2 newPoseEstimate + newPoseEstimate = odometry * newPoseEstimate; + currEstimate.insert(pose_index_+1, newPoseEstimate); + + // Add bias factor + auto cov = noiseDiagonal::Variances(gtsam::Vector6::Constant(BIAS_NOISE_CONST)); + auto factor = boost::make_shared >(B(pose_index_), + B(pose_index_+1), gtsam::imuBias::ConstantBias(), cov); + graph.add(factor); + currEstimate.insert(B(pose_index_+1), gtsam::imuBias::ConstantBias()); + + // Add imu factor + gtsam::ImuFactor imufac(X(pose_index_), V(pose_index_), X(pose_index_+1), + V(pose_index_+1), B(pose_index_+1), accum); + graph.add(imufac); + auto lastVel = currEstimate.at(V(pose_index_)); + lastVel.x() += accum.deltaVij().x(); + lastVel.y() += accum.deltaVij().y(); + currEstimate.insert(V(pose_index_+1), lastVel); + accum.resetIntegration(); + + pose_index_++; + previousPose = currPose; + Optimize(); + } } void Slam::Optimize() { gtsam::LevenbergMarquardtOptimizer optimizer(graph, currEstimate); currEstimate = optimizer.optimize(); - gtsam::Pose2 currPose = currEstimate.at(pose_index_); + auto currPose = currEstimate.at(pose_index_); //gtsam::Pose2 currPose nav_msgs::Odometry msg; msg.child_frame_id = "/base_footprint"; msg.header.frame_id = "/odom"; @@ -45,4 +84,39 @@ void Slam::Optimize() { msg.pose.pose.position.y = currPose.y(); msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(currPose.theta()); location_pub.publish(msg); +} + +void Slam::InitializeImuParams() { + // Should be replaced with actual imu measurements + boost::shared_ptr params; + params = gtsam::PreintegrationParams::MakeSharedU(KGRAVITY); + params->setAccelerometerCovariance(gtsam::I_3x3 * 0.1); + params->setGyroscopeCovariance(gtsam::I_3x3 * 0.1); + params->setIntegrationCovariance(gtsam::I_3x3 * 0.1); + params->setUse2ndOrderCoriolis(false); + params->setOmegaCoriolis(gtsam::Vector3(0, 0, 0)); + accum = gtsam::PreintegratedImuMeasurements(params); +} + +void Slam::InitializePriors(){ + // Adding Initial Position (Pose + Covariance Matrix) + gtsam::Pose2 priorPose(0.0, 0.0, 0.0); + noiseDiagonal::shared_ptr poseNoise = noiseDiagonal::Sigmas(gtsam::Vector3(0.3, 0.3, 0.1)); + graph.push_back(gtsam::PriorFactor(X(0), priorPose, poseNoise)); + currEstimate.insert(X(0), priorPose); + + // Adding Initial Velocity (Pose + Covariance Matrix) + gtsam::Vector2 priorVel(0.0, 0.0); + noiseDiagonal::shared_ptr velNoise = noiseDiagonal::Sigmas(gtsam::Vector2(0.1, 0.1)); + graph.push_back(gtsam::PriorFactor(V(0), priorVel, velNoise)); + currEstimate.insert(V(0), priorVel); + + // Adding Bias Prior + noiseDiagonal::shared_ptr biasNoise = noiseDiagonal::Sigmas(gtsam::Vector6::Constant(0.1)); + gtsam::PriorFactor biasprior(B(0), gtsam::imuBias::ConstantBias(), + biasNoise); + graph.push_back(biasprior); + currEstimate.insert(B(0), gtsam::imuBias::ConstantBias()); + + ROS_INFO_STREAM("Values initialized!"); } \ No newline at end of file From c0775564c6694bcfc5671e143bf66f82cbbb54cd Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Mon, 16 Dec 2019 21:02:42 -0500 Subject: [PATCH 03/55] fixed small issue with symbols --- igvc_perception/include/slam/slam.h | 16 ++-- .../include/slam/type_conversions.h | 16 ++++ igvc_perception/launch/slam.launch | 3 +- igvc_perception/src/slam/CMakeLists.txt | 5 +- igvc_perception/src/slam/slam.cpp | 93 +++++++++---------- igvc_perception/src/slam/type_conversions.cpp | 27 ++++++ 6 files changed, 100 insertions(+), 60 deletions(-) create mode 100644 igvc_perception/include/slam/type_conversions.h create mode 100644 igvc_perception/src/slam/type_conversions.cpp diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index 2c5617388..7251f9bd3 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -3,16 +3,14 @@ #include #include -// Pose2 == (x, y, theta) -#include +// Pose2 == (Point3, Rot3) +#include // PriorFactor == Initial Pose #include // BetweenFactor == Odom measurement #include // The factor graph we are creating. Nonlinear since angle measurements are nonlinear. #include -// May want to choose a different optimizer later, but for now we use this one. -#include #include // Helps initialize initial guess #include @@ -22,6 +20,7 @@ #include #include #include +#include class Slam { public: @@ -45,20 +44,19 @@ class Slam { // Defining some types typedef gtsam::noiseModel::Diagonal noiseDiagonal; + typedef gtsam::Vector3 Vec3; // Establishing global variables - gtsam::Values currEstimate; + gtsam::Values initEstimate, result; gtsam::NonlinearFactorGraph graph; - gtsam::Pose2 previousPose; + gtsam::Pose3 previousPose; unsigned long pose_index_; - int imu_bias_counter_; - int BIAS_UPDATE_RATE; double BIAS_NOISE_CONST; gtsam::ISAM2 isam; const double KGRAVITY = 9.81; gtsam::PreintegratedImuMeasurements accum; ros::Time lastImuMeasurement; - bool imu_recieved_; + bool imu_received_; }; #endif //SRC_SLAM_H diff --git a/igvc_perception/include/slam/type_conversions.h b/igvc_perception/include/slam/type_conversions.h new file mode 100644 index 000000000..09ae795fe --- /dev/null +++ b/igvc_perception/include/slam/type_conversions.h @@ -0,0 +1,16 @@ +#ifndef SRC_TYPE_CONVERSIONS_H +#define SRC_TYPE_CONVERSIONS_H + +#include +#include +// Pose2 == (Point3, Rot3) +#include +#include + +class Conversion { +public: + static gtsam::Pose3 getPose3FromOdom(const nav_msgs::Odometry &msg); + static nav_msgs::Odometry getOdomFromPose3(const gtsam::Pose3 &pos); +}; + +#endif //SRC_TYPE_CONVERSIONS_H diff --git a/igvc_perception/launch/slam.launch b/igvc_perception/launch/slam.launch index 2b3687e01..4be7d00ab 100644 --- a/igvc_perception/launch/slam.launch +++ b/igvc_perception/launch/slam.launch @@ -1,8 +1,7 @@ - - + \ No newline at end of file diff --git a/igvc_perception/src/slam/CMakeLists.txt b/igvc_perception/src/slam/CMakeLists.txt index 3dcbc16a1..00e64fbe0 100644 --- a/igvc_perception/src/slam/CMakeLists.txt +++ b/igvc_perception/src/slam/CMakeLists.txt @@ -13,12 +13,13 @@ if (GTSAMCMakeTools_FOUND) find_package(Eigen3 3.3 REQUIRED) add_executable(slam main.cpp - slam.cpp) + slam.cpp + type_conversions.cpp) target_include_directories(slam PRIVATE ${catkin_INCLUDE_DIRS}) target_link_libraries(slam gtsam ${catkin_LIBRARIES} Eigen3::Eigen) add_dependencies(slam ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) else() - message(WARNING "Could not find gtsam, not compiling slam!") + message(WARNING "Could not find gtsam. Not compiling slam!") endif() else() message(WARNING "Could not find GTSAMCMakeTools. Not compiling SLAM") diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index ecbd7028b..cd8da0a79 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -5,30 +5,28 @@ using gtsam::symbol_shorthand::V; using gtsam::symbol_shorthand::X; using gtsam::symbol_shorthand::B; +//Initializes the factor graph and the node Slam::Slam() : pnh_{ "~" } { odom_sub_ = pnh_.subscribe("/wheel_odometry", 1, &Slam::OdomCallback, this); imu_sub_ = pnh_.subscribe("/imu", 1, &Slam::ImuCallback, this); location_pub = pnh_.advertise("/slam/position", 1); pose_index_ = 0; - imu_bias_counter_ = 0; - imu_recieved_ = false; - BIAS_UPDATE_RATE = pnh_.param("biasUpdateRatio", 20); + imu_received_ = false; BIAS_NOISE_CONST = pnh_.param("biasNoiseConst", 0.001); InitializeImuParams(); InitializePriors(); } +//The ImuCallback adds IMU measurements to the accumulator when it recieves an IMU measurement void Slam::ImuCallback(const sensor_msgs::Imu &msg){ ROS_INFO_STREAM("Imu called!"); ros::Time currTime = ros::Time::now(); - gtsam::Vector3 measuredAcc = gtsam::Vector3(msg.linear_acceleration.x, msg.linear_acceleration.y, - msg.linear_acceleration.z); - gtsam::Vector3 measuredOmega = gtsam::Vector3(msg.angular_velocity.x, msg.angular_velocity.y, - msg.angular_velocity.z); - if (!imu_recieved_){ - imu_recieved_ = true; + Vec3 measuredAcc = Vec3(msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z); + Vec3 measuredOmega = Vec3(msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z); + if (!imu_received_){ + imu_received_ = true; accum.integrateMeasurement(measuredAcc, measuredOmega, 0.005); } else { double deltaT = (currTime-lastImuMeasurement).toSec(); @@ -37,34 +35,33 @@ void Slam::ImuCallback(const sensor_msgs::Imu &msg){ lastImuMeasurement = currTime; } +//The OdomCallback adds an Odometry measurement and adds an integrated IMU factor to the factor graph void Slam::OdomCallback(const nav_msgs::Odometry &msg){ - if (imu_recieved_) { + if (imu_received_) { // Handle the odometry - gtsam::Pose2 currPose = gtsam::Pose2(msg.pose.pose.position.x, msg.pose.pose.position.y, - tf::getYaw(msg.pose.pose.orientation)); - gtsam::Pose2 odometry = previousPose.between(currPose); - noiseDiagonal::shared_ptr odometryNoise = noiseDiagonal::Variances(gtsam::Vector3( - 0.02, 0.02, 0.01)); - graph.emplace_shared>(pose_index_, pose_index_ + 1, odometry, odometryNoise); - auto newPoseEstimate = currEstimate.at(pose_index_); //gtsam::Pose2 newPoseEstimate + gtsam::Pose3 currPose = Conversion::getPose3FromOdom(msg); + gtsam::Pose3 odometry = previousPose.between(currPose); + noiseDiagonal::shared_ptr odometryNoise = noiseDiagonal::Sigmas((gtsam::Vector(6) + << Vec3::Constant(0.1), Vec3::Constant(0.3)).finished()); + graph.emplace_shared>(X(pose_index_), X(pose_index_ + 1), odometry, odometryNoise); + auto newPoseEstimate = result.at(X(pose_index_)); //gtsam::Pose3 newPoseEstimate newPoseEstimate = odometry * newPoseEstimate; - currEstimate.insert(pose_index_+1, newPoseEstimate); + initEstimate.insert(X(pose_index_ + 1), newPoseEstimate); // Add bias factor auto cov = noiseDiagonal::Variances(gtsam::Vector6::Constant(BIAS_NOISE_CONST)); auto factor = boost::make_shared >(B(pose_index_), B(pose_index_+1), gtsam::imuBias::ConstantBias(), cov); graph.add(factor); - currEstimate.insert(B(pose_index_+1), gtsam::imuBias::ConstantBias()); + initEstimate.insert(B(pose_index_ + 1), gtsam::imuBias::ConstantBias()); // Add imu factor gtsam::ImuFactor imufac(X(pose_index_), V(pose_index_), X(pose_index_+1), V(pose_index_+1), B(pose_index_+1), accum); graph.add(imufac); - auto lastVel = currEstimate.at(V(pose_index_)); - lastVel.x() += accum.deltaVij().x(); - lastVel.y() += accum.deltaVij().y(); - currEstimate.insert(V(pose_index_+1), lastVel); + Vec3 lastVel = result.at(V(pose_index_)); + lastVel += accum.deltaVij(); + initEstimate.insert(V(pose_index_ + 1), lastVel); accum.resetIntegration(); pose_index_++; @@ -73,50 +70,52 @@ void Slam::OdomCallback(const nav_msgs::Odometry &msg){ } } +//Triggers ISAM2 to optimize the current graph and publish the current estimated pose void Slam::Optimize() { - gtsam::LevenbergMarquardtOptimizer optimizer(graph, currEstimate); - currEstimate = optimizer.optimize(); - auto currPose = currEstimate.at(pose_index_); //gtsam::Pose2 currPose - nav_msgs::Odometry msg; - msg.child_frame_id = "/base_footprint"; - msg.header.frame_id = "/odom"; - msg.pose.pose.position.x = currPose.x(); - msg.pose.pose.position.y = currPose.y(); - msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(currPose.theta()); - location_pub.publish(msg); + isam.update(graph, initEstimate); + result = isam.calculateEstimate(); + result.print(); + graph.resize(0); + initEstimate.clear(); + + auto currPose = result.at(X(pose_index_)); //gtsam::Pose2 currPose + location_pub.publish(Conversion::getOdomFromPose3(currPose)); } +//Sets the IMU Params void Slam::InitializeImuParams() { - // Should be replaced with actual imu measurements - boost::shared_ptr params; - params = gtsam::PreintegrationParams::MakeSharedU(KGRAVITY); + // Should be replaced with actual imu measurements. Values should come from the launch file. + auto params = gtsam::PreintegrationParams::MakeSharedU(KGRAVITY); params->setAccelerometerCovariance(gtsam::I_3x3 * 0.1); params->setGyroscopeCovariance(gtsam::I_3x3 * 0.1); params->setIntegrationCovariance(gtsam::I_3x3 * 0.1); params->setUse2ndOrderCoriolis(false); - params->setOmegaCoriolis(gtsam::Vector3(0, 0, 0)); + params->setOmegaCoriolis(Vec3(0, 0, 0)); accum = gtsam::PreintegratedImuMeasurements(params); } +//Adds initial values of variables in the factor graph. void Slam::InitializePriors(){ // Adding Initial Position (Pose + Covariance Matrix) - gtsam::Pose2 priorPose(0.0, 0.0, 0.0); - noiseDiagonal::shared_ptr poseNoise = noiseDiagonal::Sigmas(gtsam::Vector3(0.3, 0.3, 0.1)); - graph.push_back(gtsam::PriorFactor(X(0), priorPose, poseNoise)); - currEstimate.insert(X(0), priorPose); + gtsam::Pose3 priorPose; + noiseDiagonal::shared_ptr poseNoise = noiseDiagonal::Sigmas((gtsam::Vector(6) + << Vec3::Constant(0.1), Vec3::Constant(0.3)).finished()); + graph.push_back(gtsam::PriorFactor(X(0), priorPose, poseNoise)); + initEstimate.insert(X(0), priorPose); // Adding Initial Velocity (Pose + Covariance Matrix) - gtsam::Vector2 priorVel(0.0, 0.0); - noiseDiagonal::shared_ptr velNoise = noiseDiagonal::Sigmas(gtsam::Vector2(0.1, 0.1)); - graph.push_back(gtsam::PriorFactor(V(0), priorVel, velNoise)); - currEstimate.insert(V(0), priorVel); + Vec3 priorVel(0.0, 0.0, 0.0); + noiseDiagonal::shared_ptr velNoise = noiseDiagonal::Sigmas(Vec3::Constant(0.1)); + graph.push_back(gtsam::PriorFactor(V(0), priorVel, velNoise)); + initEstimate.insert(V(0), priorVel); // Adding Bias Prior noiseDiagonal::shared_ptr biasNoise = noiseDiagonal::Sigmas(gtsam::Vector6::Constant(0.1)); gtsam::PriorFactor biasprior(B(0), gtsam::imuBias::ConstantBias(), biasNoise); graph.push_back(biasprior); - currEstimate.insert(B(0), gtsam::imuBias::ConstantBias()); + initEstimate.insert(B(0), gtsam::imuBias::ConstantBias()); - ROS_INFO_STREAM("Values initialized!"); + Optimize(); + ROS_INFO_STREAM("Priors Initialized."); } \ No newline at end of file diff --git a/igvc_perception/src/slam/type_conversions.cpp b/igvc_perception/src/slam/type_conversions.cpp new file mode 100644 index 000000000..5efd7966d --- /dev/null +++ b/igvc_perception/src/slam/type_conversions.cpp @@ -0,0 +1,27 @@ +#include + +gtsam::Pose3 Conversion::getPose3FromOdom(const nav_msgs::Odometry &msg){ + return gtsam::Pose3( + gtsam::Rot3( + msg.pose.pose.orientation.w, + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z), + gtsam::Point3( + msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z) + ); +} + +nav_msgs::Odometry Conversion::getOdomFromPose3(const gtsam::Pose3 &pos){ + nav_msgs::Odometry msg; + msg.child_frame_id = "/base_footprint"; + msg.header.frame_id = "/odom"; + msg.pose.pose.position.x = pos.x(); + msg.pose.pose.position.y = pos.y(); + msg.pose.pose.position.z = pos.z(); + gtsam::Vector3 r = pos.rotation().xyz(); + msg.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(r.x(), r.y(), r.z()); + return msg; +} \ No newline at end of file From 088d2bd6256cb6d973e0c8a1e1e4df5926ccd551 Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Mon, 16 Dec 2019 22:02:49 -0500 Subject: [PATCH 04/55] Fixed missing boost library issue --- igvc_perception/src/slam/CMakeLists.txt | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/igvc_perception/src/slam/CMakeLists.txt b/igvc_perception/src/slam/CMakeLists.txt index 00e64fbe0..4cfb7844c 100644 --- a/igvc_perception/src/slam/CMakeLists.txt +++ b/igvc_perception/src/slam/CMakeLists.txt @@ -7,16 +7,18 @@ if (GTSAMCMakeTools_FOUND) # Find GTSAM components find_package(GTSAM REQUIRED) # Uses installed package - include_directories(${GTSAM_INCLUDE_DIR}) if(GTSAM_FOUND) + include_directories(${GTSAM_INCLUDE_DIR}) message(STATUS "Found gtsam, compiling slam") find_package(Eigen3 3.3 REQUIRED) + find_package(Boost 1.45.0 REQUIRED) + include_directories(${Boost_INCLUDE_DIRS}) add_executable(slam main.cpp slam.cpp type_conversions.cpp) target_include_directories(slam PRIVATE ${catkin_INCLUDE_DIRS}) - target_link_libraries(slam gtsam ${catkin_LIBRARIES} Eigen3::Eigen) + target_link_libraries(slam gtsam ${catkin_LIBRARIES} Eigen3::Eigen ${Boost_LIBRARIES}) add_dependencies(slam ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) else() message(WARNING "Could not find gtsam. Not compiling slam!") From 41c4ad50e1b8ba32fd0cbe7ec53475e4bf568baa Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Mon, 30 Dec 2019 01:57:01 -0500 Subject: [PATCH 05/55] Finally got it to compile again --- igvc_perception/src/slam/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/igvc_perception/src/slam/CMakeLists.txt b/igvc_perception/src/slam/CMakeLists.txt index 4cfb7844c..6e62c9054 100644 --- a/igvc_perception/src/slam/CMakeLists.txt +++ b/igvc_perception/src/slam/CMakeLists.txt @@ -11,8 +11,8 @@ if (GTSAMCMakeTools_FOUND) include_directories(${GTSAM_INCLUDE_DIR}) message(STATUS "Found gtsam, compiling slam") find_package(Eigen3 3.3 REQUIRED) - find_package(Boost 1.45.0 REQUIRED) - include_directories(${Boost_INCLUDE_DIRS}) + find_package(Boost REQUIRED) + include_directories(${Boost_INCLUDE_DIR}) add_executable(slam main.cpp slam.cpp From c7d5d64551a86c7e1eab847293b94d405877f9fa Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Sun, 12 Jan 2020 23:59:41 -0500 Subject: [PATCH 06/55] Saving changes before reinstalling ubuntu --- igvc_perception/src/slam/slam.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index cd8da0a79..997e7bd04 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -30,7 +30,8 @@ void Slam::ImuCallback(const sensor_msgs::Imu &msg){ accum.integrateMeasurement(measuredAcc, measuredOmega, 0.005); } else { double deltaT = (currTime-lastImuMeasurement).toSec(); - accum.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + if (deltaT > 0) + accum.integrateMeasurement(measuredAcc, measuredOmega, deltaT); } lastImuMeasurement = currTime; } From a7265703fe458d7575fda8e573384a796e8f1c85 Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Sun, 19 Jan 2020 18:58:29 -0500 Subject: [PATCH 07/55] changed CMakeList.txt to closer reflect Jason's file --- external/rj-ros-common | 2 +- igvc_perception/src/slam/CMakeLists.txt | 49 ++++++++++++------------- 2 files changed, 25 insertions(+), 26 deletions(-) diff --git a/external/rj-ros-common b/external/rj-ros-common index 48e661eee..4578f74eb 160000 --- a/external/rj-ros-common +++ b/external/rj-ros-common @@ -1 +1 @@ -Subproject commit 48e661eee71057ca684c5a8ba62a1811a9285b23 +Subproject commit 4578f74ebf294a470952ce190e3299176f6bdd86 diff --git a/igvc_perception/src/slam/CMakeLists.txt b/igvc_perception/src/slam/CMakeLists.txt index 6e62c9054..3f1b169dd 100644 --- a/igvc_perception/src/slam/CMakeLists.txt +++ b/igvc_perception/src/slam/CMakeLists.txt @@ -1,28 +1,27 @@ -# Include GTSAM CMake tools -find_package(GTSAMCMakeTools) +# Find and add Boost +find_package(Boost REQUIRED) +find_package(Boost COMPONENTS system filesystem timer REQUIRED) +include_directories(${Boost_INCLUDE_DIR}) +list(APPEND NEEDED_LIBRARIES ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY}) -if (GTSAMCMakeTools_FOUND) - include(GtsamBuildTypes) # Load build type flags and default to Debug mode - include(GtsamTesting) # Easy functions for creating unit tests and scripts +# Find Eigen +find_package(Eigen3 REQUIRED) +include_directories(${EIGEN3_INCLUDE_DIR}) - # Find GTSAM components - find_package(GTSAM REQUIRED) # Uses installed package - if(GTSAM_FOUND) - include_directories(${GTSAM_INCLUDE_DIR}) - message(STATUS "Found gtsam, compiling slam") - find_package(Eigen3 3.3 REQUIRED) - find_package(Boost REQUIRED) - include_directories(${Boost_INCLUDE_DIR}) - add_executable(slam - main.cpp - slam.cpp - type_conversions.cpp) - target_include_directories(slam PRIVATE ${catkin_INCLUDE_DIRS}) - target_link_libraries(slam gtsam ${catkin_LIBRARIES} Eigen3::Eigen ${Boost_LIBRARIES}) - add_dependencies(slam ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - else() - message(WARNING "Could not find gtsam. Not compiling slam!") - endif() +# Find GTSAM components +find_package(GTSAM REQUIRED) # Uses installed package +if(GTSAM_FOUND) + get_target_property(GTSAM_INCLUDE_DIR gtsam INTERFACE_INCLUDE_DIRECTORIES) + set(GTSAM_LIBRARIES gtsam) + include_directories(${GTSAM_INCLUDE_DIR}) + message(STATUS "Found gtsam, compiling slam") + add_executable(slam + main.cpp + slam.cpp + type_conversions.cpp) + target_include_directories(slam PRIVATE ${catkin_INCLUDE_DIRS}) + target_link_libraries(slam gtsam ${catkin_LIBRARIES} Eigen3::Eigen ${Boost_LIBRARIES}) + add_dependencies(slam ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) else() - message(WARNING "Could not find GTSAMCMakeTools. Not compiling SLAM") -endif () \ No newline at end of file + message(WARNING "Could not find gtsam. Not compiling slam!") +endif() From bb3ca9767064e84a42f2c2a4bfa1dbbba294d924 Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Sun, 23 Feb 2020 16:54:21 -0500 Subject: [PATCH 08/55] working imu and odom factors --- igvc_perception/include/slam/slam.h | 6 +- igvc_perception/include/tests/SlamTests.h | 18 +++++ igvc_perception/launch/slam.launch | 2 +- igvc_perception/launch/slam_simulation.launch | 5 ++ igvc_perception/src/slam/slam.cpp | 75 ++++++++++++------- igvc_perception/src/tests/CMakeLists.txt | 10 +++ igvc_perception/src/tests/SlamTests.cpp | 5 ++ igvc_perception/src/tests/main.cpp | 8 ++ 8 files changed, 97 insertions(+), 32 deletions(-) create mode 100644 igvc_perception/include/tests/SlamTests.h create mode 100644 igvc_perception/launch/slam_simulation.launch create mode 100644 igvc_perception/src/tests/SlamTests.cpp create mode 100644 igvc_perception/src/tests/main.cpp diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index 7251f9bd3..e2ef8ad2f 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -50,13 +50,15 @@ class Slam { gtsam::Values initEstimate, result; gtsam::NonlinearFactorGraph graph; gtsam::Pose3 previousPose; - unsigned long pose_index_; + unsigned long curr_index_; + unsigned long last_imu_index_; double BIAS_NOISE_CONST; gtsam::ISAM2 isam; const double KGRAVITY = 9.81; gtsam::PreintegratedImuMeasurements accum; ros::Time lastImuMeasurement; - bool imu_received_; + bool imu_connected_; + bool imu_update_available_; }; #endif //SRC_SLAM_H diff --git a/igvc_perception/include/tests/SlamTests.h b/igvc_perception/include/tests/SlamTests.h new file mode 100644 index 000000000..02a6d3798 --- /dev/null +++ b/igvc_perception/include/tests/SlamTests.h @@ -0,0 +1,18 @@ +#ifndef SRC_SLAMTESTS_H +#define SRC_SLAMTESTS_H + +#include +#include +#include + +class SlamTests { +public: + SlamTests(); + +private: + ros::NodeHandle pnh_; + ros::Subscriber slam_sub_; + ros::Subscriber ground_truth_sub_; +}; + +#endif //SRC_SLAMTESTS_H diff --git a/igvc_perception/launch/slam.launch b/igvc_perception/launch/slam.launch index 4be7d00ab..5622dcaaf 100644 --- a/igvc_perception/launch/slam.launch +++ b/igvc_perception/launch/slam.launch @@ -1,7 +1,7 @@ - + \ No newline at end of file diff --git a/igvc_perception/launch/slam_simulation.launch b/igvc_perception/launch/slam_simulation.launch new file mode 100644 index 000000000..7718b15a3 --- /dev/null +++ b/igvc_perception/launch/slam_simulation.launch @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 997e7bd04..a68581cd4 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -11,8 +11,10 @@ Slam::Slam() : pnh_{ "~" } { imu_sub_ = pnh_.subscribe("/imu", 1, &Slam::ImuCallback, this); location_pub = pnh_.advertise("/slam/position", 1); - pose_index_ = 0; - imu_received_ = false; + curr_index_ = 0; + last_imu_index_ = 0; + imu_connected_ = false; + imu_update_available_ = false; BIAS_NOISE_CONST = pnh_.param("biasNoiseConst", 0.001); InitializeImuParams(); @@ -21,65 +23,80 @@ Slam::Slam() : pnh_{ "~" } { //The ImuCallback adds IMU measurements to the accumulator when it recieves an IMU measurement void Slam::ImuCallback(const sensor_msgs::Imu &msg){ - ROS_INFO_STREAM("Imu called!"); + //ROS_INFO_STREAM("Imu called!"); ros::Time currTime = ros::Time::now(); Vec3 measuredAcc = Vec3(msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z); Vec3 measuredOmega = Vec3(msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z); - if (!imu_received_){ - imu_received_ = true; + if (!imu_connected_){ + imu_connected_ = true; accum.integrateMeasurement(measuredAcc, measuredOmega, 0.005); } else { double deltaT = (currTime-lastImuMeasurement).toSec(); if (deltaT > 0) accum.integrateMeasurement(measuredAcc, measuredOmega, deltaT); } + imu_update_available_ = true; lastImuMeasurement = currTime; } //The OdomCallback adds an Odometry measurement and adds an integrated IMU factor to the factor graph void Slam::OdomCallback(const nav_msgs::Odometry &msg){ - if (imu_received_) { + if (imu_connected_) { // Handle the odometry gtsam::Pose3 currPose = Conversion::getPose3FromOdom(msg); gtsam::Pose3 odometry = previousPose.between(currPose); noiseDiagonal::shared_ptr odometryNoise = noiseDiagonal::Sigmas((gtsam::Vector(6) << Vec3::Constant(0.1), Vec3::Constant(0.3)).finished()); - graph.emplace_shared>(X(pose_index_), X(pose_index_ + 1), odometry, odometryNoise); - auto newPoseEstimate = result.at(X(pose_index_)); //gtsam::Pose3 newPoseEstimate + graph.emplace_shared>(X(curr_index_), X(curr_index_ + 1), odometry, odometryNoise); + auto newPoseEstimate = result.at(X(curr_index_)); //gtsam::Pose3 newPoseEstimate newPoseEstimate = odometry * newPoseEstimate; - initEstimate.insert(X(pose_index_ + 1), newPoseEstimate); - - // Add bias factor - auto cov = noiseDiagonal::Variances(gtsam::Vector6::Constant(BIAS_NOISE_CONST)); - auto factor = boost::make_shared >(B(pose_index_), - B(pose_index_+1), gtsam::imuBias::ConstantBias(), cov); - graph.add(factor); - initEstimate.insert(B(pose_index_ + 1), gtsam::imuBias::ConstantBias()); - - // Add imu factor - gtsam::ImuFactor imufac(X(pose_index_), V(pose_index_), X(pose_index_+1), - V(pose_index_+1), B(pose_index_+1), accum); - graph.add(imufac); - Vec3 lastVel = result.at(V(pose_index_)); - lastVel += accum.deltaVij(); - initEstimate.insert(V(pose_index_ + 1), lastVel); - accum.resetIntegration(); - - pose_index_++; + initEstimate.insert(X(curr_index_ + 1), newPoseEstimate); + + if (imu_update_available_) { + // Add bias factor + auto cov = noiseDiagonal::Variances(gtsam::Vector6::Constant(BIAS_NOISE_CONST)); + auto factor = boost::make_shared >(B(last_imu_index_), + B(curr_index_ + 1), + gtsam::imuBias::ConstantBias(), + cov); + graph.add(factor); + initEstimate.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); + + // Add imu factor + gtsam::ImuFactor imufac(X(last_imu_index_), V(last_imu_index_), X(curr_index_ + 1), + V(curr_index_ + 1), B(curr_index_ + 1), accum); + graph.add(imufac); + Vec3 lastVel = result.at(V(last_imu_index_)); + lastVel += accum.deltaVij(); + last_imu_index_ = curr_index_ + 1; + initEstimate.insert(V(last_imu_index_), lastVel); + accum.resetIntegration(); + } + + curr_index_++; previousPose = currPose; Optimize(); + imu_update_available_ = false; } } //Triggers ISAM2 to optimize the current graph and publish the current estimated pose void Slam::Optimize() { + static int iteration = 0; + if(last_imu_index_ == curr_index_){ + ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ + << " curr_index: " << curr_index_ << " last_imu_index: " << last_imu_index_); + } else { + ROS_WARN_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ + << " curr_index: " << curr_index_ << " last_imu_index: " << last_imu_index_); + } +// graph.print(); isam.update(graph, initEstimate); result = isam.calculateEstimate(); - result.print(); graph.resize(0); initEstimate.clear(); - auto currPose = result.at(X(pose_index_)); //gtsam::Pose2 currPose + auto currPose = result.at(X(curr_index_)); //gtsam::Pose2 currPose location_pub.publish(Conversion::getOdomFromPose3(currPose)); } diff --git a/igvc_perception/src/tests/CMakeLists.txt b/igvc_perception/src/tests/CMakeLists.txt index e69de29bb..27949317f 100644 --- a/igvc_perception/src/tests/CMakeLists.txt +++ b/igvc_perception/src/tests/CMakeLists.txt @@ -0,0 +1,10 @@ +add_executable(slam_tests main.cpp SlamTests.cpp) +add_dependencies(slam_tests ${catkin_EXPORTED_TARGETS}) +target_link_libraries(slam_tests ${catkin_LIBRARIES}) + +install( + TARGETS slam_tests + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) \ No newline at end of file diff --git a/igvc_perception/src/tests/SlamTests.cpp b/igvc_perception/src/tests/SlamTests.cpp new file mode 100644 index 000000000..fdb09d58f --- /dev/null +++ b/igvc_perception/src/tests/SlamTests.cpp @@ -0,0 +1,5 @@ +#include "tests/SlamTests.h" + +SlamTests::SlamTests() { + +} diff --git a/igvc_perception/src/tests/main.cpp b/igvc_perception/src/tests/main.cpp new file mode 100644 index 000000000..05f4ca3a2 --- /dev/null +++ b/igvc_perception/src/tests/main.cpp @@ -0,0 +1,8 @@ +#include +#include + +int main(int argc, char** argv) { + ros::init(argc, argv, "slam_test"); + SlamTests testing_node; + ros::spin(); +} \ No newline at end of file From 68caa59ec0bb2422227886f3b7e6cf5e53e5155d Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Wed, 26 Feb 2020 19:15:13 -0500 Subject: [PATCH 09/55] added GPS factors --- igvc_perception/include/slam/slam.h | 14 ++-- .../include/slam/type_conversions.h | 1 + igvc_perception/launch/slam.launch | 4 +- igvc_perception/src/slam/slam.cpp | 66 +++++++++++++++---- igvc_perception/src/slam/type_conversions.cpp | 8 +++ 5 files changed, 69 insertions(+), 24 deletions(-) diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index e2ef8ad2f..8edea0464 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -31,16 +32,17 @@ class Slam { ros::Subscriber odom_sub_; ros::Subscriber imu_sub_; - //ros::Subscriber gps_sub_; + ros::Subscriber gps_sub_; ros::Publisher location_pub; - + void GpsCallback(const nav_msgs::Odometry &msg); void ImuCallback(const sensor_msgs::Imu &msg); void OdomCallback(const nav_msgs::Odometry &msg); void Optimize(); void InitializeImuParams(); void InitializePriors(); + void InitializeNoiseMatrices(); // Defining some types typedef gtsam::noiseModel::Diagonal noiseDiagonal; @@ -50,15 +52,13 @@ class Slam { gtsam::Values initEstimate, result; gtsam::NonlinearFactorGraph graph; gtsam::Pose3 previousPose; - unsigned long curr_index_; - unsigned long last_imu_index_; - double BIAS_NOISE_CONST; + unsigned long curr_index_, last_imu_index_; + noiseDiagonal::shared_ptr odometry_noise_, gps_noise_, bias_noise_; gtsam::ISAM2 isam; const double KGRAVITY = 9.81; gtsam::PreintegratedImuMeasurements accum; ros::Time lastImuMeasurement; - bool imu_connected_; - bool imu_update_available_; + bool imu_connected_, imu_update_available_; }; #endif //SRC_SLAM_H diff --git a/igvc_perception/include/slam/type_conversions.h b/igvc_perception/include/slam/type_conversions.h index 09ae795fe..00295ffa1 100644 --- a/igvc_perception/include/slam/type_conversions.h +++ b/igvc_perception/include/slam/type_conversions.h @@ -10,6 +10,7 @@ class Conversion { public: static gtsam::Pose3 getPose3FromOdom(const nav_msgs::Odometry &msg); + static gtsam::Point3 getPoint3FromOdom(const nav_msgs::Odometry &msg); static nav_msgs::Odometry getOdomFromPose3(const gtsam::Pose3 &pos); }; diff --git a/igvc_perception/launch/slam.launch b/igvc_perception/launch/slam.launch index 5622dcaaf..641ea6881 100644 --- a/igvc_perception/launch/slam.launch +++ b/igvc_perception/launch/slam.launch @@ -1,7 +1,5 @@ - - - + \ No newline at end of file diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index a68581cd4..db51d8e4f 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -5,23 +5,39 @@ using gtsam::symbol_shorthand::V; using gtsam::symbol_shorthand::X; using gtsam::symbol_shorthand::B; -//Initializes the factor graph and the node +/** + * Initializes the factor graph and all the subscribers and publishers + */ Slam::Slam() : pnh_{ "~" } { - odom_sub_ = pnh_.subscribe("/wheel_odometry", 1, &Slam::OdomCallback, this); - imu_sub_ = pnh_.subscribe("/imu", 1, &Slam::ImuCallback, this); + odom_sub_ = pnh_.subscribe("/wheel_odometry", 10, &Slam::OdomCallback, this); + imu_sub_ = pnh_.subscribe("/imu", 100, &Slam::ImuCallback, this); + gps_sub_ = pnh_.subscribe("/odometry/gps", 1, &Slam::GpsCallback, this); location_pub = pnh_.advertise("/slam/position", 1); curr_index_ = 0; last_imu_index_ = 0; imu_connected_ = false; imu_update_available_ = false; - BIAS_NOISE_CONST = pnh_.param("biasNoiseConst", 0.001); + InitializeNoiseMatrices(); InitializeImuParams(); InitializePriors(); } -//The ImuCallback adds IMU measurements to the accumulator when it recieves an IMU measurement +/** + * The GpsCallback adds GPS measurements to the factor graph + * @param msg An odometery message derived from GPS measurements (published by navsat_transform_node) + */ +void Slam::GpsCallback(const nav_msgs::Odometry &msg){ + gtsam::Point3 currPoint = Conversion::getPoint3FromOdom(msg); + gtsam::GPSFactor gpsFactor(X(curr_index_), currPoint, gps_noise_); + graph.add(gpsFactor); +} + +/** + * The ImuCallback adds IMU measurements to the accumulator when it receives an IMU measurement + * @param msg An imu sensor message (published by yostlab_driver_node) + */ void Slam::ImuCallback(const sensor_msgs::Imu &msg){ //ROS_INFO_STREAM("Imu called!"); ros::Time currTime = ros::Time::now(); @@ -39,26 +55,27 @@ void Slam::ImuCallback(const sensor_msgs::Imu &msg){ lastImuMeasurement = currTime; } -//The OdomCallback adds an Odometry measurement and adds an integrated IMU factor to the factor graph +/** + * The OdomCallback adds an Odometry measurement and adds an integrated IMU factor to the factor graph + * @param msg An odom message published by motor_controller_node + */ void Slam::OdomCallback(const nav_msgs::Odometry &msg){ if (imu_connected_) { // Handle the odometry gtsam::Pose3 currPose = Conversion::getPose3FromOdom(msg); gtsam::Pose3 odometry = previousPose.between(currPose); - noiseDiagonal::shared_ptr odometryNoise = noiseDiagonal::Sigmas((gtsam::Vector(6) - << Vec3::Constant(0.1), Vec3::Constant(0.3)).finished()); - graph.emplace_shared>(X(curr_index_), X(curr_index_ + 1), odometry, odometryNoise); + graph.emplace_shared>(X(curr_index_), X(curr_index_ + 1), odometry, + odometry_noise_); auto newPoseEstimate = result.at(X(curr_index_)); //gtsam::Pose3 newPoseEstimate newPoseEstimate = odometry * newPoseEstimate; initEstimate.insert(X(curr_index_ + 1), newPoseEstimate); if (imu_update_available_) { // Add bias factor - auto cov = noiseDiagonal::Variances(gtsam::Vector6::Constant(BIAS_NOISE_CONST)); auto factor = boost::make_shared >(B(last_imu_index_), B(curr_index_ + 1), gtsam::imuBias::ConstantBias(), - cov); + bias_noise_); graph.add(factor); initEstimate.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); @@ -80,7 +97,9 @@ void Slam::OdomCallback(const nav_msgs::Odometry &msg){ } } -//Triggers ISAM2 to optimize the current graph and publish the current estimated pose +/** + * Triggers ISAM2 to optimize the current graph and publish the current estimated pose + */ void Slam::Optimize() { static int iteration = 0; if(last_imu_index_ == curr_index_){ @@ -100,7 +119,9 @@ void Slam::Optimize() { location_pub.publish(Conversion::getOdomFromPose3(currPose)); } -//Sets the IMU Params +/** + * Sets the IMU Params + */ void Slam::InitializeImuParams() { // Should be replaced with actual imu measurements. Values should come from the launch file. auto params = gtsam::PreintegrationParams::MakeSharedU(KGRAVITY); @@ -112,7 +133,9 @@ void Slam::InitializeImuParams() { accum = gtsam::PreintegratedImuMeasurements(params); } -//Adds initial values of variables in the factor graph. +/** + * Adds initial values of variables in the factor graph. + */ void Slam::InitializePriors(){ // Adding Initial Position (Pose + Covariance Matrix) gtsam::Pose3 priorPose; @@ -136,4 +159,19 @@ void Slam::InitializePriors(){ Optimize(); ROS_INFO_STREAM("Priors Initialized."); +} + +/** + * Initializes the shared noise matrices from parameters in the launch file + */ +void Slam::InitializeNoiseMatrices(){ + double bias_noise = pnh_.param("biasNoiseConst", 0.03); + double gps_xy_noise = pnh_.param("gpsXYNoiseConstant", 0.15); + double gps_z_noise = pnh_.param("gpsZNoiseConstant", 0.15); + double odom_pos_noise = pnh_.param("odomPosNoiseConstant", 0.1); + double odom_orient_noise = pnh_.param("odomOrientNoiseConstant", 0.3); + gps_noise_ = noiseDiagonal::Sigmas(Vec3(gps_xy_noise, gps_xy_noise, gps_z_noise)); + odometry_noise_ = noiseDiagonal::Sigmas((gtsam::Vector(6) << Vec3::Constant(odom_pos_noise), + Vec3::Constant(odom_orient_noise)).finished()); + bias_noise_ = noiseDiagonal::Sigmas(gtsam::Vector6::Constant(bias_noise)); } \ No newline at end of file diff --git a/igvc_perception/src/slam/type_conversions.cpp b/igvc_perception/src/slam/type_conversions.cpp index 5efd7966d..0fcd3c162 100644 --- a/igvc_perception/src/slam/type_conversions.cpp +++ b/igvc_perception/src/slam/type_conversions.cpp @@ -24,4 +24,12 @@ nav_msgs::Odometry Conversion::getOdomFromPose3(const gtsam::Pose3 &pos){ gtsam::Vector3 r = pos.rotation().xyz(); msg.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(r.x(), r.y(), r.z()); return msg; +} + +gtsam::Point3 Conversion::getPoint3FromOdom(const nav_msgs::Odometry &msg){ + return gtsam::Point3( + msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z + ); } \ No newline at end of file From 588fae31b020129742824d5c5b25302ed57c795d Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Wed, 26 Feb 2020 20:59:45 -0500 Subject: [PATCH 10/55] checked for covariance equaling zero --- igvc_perception/src/slam/slam.cpp | 38 +++++++++++++++++-------------- 1 file changed, 21 insertions(+), 17 deletions(-) diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index db51d8e4f..44a647f09 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -71,22 +71,26 @@ void Slam::OdomCallback(const nav_msgs::Odometry &msg){ initEstimate.insert(X(curr_index_ + 1), newPoseEstimate); if (imu_update_available_) { - // Add bias factor - auto factor = boost::make_shared >(B(last_imu_index_), - B(curr_index_ + 1), - gtsam::imuBias::ConstantBias(), - bias_noise_); - graph.add(factor); - initEstimate.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); - - // Add imu factor - gtsam::ImuFactor imufac(X(last_imu_index_), V(last_imu_index_), X(curr_index_ + 1), - V(curr_index_ + 1), B(curr_index_ + 1), accum); - graph.add(imufac); - Vec3 lastVel = result.at(V(last_imu_index_)); - lastVel += accum.deltaVij(); - last_imu_index_ = curr_index_ + 1; - initEstimate.insert(V(last_imu_index_), lastVel); + if (accum.preintMeasCov().trace() != 0) { + // Add bias factor + auto factor = boost::make_shared >( + B(last_imu_index_), + B(curr_index_ + 1), + gtsam::imuBias::ConstantBias(), + bias_noise_); + graph.add(factor); + initEstimate.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); + + // Add imu factor + gtsam::ImuFactor imufac(X(last_imu_index_), V(last_imu_index_), X(curr_index_ + 1), + V(curr_index_ + 1), B(curr_index_ + 1), accum); + + graph.add(imufac); + Vec3 lastVel = result.at(V(last_imu_index_)); + lastVel += accum.deltaVij(); + last_imu_index_ = curr_index_ + 1; + initEstimate.insert(V(last_imu_index_), lastVel); + } accum.resetIntegration(); } @@ -109,7 +113,7 @@ void Slam::Optimize() { ROS_WARN_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ << " curr_index: " << curr_index_ << " last_imu_index: " << last_imu_index_); } -// graph.print(); + //graph.print(); isam.update(graph, initEstimate); result = isam.calculateEstimate(); graph.resize(0); From 750170dc5b72476de1fa142badc8eedc3efc2c8b Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Sun, 1 Mar 2020 18:19:34 -0500 Subject: [PATCH 11/55] changed the values of wheel spacing --- igvc_navigation/launch/wheel_odometry.launch | 2 +- igvc_navigation/src/wheel_odometer/Odometer.cpp | 2 +- igvc_perception/src/slam/slam.cpp | 1 - 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/igvc_navigation/launch/wheel_odometry.launch b/igvc_navigation/launch/wheel_odometry.launch index 17da9572f..a0263b2b5 100644 --- a/igvc_navigation/launch/wheel_odometry.launch +++ b/igvc_navigation/launch/wheel_odometry.launch @@ -1,6 +1,6 @@ - + diff --git a/igvc_navigation/src/wheel_odometer/Odometer.cpp b/igvc_navigation/src/wheel_odometer/Odometer.cpp index 33726eb23..80114e39f 100644 --- a/igvc_navigation/src/wheel_odometer/Odometer.cpp +++ b/igvc_navigation/src/wheel_odometer/Odometer.cpp @@ -78,7 +78,7 @@ void Odometer::enc_callback(const igvc_msgs::velocity_pair& msg) odom.header.seq = seq++; // setting reference frames - odom.header.frame_id = "wheel_odom"; + odom.header.frame_id = "odom"; odom.child_frame_id = "base_footprint"; // set time then publish diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 44a647f09..5c82fa863 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -39,7 +39,6 @@ void Slam::GpsCallback(const nav_msgs::Odometry &msg){ * @param msg An imu sensor message (published by yostlab_driver_node) */ void Slam::ImuCallback(const sensor_msgs::Imu &msg){ - //ROS_INFO_STREAM("Imu called!"); ros::Time currTime = ros::Time::now(); Vec3 measuredAcc = Vec3(msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z); Vec3 measuredOmega = Vec3(msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z); From 298156d452a815df224a40ed4232045115e108e8 Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Wed, 4 Mar 2020 20:38:20 -0500 Subject: [PATCH 12/55] removed faulty odometry and just using GPS and IMU --- igvc_perception/include/slam/slam.h | 5 +- igvc_perception/src/slam/slam.cpp | 99 ++++++++++++++++------------- 2 files changed, 58 insertions(+), 46 deletions(-) diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index 8edea0464..fe564ab16 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -38,7 +38,8 @@ class Slam { void GpsCallback(const nav_msgs::Odometry &msg); void ImuCallback(const sensor_msgs::Imu &msg); - void OdomCallback(const nav_msgs::Odometry &msg); +// void OdomCallback(const nav_msgs::Odometry &msg); + void IntegrateAndAddIMUFactor(); void Optimize(); void InitializeImuParams(); void InitializePriors(); @@ -52,7 +53,7 @@ class Slam { gtsam::Values initEstimate, result; gtsam::NonlinearFactorGraph graph; gtsam::Pose3 previousPose; - unsigned long curr_index_, last_imu_index_; + unsigned long curr_index_; noiseDiagonal::shared_ptr odometry_noise_, gps_noise_, bias_noise_; gtsam::ISAM2 isam; const double KGRAVITY = 9.81; diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 5c82fa863..ffcaf07e0 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -9,13 +9,12 @@ using gtsam::symbol_shorthand::B; * Initializes the factor graph and all the subscribers and publishers */ Slam::Slam() : pnh_{ "~" } { - odom_sub_ = pnh_.subscribe("/wheel_odometry", 10, &Slam::OdomCallback, this); +// odom_sub_ = pnh_.subscribe("/wheel_odometry", 10, &Slam::OdomCallback, this); imu_sub_ = pnh_.subscribe("/imu", 100, &Slam::ImuCallback, this); gps_sub_ = pnh_.subscribe("/odometry/gps", 1, &Slam::GpsCallback, this); location_pub = pnh_.advertise("/slam/position", 1); curr_index_ = 0; - last_imu_index_ = 0; imu_connected_ = false; imu_update_available_ = false; @@ -32,6 +31,12 @@ void Slam::GpsCallback(const nav_msgs::Odometry &msg){ gtsam::Point3 currPoint = Conversion::getPoint3FromOdom(msg); gtsam::GPSFactor gpsFactor(X(curr_index_), currPoint, gps_noise_); graph.add(gpsFactor); + + if (imu_update_available_) { + IntegrateAndAddIMUFactor(); + } + + imu_update_available_ = false; } /** @@ -58,46 +63,57 @@ void Slam::ImuCallback(const sensor_msgs::Imu &msg){ * The OdomCallback adds an Odometry measurement and adds an integrated IMU factor to the factor graph * @param msg An odom message published by motor_controller_node */ -void Slam::OdomCallback(const nav_msgs::Odometry &msg){ - if (imu_connected_) { - // Handle the odometry - gtsam::Pose3 currPose = Conversion::getPose3FromOdom(msg); - gtsam::Pose3 odometry = previousPose.between(currPose); - graph.emplace_shared>(X(curr_index_), X(curr_index_ + 1), odometry, - odometry_noise_); +//void Slam::OdomCallback(const nav_msgs::Odometry &msg){ +// if (imu_connected_) { +// // Handle the odometry +// gtsam::Pose3 currPose = Conversion::getPose3FromOdom(msg); +// gtsam::Pose3 odometry = previousPose.between(currPose); +// graph.emplace_shared>(X(curr_index_), X(curr_index_ + 1), odometry, +// odometry_noise_); +// auto newPoseEstimate = result.at(X(curr_index_)); //gtsam::Pose3 newPoseEstimate +// newPoseEstimate = odometry * newPoseEstimate; +// initEstimate.insert(X(curr_index_ + 1), newPoseEstimate); +// +// if (imu_update_available_) { +// IntegrateAndAddIMUFactor(); +// } +// +// curr_index_++; +// previousPose = currPose; +// Optimize(); +// imu_update_available_ = false; +// } +//} + +/** + * If there are IMU measurements in the acumulator, this adds them as a single factor to the factor graph. + */ +void Slam::IntegrateAndAddIMUFactor(){ + if (accum.preintMeasCov().trace() != 0) { + // Add bias factor + auto factor = boost::make_shared >( + B(curr_index_), + B(curr_index_ + 1), + gtsam::imuBias::ConstantBias(), + bias_noise_); + graph.add(factor); + initEstimate.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); + + // Add imu factor + gtsam::ImuFactor imufac(X(curr_index_), V(curr_index_), X(curr_index_ + 1), + V(curr_index_ + 1), B(curr_index_ + 1), accum); + + graph.add(imufac); auto newPoseEstimate = result.at(X(curr_index_)); //gtsam::Pose3 newPoseEstimate - newPoseEstimate = odometry * newPoseEstimate; + newPoseEstimate = gtsam::Pose3(accum.deltaRij()*newPoseEstimate.rotation(), accum.deltaPij()+newPoseEstimate.translation()); initEstimate.insert(X(curr_index_ + 1), newPoseEstimate); - - if (imu_update_available_) { - if (accum.preintMeasCov().trace() != 0) { - // Add bias factor - auto factor = boost::make_shared >( - B(last_imu_index_), - B(curr_index_ + 1), - gtsam::imuBias::ConstantBias(), - bias_noise_); - graph.add(factor); - initEstimate.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); - - // Add imu factor - gtsam::ImuFactor imufac(X(last_imu_index_), V(last_imu_index_), X(curr_index_ + 1), - V(curr_index_ + 1), B(curr_index_ + 1), accum); - - graph.add(imufac); - Vec3 lastVel = result.at(V(last_imu_index_)); - lastVel += accum.deltaVij(); - last_imu_index_ = curr_index_ + 1; - initEstimate.insert(V(last_imu_index_), lastVel); - } - accum.resetIntegration(); - } - + Vec3 lastVel = result.at(V(curr_index_)); + lastVel += accum.deltaVij(); + initEstimate.insert(V(curr_index_ + 1), lastVel); curr_index_++; - previousPose = currPose; Optimize(); - imu_update_available_ = false; } + accum.resetIntegration(); } /** @@ -105,13 +121,8 @@ void Slam::OdomCallback(const nav_msgs::Odometry &msg){ */ void Slam::Optimize() { static int iteration = 0; - if(last_imu_index_ == curr_index_){ - ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ - << " curr_index: " << curr_index_ << " last_imu_index: " << last_imu_index_); - } else { - ROS_WARN_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ - << " curr_index: " << curr_index_ << " last_imu_index: " << last_imu_index_); - } + ROS_WARN_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ + << " curr_index: " << curr_index_); //graph.print(); isam.update(graph, initEstimate); result = isam.calculateEstimate(); From 5d635eae1b0e233d786d735dcf89024d4f64e91b Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Wed, 18 Mar 2020 17:51:42 -0400 Subject: [PATCH 13/55] added magnetometer to gazebo simulation --- igvc_description/urdf/jessii.urdf.xacro | 18 ++++++++++++++++-- igvc_platform/src/imu/YostLabDriver.cpp | 2 +- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/igvc_description/urdf/jessii.urdf.xacro b/igvc_description/urdf/jessii.urdf.xacro index 4942613f5..56021ecaf 100644 --- a/igvc_description/urdf/jessii.urdf.xacro +++ b/igvc_description/urdf/jessii.urdf.xacro @@ -590,7 +590,7 @@ imu /imu 0 0 0 - 0.00000001 + 0.00000005 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 @@ -610,7 +610,7 @@ magnetometer /magnetometer 0 0 0 - 0.00000001 + 0.00000005 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 @@ -623,6 +623,20 @@ 1.57079632679 + + 200.0 + .0000489162 + magnetometer + /magnetometer_mag + 0.0 + -5.2290 + 62.2126 + 0 0 0 + 0 0 0 + 0 0 0 + 0.00000005 0.00000005 0.00000005 + + 20.0 base_link diff --git a/igvc_platform/src/imu/YostLabDriver.cpp b/igvc_platform/src/imu/YostLabDriver.cpp index 58b8c4c4a..ddd8406a4 100644 --- a/igvc_platform/src/imu/YostLabDriver.cpp +++ b/igvc_platform/src/imu/YostLabDriver.cpp @@ -298,7 +298,7 @@ void YostLabDriver::createAndPublishIMUMessage(std::vector &parsed_val) sensor_temp_ = parsed_val[13]; imu_pub_.publish(imu_msg); - magnet_pub_.publish(magnet_msg); + magnet_pub_.publish(magnet_msg); // Published in teslas lastUpdateTime_ = ros::Time::now(); last_quat_ = quat; msg_counter_++; From 416af9a794e8cbe48526ea7ad475523bb30a5753 Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Thu, 19 Mar 2020 15:45:32 -0400 Subject: [PATCH 14/55] created a conversion node so that gazebo publishes correct message type --- igvc_description/urdf/jessii.urdf.xacro | 2 +- igvc_gazebo/CMakeLists.txt | 1 + igvc_gazebo/launch/simulation.launch | 5 + igvc_gazebo/launch/simulation_low.launch | 5 + igvc_gazebo/nodes/magnetometer/CMakeLists.txt | 10 ++ igvc_gazebo/nodes/magnetometer/main.cpp | 30 ++++ igvc_perception/include/slam/slam.h | 42 +++--- igvc_perception/launch/slam.launch | 5 +- igvc_perception/src/slam/slam.cpp | 140 ++++++++---------- igvc_platform/src/imu/YostLabDriver.cpp | 2 +- 10 files changed, 147 insertions(+), 95 deletions(-) create mode 100644 igvc_gazebo/nodes/magnetometer/CMakeLists.txt create mode 100644 igvc_gazebo/nodes/magnetometer/main.cpp diff --git a/igvc_description/urdf/jessii.urdf.xacro b/igvc_description/urdf/jessii.urdf.xacro index 56021ecaf..ba082639d 100644 --- a/igvc_description/urdf/jessii.urdf.xacro +++ b/igvc_description/urdf/jessii.urdf.xacro @@ -627,7 +627,7 @@ 200.0 .0000489162 magnetometer - /magnetometer_mag + /magnetometer/vector 0.0 -5.2290 62.2126 diff --git a/igvc_gazebo/CMakeLists.txt b/igvc_gazebo/CMakeLists.txt index 5740618cb..b2993d459 100644 --- a/igvc_gazebo/CMakeLists.txt +++ b/igvc_gazebo/CMakeLists.txt @@ -46,6 +46,7 @@ install(DIRECTORY config/ FILES_MATCHING PATTERN "*.yaml" ) +add_subdirectory(nodes/magnetometer) add_subdirectory(nodes/control) add_subdirectory(nodes/scan_to_pointcloud) add_subdirectory(nodes/ground_truth) diff --git a/igvc_gazebo/launch/simulation.launch b/igvc_gazebo/launch/simulation.launch index dc3649811..0f780ab8f 100644 --- a/igvc_gazebo/launch/simulation.launch +++ b/igvc_gazebo/launch/simulation.launch @@ -53,6 +53,11 @@ + + + + + diff --git a/igvc_gazebo/launch/simulation_low.launch b/igvc_gazebo/launch/simulation_low.launch index dc3649811..61dde2bbc 100644 --- a/igvc_gazebo/launch/simulation_low.launch +++ b/igvc_gazebo/launch/simulation_low.launch @@ -59,6 +59,11 @@ + + + + + diff --git a/igvc_gazebo/nodes/magnetometer/CMakeLists.txt b/igvc_gazebo/nodes/magnetometer/CMakeLists.txt new file mode 100644 index 000000000..790bfd43d --- /dev/null +++ b/igvc_gazebo/nodes/magnetometer/CMakeLists.txt @@ -0,0 +1,10 @@ +add_executable(mag_republisher main.cpp) +add_dependencies(mag_republisher ${catkin_EXPORTED_TARGETS}) +target_link_libraries(mag_republisher ${catkin_LIBRARIES}) + +install( + TARGETS mag_republisher + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) \ No newline at end of file diff --git a/igvc_gazebo/nodes/magnetometer/main.cpp b/igvc_gazebo/nodes/magnetometer/main.cpp new file mode 100644 index 000000000..8675c44ed --- /dev/null +++ b/igvc_gazebo/nodes/magnetometer/main.cpp @@ -0,0 +1,30 @@ +#include +#include +#include + +ros::Publisher mag_field_pub_; + +void magCallback(const geometry_msgs::Vector3Stamped& msg) +{ + sensor_msgs::MagneticField magnet_msg; + magnet_msg.header.seq = msg.header.seq; + magnet_msg.header.stamp = msg.header.stamp; + magnet_msg.header.frame_id = msg.header.frame_id; + magnet_msg.magnetic_field.x = msg.vector.x; + magnet_msg.magnetic_field.y = msg.vector.y; + magnet_msg.magnetic_field.z = msg.vector.z; + magnet_msg.magnetic_field_covariance = { 1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6 }; + mag_field_pub_.publish(magnet_msg); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "mag_republisher"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + std::string sub_topic = pnh.param("mag_sub_topic", std::string("/magnetometer/vector")); + std::string pub_topic = pnh.param("mag_pub_topic", std::string("/magnetometer_mag")); + mag_field_pub_ = nh.advertise(pub_topic, 10); + ros::Subscriber scan_sub = nh.subscribe(sub_topic, 1, magCallback); + ros::spin(); +} \ No newline at end of file diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index 5ae88831f..fd28961be 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -14,14 +14,17 @@ #include // Helps initialize initial guess #include +#include #include #include #include #include +#include #include #include #include #include +#include class Slam { @@ -34,33 +37,38 @@ class Slam ros::Subscriber odom_sub_; ros::Subscriber imu_sub_; ros::Subscriber gps_sub_; + ros::Subscriber mag_sub_; - ros::Publisher location_pub; + ros::Publisher location_pub_; - void GpsCallback(const nav_msgs::Odometry &msg); - void ImuCallback(const sensor_msgs::Imu &msg); - // void OdomCallback(const nav_msgs::Odometry &msg); - void IntegrateAndAddIMUFactor(); - void Optimize(); - void InitializeImuParams(); - void InitializePriors(); - void InitializeNoiseMatrices(); + void gpsCallback(const nav_msgs::Odometry &msg); + void imuCallback(const sensor_msgs::Imu &msg); + void magCallback(const sensor_msgs::MagneticField &msg); + void integrateAndAddIMUFactor(); + void optimize(); + void initializeImuParams(); + void initializePriors(); + void initializeNoiseMatrices(); + void intializeDirectionOfLocalMagField(); // Defining some types typedef gtsam::noiseModel::Diagonal noiseDiagonal; typedef gtsam::Vector3 Vec3; // Establishing global variables - gtsam::Values initEstimate, result; - gtsam::NonlinearFactorGraph graph; - gtsam::Pose3 previousPose; + gtsam::Values initEstimate_, result_; + gtsam::NonlinearFactorGraph graph_; + gtsam::Pose3 previousPose_; unsigned long curr_index_; - noiseDiagonal::shared_ptr odometry_noise_, gps_noise_, bias_noise_; - gtsam::ISAM2 isam; - const double KGRAVITY = 9.81; - gtsam::PreintegratedImuMeasurements accum; - ros::Time lastImuMeasurement; + noiseDiagonal::shared_ptr gps_noise_, bias_noise_, mag_noise_; + gtsam::Unit3 local_mag_field_; + gtsam::Point3 curr_mag_reading_; + gtsam::ISAM2 isam_; + gtsam::PreintegratedImuMeasurements accum_; + ros::Time lastImuMeasurement_; bool imu_connected_, imu_update_available_; + + const double KGRAVITY = 9.81; }; #endif // SRC_SLAM_H diff --git a/igvc_perception/launch/slam.launch b/igvc_perception/launch/slam.launch index 641ea6881..4f83a8f33 100644 --- a/igvc_perception/launch/slam.launch +++ b/igvc_perception/launch/slam.launch @@ -1,5 +1,8 @@ - + + + + \ No newline at end of file diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 8c8cfe1d8..2b2068ad7 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -11,42 +11,44 @@ using gtsam::symbol_shorthand::X; Slam::Slam() : pnh_{ "~" } { // odom_sub_ = pnh_.subscribe("/wheel_odometry", 10, &Slam::OdomCallback, this); - imu_sub_ = pnh_.subscribe("/imu", 100, &Slam::ImuCallback, this); - gps_sub_ = pnh_.subscribe("/odometry/gps", 1, &Slam::GpsCallback, this); - location_pub = pnh_.advertise("/slam/position", 1); + imu_sub_ = pnh_.subscribe("/imu", 100, &Slam::imuCallback, this); + gps_sub_ = pnh_.subscribe("/odometry/gps", 1, &Slam::gpsCallback, this); + mag_sub_ = pnh_.subscribe("/magnetometer_mag", 1, &Slam::magCallback, this); + location_pub_ = pnh_.advertise("/slam/position", 1); curr_index_ = 0; imu_connected_ = false; imu_update_available_ = false; - InitializeNoiseMatrices(); - InitializeImuParams(); - InitializePriors(); + intializeDirectionOfLocalMagField(); + initializeNoiseMatrices(); + initializeImuParams(); + initializePriors(); } /** - * The GpsCallback adds GPS measurements to the factor graph + * The gpsCallback adds GPS measurements to the factor graph * @param msg An odometery message derived from GPS measurements (published by navsat_transform_node) */ -void Slam::GpsCallback(const nav_msgs::Odometry &msg) +void Slam::gpsCallback(const nav_msgs::Odometry &msg) { gtsam::Point3 currPoint = Conversion::getPoint3FromOdom(msg); gtsam::GPSFactor gpsFactor(X(curr_index_), currPoint, gps_noise_); - graph.add(gpsFactor); + graph_.add(gpsFactor); if (imu_update_available_) { - IntegrateAndAddIMUFactor(); + integrateAndAddIMUFactor(); } imu_update_available_ = false; } /** - * The ImuCallback adds IMU measurements to the accumulator when it receives an IMU measurement + * The imuCallback adds IMU measurements to the accumulator when it receives an IMU measurement * @param msg An imu sensor message (published by yostlab_driver_node) */ -void Slam::ImuCallback(const sensor_msgs::Imu &msg) +void Slam::imuCallback(const sensor_msgs::Imu &msg) { ros::Time currTime = ros::Time::now(); Vec3 measuredAcc = Vec3(msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z); @@ -54,97 +56,80 @@ void Slam::ImuCallback(const sensor_msgs::Imu &msg) if (!imu_connected_) { imu_connected_ = true; - accum.integrateMeasurement(measuredAcc, measuredOmega, 0.005); + accum_.integrateMeasurement(measuredAcc, measuredOmega, 0.005); } else { - double deltaT = (currTime - lastImuMeasurement).toSec(); + double deltaT = (currTime - lastImuMeasurement_).toSec(); if (deltaT > 0) - accum.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + accum_.integrateMeasurement(measuredAcc, measuredOmega, deltaT); } imu_update_available_ = true; - lastImuMeasurement = currTime; + lastImuMeasurement_ = currTime; } /** - * The OdomCallback adds an Odometry measurement and adds an integrated IMU factor to the factor graph - * @param msg An odom message published by motor_controller_node + * The magCallback updates the curr_mag_reading_ with its most recent value + * @param msg An MagneticField sensor message (published by yostlab_driver_node) */ -// void Slam::OdomCallback(const nav_msgs::Odometry &msg){ -// if (imu_connected_) { -// // Handle the odometry -// gtsam::Pose3 currPose = Conversion::getPose3FromOdom(msg); -// gtsam::Pose3 odometry = previousPose.between(currPose); -// graph.emplace_shared>(X(curr_index_), X(curr_index_ + 1), odometry, -// odometry_noise_); -// auto newPoseEstimate = result.at(X(curr_index_)); //gtsam::Pose3 newPoseEstimate -// newPoseEstimate = odometry * newPoseEstimate; -// initEstimate.insert(X(curr_index_ + 1), newPoseEstimate); -// -// if (imu_update_available_) { -// IntegrateAndAddIMUFactor(); -// } -// -// curr_index_++; -// previousPose = currPose; -// Optimize(); -// imu_update_available_ = false; -// } -//} +void Slam::magCallback(const sensor_msgs::MagneticField &msg) +{ + curr_mag_reading_ = gtsam::Point3(msg.magnetic_field.x, msg.magnetic_field.y, msg.magnetic_field.z); +} /** * If there are IMU measurements in the acumulator, this adds them as a single factor to the factor graph. */ -void Slam::IntegrateAndAddIMUFactor() +void Slam::integrateAndAddIMUFactor() { - if (accum.preintMeasCov().trace() != 0) + if (accum_.preintMeasCov().trace() != 0) { // Add bias factor auto factor = boost::make_shared >( B(curr_index_), B(curr_index_ + 1), gtsam::imuBias::ConstantBias(), bias_noise_); - graph.add(factor); - initEstimate.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); + graph_.add(factor); + initEstimate_.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); // Add imu factor gtsam::ImuFactor imufac(X(curr_index_), V(curr_index_), X(curr_index_ + 1), V(curr_index_ + 1), B(curr_index_ + 1), - accum); + accum_); - graph.add(imufac); - auto newPoseEstimate = result.at(X(curr_index_)); // gtsam::Pose3 newPoseEstimate + graph_.add(imufac); + auto newPoseEstimate = result_.at(X(curr_index_)); // gtsam::Pose3 newPoseEstimate newPoseEstimate = - gtsam::Pose3(accum.deltaRij() * newPoseEstimate.rotation(), accum.deltaPij() + newPoseEstimate.translation()); - initEstimate.insert(X(curr_index_ + 1), newPoseEstimate); - Vec3 lastVel = result.at(V(curr_index_)); - lastVel += accum.deltaVij(); - initEstimate.insert(V(curr_index_ + 1), lastVel); + gtsam::Pose3(accum_.deltaRij() * newPoseEstimate.rotation(), accum_.deltaPij() + newPoseEstimate.translation()); + initEstimate_.insert(X(curr_index_ + 1), newPoseEstimate); + Vec3 lastVel = result_.at(V(curr_index_)); + lastVel += accum_.deltaVij(); + initEstimate_.insert(V(curr_index_ + 1), lastVel); curr_index_++; - Optimize(); + optimize(); } - accum.resetIntegration(); + accum_.resetIntegration(); } /** * Triggers ISAM2 to optimize the current graph and publish the current estimated pose */ -void Slam::Optimize() +void Slam::optimize() { static int iteration = 0; ROS_WARN_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ << " curr_index: " << curr_index_); // graph.print(); - isam.update(graph, initEstimate); - result = isam.calculateEstimate(); - graph.resize(0); - initEstimate.clear(); + isam_.update(graph_, initEstimate_); + result_ = isam_.calculateEstimate(); + graph_.resize(0); + initEstimate_.clear(); - auto currPose = result.at(X(curr_index_)); // gtsam::Pose2 currPose - location_pub.publish(Conversion::getOdomFromPose3(currPose)); + auto currPose = result_.at(X(curr_index_)); // gtsam::Pose2 currPose + location_pub_.publish(Conversion::getOdomFromPose3(currPose)); } /** * Sets the IMU Params */ -void Slam::InitializeImuParams() +void Slam::initializeImuParams() { // Should be replaced with actual imu measurements. Values should come from the launch file. auto params = gtsam::PreintegrationParams::MakeSharedU(KGRAVITY); @@ -153,49 +138,54 @@ void Slam::InitializeImuParams() params->setIntegrationCovariance(gtsam::I_3x3 * 0.1); params->setUse2ndOrderCoriolis(false); params->setOmegaCoriolis(Vec3(0, 0, 0)); - accum = gtsam::PreintegratedImuMeasurements(params); + accum_ = gtsam::PreintegratedImuMeasurements(params); } /** * Adds initial values of variables in the factor graph. */ -void Slam::InitializePriors() +void Slam::initializePriors() { // Adding Initial Position (Pose + Covariance Matrix) gtsam::Pose3 priorPose; noiseDiagonal::shared_ptr poseNoise = noiseDiagonal::Sigmas((gtsam::Vector(6) << Vec3::Constant(0.1), Vec3::Constant(0.3)).finished()); - graph.push_back(gtsam::PriorFactor(X(0), priorPose, poseNoise)); - initEstimate.insert(X(0), priorPose); + graph_.push_back(gtsam::PriorFactor(X(0), priorPose, poseNoise)); + initEstimate_.insert(X(0), priorPose); // Adding Initial Velocity (Pose + Covariance Matrix) Vec3 priorVel(0.0, 0.0, 0.0); noiseDiagonal::shared_ptr velNoise = noiseDiagonal::Sigmas(Vec3::Constant(0.1)); - graph.push_back(gtsam::PriorFactor(V(0), priorVel, velNoise)); - initEstimate.insert(V(0), priorVel); + graph_.push_back(gtsam::PriorFactor(V(0), priorVel, velNoise)); + initEstimate_.insert(V(0), priorVel); // Adding Bias Prior noiseDiagonal::shared_ptr biasNoise = noiseDiagonal::Sigmas(gtsam::Vector6::Constant(0.1)); gtsam::PriorFactor biasprior(B(0), gtsam::imuBias::ConstantBias(), biasNoise); - graph.push_back(biasprior); - initEstimate.insert(B(0), gtsam::imuBias::ConstantBias()); + graph_.push_back(biasprior); + initEstimate_.insert(B(0), gtsam::imuBias::ConstantBias()); - Optimize(); + optimize(); ROS_INFO_STREAM("Priors Initialized."); } /** * Initializes the shared noise matrices from parameters in the launch file */ -void Slam::InitializeNoiseMatrices() +void Slam::initializeNoiseMatrices() { double bias_noise = pnh_.param("biasNoiseConst", 0.03); double gps_xy_noise = pnh_.param("gpsXYNoiseConstant", 0.15); double gps_z_noise = pnh_.param("gpsZNoiseConstant", 0.15); - double odom_pos_noise = pnh_.param("odomPosNoiseConstant", 0.1); - double odom_orient_noise = pnh_.param("odomOrientNoiseConstant", 0.3); + double mag_noise = pnh_.param("magNoiseConstant", 0.00000005); gps_noise_ = noiseDiagonal::Sigmas(Vec3(gps_xy_noise, gps_xy_noise, gps_z_noise)); - odometry_noise_ = noiseDiagonal::Sigmas( - (gtsam::Vector(6) << Vec3::Constant(odom_pos_noise), Vec3::Constant(odom_orient_noise)).finished()); + mag_noise_ = noiseDiagonal::Sigmas(Vec3::Constant(mag_noise)); bias_noise_ = noiseDiagonal::Sigmas(gtsam::Vector6::Constant(bias_noise)); +} + +void Slam::intializeDirectionOfLocalMagField() +{ + std::vector lmg = + pnh_.param("localMagneticField", std::vector{ 0.0000227095, -0.0000020783, 0.0000432753 }); + local_mag_field_ = gtsam::Unit3(lmg[0], lmg[1], lmg[2]); } \ No newline at end of file diff --git a/igvc_platform/src/imu/YostLabDriver.cpp b/igvc_platform/src/imu/YostLabDriver.cpp index ddd8406a4..0c2824e92 100644 --- a/igvc_platform/src/imu/YostLabDriver.cpp +++ b/igvc_platform/src/imu/YostLabDriver.cpp @@ -293,7 +293,7 @@ void YostLabDriver::createAndPublishIMUMessage(std::vector &parsed_val) magnet_msg.magnetic_field.x = compass_raw[0]; magnet_msg.magnetic_field.y = compass_raw[1]; magnet_msg.magnetic_field.z = compass_raw[2]; - magnet_msg.magnetic_field_covariance = { 1e-5, 0, 0, 0, 1e-5, 0, 0, 0, 1e-5 }; + magnet_msg.magnetic_field_covariance = { 1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6 }; sensor_temp_ = parsed_val[13]; From 72163dab842c191eb733f4a2a45b57722c64c8a1 Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Sun, 29 Mar 2020 00:50:41 -0400 Subject: [PATCH 15/55] working on adding mag factor --- igvc_gazebo/launch/simulation.launch | 1 + igvc_gazebo/launch/simulation_low.launch | 1 + igvc_gazebo/nodes/magnetometer/main.cpp | 10 ++++++---- igvc_perception/include/slam/slam.h | 2 +- igvc_perception/launch/slam.launch | 2 +- igvc_perception/src/slam/slam.cpp | 19 +++++++++++++++---- 6 files changed, 25 insertions(+), 10 deletions(-) diff --git a/igvc_gazebo/launch/simulation.launch b/igvc_gazebo/launch/simulation.launch index 0f780ab8f..134b86eb1 100644 --- a/igvc_gazebo/launch/simulation.launch +++ b/igvc_gazebo/launch/simulation.launch @@ -56,6 +56,7 @@ + diff --git a/igvc_gazebo/launch/simulation_low.launch b/igvc_gazebo/launch/simulation_low.launch index 61dde2bbc..a08a93c92 100644 --- a/igvc_gazebo/launch/simulation_low.launch +++ b/igvc_gazebo/launch/simulation_low.launch @@ -62,6 +62,7 @@ + diff --git a/igvc_gazebo/nodes/magnetometer/main.cpp b/igvc_gazebo/nodes/magnetometer/main.cpp index 8675c44ed..7d8391316 100644 --- a/igvc_gazebo/nodes/magnetometer/main.cpp +++ b/igvc_gazebo/nodes/magnetometer/main.cpp @@ -2,7 +2,8 @@ #include #include -ros::Publisher mag_field_pub_; +ros::Publisher g_mag_field_pub; +static double g_mag_field_covar; void magCallback(const geometry_msgs::Vector3Stamped& msg) { @@ -13,8 +14,8 @@ void magCallback(const geometry_msgs::Vector3Stamped& msg) magnet_msg.magnetic_field.x = msg.vector.x; magnet_msg.magnetic_field.y = msg.vector.y; magnet_msg.magnetic_field.z = msg.vector.z; - magnet_msg.magnetic_field_covariance = { 1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6 }; - mag_field_pub_.publish(magnet_msg); + magnet_msg.magnetic_field_covariance = { g_mag_field_covar, 0, 0, 0, g_mag_field_covar, 0, 0, 0, g_mag_field_covar }; + g_mag_field_pub.publish(magnet_msg); } int main(int argc, char** argv) @@ -24,7 +25,8 @@ int main(int argc, char** argv) ros::NodeHandle pnh("~"); std::string sub_topic = pnh.param("mag_sub_topic", std::string("/magnetometer/vector")); std::string pub_topic = pnh.param("mag_pub_topic", std::string("/magnetometer_mag")); - mag_field_pub_ = nh.advertise(pub_topic, 10); + g_mag_field_covar = pnh.param("mag_field_variance", 1e-6); + g_mag_field_pub = nh.advertise(pub_topic, 10); ros::Subscriber scan_sub = nh.subscribe(sub_topic, 1, magCallback); ros::spin(); } \ No newline at end of file diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index fd28961be..ce1ca8012 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -49,7 +49,7 @@ class Slam void initializeImuParams(); void initializePriors(); void initializeNoiseMatrices(); - void intializeDirectionOfLocalMagField(); + void initializeDirectionOfLocalMagField(); // Defining some types typedef gtsam::noiseModel::Diagonal noiseDiagonal; diff --git a/igvc_perception/launch/slam.launch b/igvc_perception/launch/slam.launch index 4f83a8f33..98593fe30 100644 --- a/igvc_perception/launch/slam.launch +++ b/igvc_perception/launch/slam.launch @@ -3,6 +3,6 @@ - + \ No newline at end of file diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 2b2068ad7..31ffbd43b 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -20,7 +20,7 @@ Slam::Slam() : pnh_{ "~" } imu_connected_ = false; imu_update_available_ = false; - intializeDirectionOfLocalMagField(); + initializeDirectionOfLocalMagField(); initializeNoiseMatrices(); initializeImuParams(); initializePriors(); @@ -78,7 +78,15 @@ void Slam::magCallback(const sensor_msgs::MagneticField &msg) } /** - * If there are IMU measurements in the acumulator, this adds them as a single factor to the factor graph. + * Adds the magFactor + */ +void Slam::addMagFactor() +{ + +} + +/** + * If there are IMU measurements in the accumulator, this adds them as a single factor to the factor graph. */ void Slam::integrateAndAddIMUFactor() { @@ -183,9 +191,12 @@ void Slam::initializeNoiseMatrices() bias_noise_ = noiseDiagonal::Sigmas(gtsam::Vector6::Constant(bias_noise)); } -void Slam::intializeDirectionOfLocalMagField() +/** + * Initialize the local magnetic field direction from launch params + */ +void Slam::initializeDirectionOfLocalMagField() { std::vector lmg = - pnh_.param("localMagneticField", std::vector{ 0.0000227095, -0.0000020783, 0.0000432753 }); + pnh_.param("localMagneticField", std::vector{ 0.0000227095, 0.0000020783, -0.0000432753 }); local_mag_field_ = gtsam::Unit3(lmg[0], lmg[1], lmg[2]); } \ No newline at end of file From ed21025452e47b0a9d49dcb4c8b8d317946815da Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Sun, 29 Mar 2020 23:08:08 -0400 Subject: [PATCH 16/55] added custom mag factor --- igvc_perception/include/slam/MagPoseFactor.h | 49 ++++++++++++++++ igvc_perception/include/slam/slam.h | 10 +++- igvc_perception/launch/slam_simulation.launch | 5 -- igvc_perception/src/slam/slam.cpp | 57 +++++++++++-------- 4 files changed, 88 insertions(+), 33 deletions(-) create mode 100644 igvc_perception/include/slam/MagPoseFactor.h delete mode 100644 igvc_perception/launch/slam_simulation.launch diff --git a/igvc_perception/include/slam/MagPoseFactor.h b/igvc_perception/include/slam/MagPoseFactor.h new file mode 100644 index 000000000..0d6deba09 --- /dev/null +++ b/igvc_perception/include/slam/MagPoseFactor.h @@ -0,0 +1,49 @@ +#ifndef SRC_MAGPOSEFACTOR_H +#define SRC_MAGPOSEFACTOR_H + +#include +#include + +class MagPoseFactor: public gtsam::NoiseModelFactor1 { + + const gtsam::Point3 measured_; ///< The measured magnetometer values + const gtsam::Point3 nM_; ///< Local magnetic field (mag output units) + const gtsam::Point3 bias_; ///< bias + +public: + /** + * Constructor of factor that estimates nav to body rotation bRn + * @param key of the unknown Pose X in the factor graph + * @param measured magnetometer reading, a 3-vector + * @param scale by which a unit vector is scaled to yield a magnetometer reading + * @param direction of the local magnetic field, see e.g. http://www.ngdc.noaa.gov/geomag-web/#igrfwmm + * @param bias of the magnetometer, modeled as purely additive (after scaling) + * @param model of the additive Gaussian noise that is assumed + */ + MagPoseFactor(gtsam::Key key, const gtsam::Point3& measured, double scale, + const gtsam::Unit3& direction, const gtsam::Point3& bias, + const gtsam::SharedNoiseModel& model) : + gtsam::NoiseModelFactor1(model, key), // + measured_(measured), nM_(scale * direction), bias_(bias) { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new MagPoseFactor(*this))); + } + + + /** + * @brief vector of errors + */ + gtsam::Vector evaluateError(const gtsam::Pose3& nRb, + boost::optional H = boost::none) const { + // measured bM = nRbÕ * nM + b + gtsam::Point3 hx = nRb.rotation().unrotate(nM_, H) + bias_; + auto error = ((gtsam::Point3)(hx - measured_)).vector(); + return error; + } +}; + +#endif //SRC_MAGPOSEFACTOR_H diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index ce1ca8012..c514c36ec 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -14,6 +14,8 @@ #include // Helps initialize initial guess #include +// Custom Mag Factor based around Pose3 +#include #include #include #include @@ -50,23 +52,25 @@ class Slam void initializePriors(); void initializeNoiseMatrices(); void initializeDirectionOfLocalMagField(); + void addMagFactor(); // Defining some types typedef gtsam::noiseModel::Diagonal noiseDiagonal; typedef gtsam::Vector3 Vec3; // Establishing global variables - gtsam::Values initEstimate_, result_; + gtsam::Values init_estimate_, result_; gtsam::NonlinearFactorGraph graph_; - gtsam::Pose3 previousPose_; + gtsam::Pose3 previous_pose_; unsigned long curr_index_; noiseDiagonal::shared_ptr gps_noise_, bias_noise_, mag_noise_; gtsam::Unit3 local_mag_field_; gtsam::Point3 curr_mag_reading_; gtsam::ISAM2 isam_; gtsam::PreintegratedImuMeasurements accum_; - ros::Time lastImuMeasurement_; + ros::Time last_imu_measurement_; bool imu_connected_, imu_update_available_; + double scale_; const double KGRAVITY = 9.81; }; diff --git a/igvc_perception/launch/slam_simulation.launch b/igvc_perception/launch/slam_simulation.launch deleted file mode 100644 index 7718b15a3..000000000 --- a/igvc_perception/launch/slam_simulation.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - \ No newline at end of file diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 31ffbd43b..077521e4b 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -32,16 +32,17 @@ Slam::Slam() : pnh_{ "~" } */ void Slam::gpsCallback(const nav_msgs::Odometry &msg) { - gtsam::Point3 currPoint = Conversion::getPoint3FromOdom(msg); - gtsam::GPSFactor gpsFactor(X(curr_index_), currPoint, gps_noise_); - graph_.add(gpsFactor); + gtsam::Point3 curr_point = Conversion::getPoint3FromOdom(msg); + gtsam::GPSFactor gps_factor (X(curr_index_+1), curr_point, gps_noise_); + graph_.add(gps_factor); + + addMagFactor(); if (imu_update_available_) { integrateAndAddIMUFactor(); + imu_update_available_ = false; } - - imu_update_available_ = false; } /** @@ -50,22 +51,22 @@ void Slam::gpsCallback(const nav_msgs::Odometry &msg) */ void Slam::imuCallback(const sensor_msgs::Imu &msg) { - ros::Time currTime = ros::Time::now(); - Vec3 measuredAcc = Vec3(msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z); - Vec3 measuredOmega = Vec3(msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z); + ros::Time curr_time = ros::Time::now(); + Vec3 measured_acc = Vec3(msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z); + Vec3 measured_omega = Vec3(msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z); if (!imu_connected_) { imu_connected_ = true; - accum_.integrateMeasurement(measuredAcc, measuredOmega, 0.005); + accum_.integrateMeasurement(measured_acc, measured_omega, 0.005); } else { - double deltaT = (currTime - lastImuMeasurement_).toSec(); + double deltaT = (curr_time - last_imu_measurement_).toSec(); if (deltaT > 0) - accum_.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + accum_.integrateMeasurement(measured_acc, measured_omega, deltaT); } imu_update_available_ = true; - lastImuMeasurement_ = currTime; + last_imu_measurement_ = curr_time; } /** @@ -78,11 +79,13 @@ void Slam::magCallback(const sensor_msgs::MagneticField &msg) } /** - * Adds the magFactor + * Adds the magFactor to the factor graph */ void Slam::addMagFactor() { - + MagPoseFactor mag_factor (X(curr_index_), curr_mag_reading_, scale_, + local_mag_field_, gtsam::Point3(1e-9, 1e-9, 1e-9), mag_noise_); + graph_.add(mag_factor); } /** @@ -96,7 +99,7 @@ void Slam::integrateAndAddIMUFactor() auto factor = boost::make_shared >( B(curr_index_), B(curr_index_ + 1), gtsam::imuBias::ConstantBias(), bias_noise_); graph_.add(factor); - initEstimate_.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); + init_estimate_.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); // Add imu factor gtsam::ImuFactor imufac(X(curr_index_), V(curr_index_), X(curr_index_ + 1), V(curr_index_ + 1), B(curr_index_ + 1), @@ -106,10 +109,10 @@ void Slam::integrateAndAddIMUFactor() auto newPoseEstimate = result_.at(X(curr_index_)); // gtsam::Pose3 newPoseEstimate newPoseEstimate = gtsam::Pose3(accum_.deltaRij() * newPoseEstimate.rotation(), accum_.deltaPij() + newPoseEstimate.translation()); - initEstimate_.insert(X(curr_index_ + 1), newPoseEstimate); + init_estimate_.insert(X(curr_index_ + 1), newPoseEstimate); Vec3 lastVel = result_.at(V(curr_index_)); lastVel += accum_.deltaVij(); - initEstimate_.insert(V(curr_index_ + 1), lastVel); + init_estimate_.insert(V(curr_index_ + 1), lastVel); curr_index_++; optimize(); } @@ -124,14 +127,16 @@ void Slam::optimize() static int iteration = 0; ROS_WARN_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ << " curr_index: " << curr_index_); - // graph.print(); - isam_.update(graph_, initEstimate_); + graph_.print(); + isam_.update(graph_, init_estimate_); result_ = isam_.calculateEstimate(); graph_.resize(0); - initEstimate_.clear(); + init_estimate_.clear(); + + result_.print(); - auto currPose = result_.at(X(curr_index_)); // gtsam::Pose2 currPose - location_pub_.publish(Conversion::getOdomFromPose3(currPose)); + auto curr_pose = result_.at(X(curr_index_)); // gtsam::Pose3 currPose + location_pub_.publish(Conversion::getOdomFromPose3(curr_pose)); } /** @@ -159,19 +164,19 @@ void Slam::initializePriors() noiseDiagonal::shared_ptr poseNoise = noiseDiagonal::Sigmas((gtsam::Vector(6) << Vec3::Constant(0.1), Vec3::Constant(0.3)).finished()); graph_.push_back(gtsam::PriorFactor(X(0), priorPose, poseNoise)); - initEstimate_.insert(X(0), priorPose); + init_estimate_.insert(X(0), priorPose); // Adding Initial Velocity (Pose + Covariance Matrix) Vec3 priorVel(0.0, 0.0, 0.0); noiseDiagonal::shared_ptr velNoise = noiseDiagonal::Sigmas(Vec3::Constant(0.1)); graph_.push_back(gtsam::PriorFactor(V(0), priorVel, velNoise)); - initEstimate_.insert(V(0), priorVel); + init_estimate_.insert(V(0), priorVel); // Adding Bias Prior noiseDiagonal::shared_ptr biasNoise = noiseDiagonal::Sigmas(gtsam::Vector6::Constant(0.1)); gtsam::PriorFactor biasprior(B(0), gtsam::imuBias::ConstantBias(), biasNoise); graph_.push_back(biasprior); - initEstimate_.insert(B(0), gtsam::imuBias::ConstantBias()); + init_estimate_.insert(B(0), gtsam::imuBias::ConstantBias()); optimize(); ROS_INFO_STREAM("Priors Initialized."); @@ -199,4 +204,6 @@ void Slam::initializeDirectionOfLocalMagField() std::vector lmg = pnh_.param("localMagneticField", std::vector{ 0.0000227095, 0.0000020783, -0.0000432753 }); local_mag_field_ = gtsam::Unit3(lmg[0], lmg[1], lmg[2]); + scale_ = Vec3(lmg[0], lmg[1], lmg[2]).norm(); + ROS_WARN_STREAM("scale_:" << scale_); } \ No newline at end of file From fecb4247dc8fdf9733ddb89390b72231b04f0e62 Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Sun, 29 Mar 2020 23:09:09 -0400 Subject: [PATCH 17/55] make format --- igvc_perception/include/slam/MagPoseFactor.h | 37 +++++++++++--------- igvc_perception/src/slam/slam.cpp | 6 ++-- 2 files changed, 23 insertions(+), 20 deletions(-) diff --git a/igvc_perception/include/slam/MagPoseFactor.h b/igvc_perception/include/slam/MagPoseFactor.h index 0d6deba09..b9c1cba6d 100644 --- a/igvc_perception/include/slam/MagPoseFactor.h +++ b/igvc_perception/include/slam/MagPoseFactor.h @@ -4,11 +4,11 @@ #include #include -class MagPoseFactor: public gtsam::NoiseModelFactor1 { - - const gtsam::Point3 measured_; ///< The measured magnetometer values - const gtsam::Point3 nM_; ///< Local magnetic field (mag output units) - const gtsam::Point3 bias_; ///< bias +class MagPoseFactor : public gtsam::NoiseModelFactor1 +{ + const gtsam::Point3 measured_; ///< The measured magnetometer values + const gtsam::Point3 nM_; ///< Local magnetic field (mag output units) + const gtsam::Point3 bias_; ///< bias public: /** @@ -20,25 +20,28 @@ class MagPoseFactor: public gtsam::NoiseModelFactor1 { * @param bias of the magnetometer, modeled as purely additive (after scaling) * @param model of the additive Gaussian noise that is assumed */ - MagPoseFactor(gtsam::Key key, const gtsam::Point3& measured, double scale, - const gtsam::Unit3& direction, const gtsam::Point3& bias, - const gtsam::SharedNoiseModel& model) : - gtsam::NoiseModelFactor1(model, key), // - measured_(measured), nM_(scale * direction), bias_(bias) { + MagPoseFactor(gtsam::Key key, const gtsam::Point3& measured, double scale, const gtsam::Unit3& direction, + const gtsam::Point3& bias, const gtsam::SharedNoiseModel& model) + : gtsam::NoiseModelFactor1(model, key) + , // + measured_(measured) + , nM_(scale * direction) + , bias_(bias) + { } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + virtual gtsam::NonlinearFactor::shared_ptr clone() const + { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new MagPoseFactor(*this))); } - /** - * @brief vector of errors - */ - gtsam::Vector evaluateError(const gtsam::Pose3& nRb, - boost::optional H = boost::none) const { + * @brief vector of errors + */ + gtsam::Vector evaluateError(const gtsam::Pose3& nRb, boost::optional H = boost::none) const + { // measured bM = nRbÕ * nM + b gtsam::Point3 hx = nRb.rotation().unrotate(nM_, H) + bias_; auto error = ((gtsam::Point3)(hx - measured_)).vector(); @@ -46,4 +49,4 @@ class MagPoseFactor: public gtsam::NoiseModelFactor1 { } }; -#endif //SRC_MAGPOSEFACTOR_H +#endif // SRC_MAGPOSEFACTOR_H diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 077521e4b..2d67a02e6 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -33,7 +33,7 @@ Slam::Slam() : pnh_{ "~" } void Slam::gpsCallback(const nav_msgs::Odometry &msg) { gtsam::Point3 curr_point = Conversion::getPoint3FromOdom(msg); - gtsam::GPSFactor gps_factor (X(curr_index_+1), curr_point, gps_noise_); + gtsam::GPSFactor gps_factor(X(curr_index_ + 1), curr_point, gps_noise_); graph_.add(gps_factor); addMagFactor(); @@ -83,8 +83,8 @@ void Slam::magCallback(const sensor_msgs::MagneticField &msg) */ void Slam::addMagFactor() { - MagPoseFactor mag_factor (X(curr_index_), curr_mag_reading_, scale_, - local_mag_field_, gtsam::Point3(1e-9, 1e-9, 1e-9), mag_noise_); + MagPoseFactor mag_factor(X(curr_index_), curr_mag_reading_, scale_, local_mag_field_, gtsam::Point3(1e-9, 1e-9, 1e-9), + mag_noise_); graph_.add(mag_factor); } From a494974b56c0e1327e48d46cbb2c9d57b932a32c Mon Sep 17 00:00:00 2001 From: Oswin So Date: Thu, 2 Apr 2020 07:16:42 +0800 Subject: [PATCH 18/55] Fixes for MagPoseFactor jacobian bug --- igvc_perception/include/slam/MagPoseFactor.h | 26 +++++++++++-- igvc_perception/include/slam/slam.h | 1 + igvc_perception/launch/slam.launch | 3 +- igvc_perception/src/slam/slam.cpp | 37 ++++++++++++++----- igvc_perception/src/slam/type_conversions.cpp | 1 + 5 files changed, 55 insertions(+), 13 deletions(-) diff --git a/igvc_perception/include/slam/MagPoseFactor.h b/igvc_perception/include/slam/MagPoseFactor.h index b9c1cba6d..a8aa0be14 100644 --- a/igvc_perception/include/slam/MagPoseFactor.h +++ b/igvc_perception/include/slam/MagPoseFactor.h @@ -40,12 +40,32 @@ class MagPoseFactor : public gtsam::NoiseModelFactor1 /** * @brief vector of errors */ - gtsam::Vector evaluateError(const gtsam::Pose3& nRb, boost::optional H = boost::none) const + gtsam::Vector evaluateError(const gtsam::Pose3& nRb, boost::optional H) const override { // measured bM = nRbÕ * nM + b - gtsam::Point3 hx = nRb.rotation().unrotate(nM_, H) + bias_; - auto error = ((gtsam::Point3)(hx - measured_)).vector(); + gtsam::Point3 hx = nRb.rotation().unrotate(nM_, H, boost::none) + bias_; + // H is 3x3 from above unrotate operation, but needs to be 3x6, ie. set the 3x3 block on the right + // to all zeros. + if (H) { + H->conservativeResize(3, 6); + H->rightCols(3).setZero(); + } + const auto error = hx - measured_; return error; +// std::cout << "> [MagPoseFactor] evaluateError\n"; +// std::cout << " hx: " << hx.transpose() << "\n"; +// std::cout << " measured_: " << measured_.transpose() << "\n"; +// std::cout << " error: " << error.transpose() << "\n"; +// const auto whitened = noiseModel_ ? noiseModel_->whiten(error) : error; +// std::cout << " whitened: " << whitened.transpose() << std::endl; +// std::cout << " tot error: " << whitened.norm() << std::endl; +// return gtsam::Vector3::Zero(); + } + + void print(const std::string& s, const gtsam::KeyFormatter& keyFormatter) const override { + std::cout << s << "MagPoseFactor on " << keyFormatter(key()) << "\n"; + std::cout << s << " Mag measurement: " << measured_.transpose() << std::endl; + noiseModel_->print(" noise model: "); } }; diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index c514c36ec..880e27962 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -60,6 +60,7 @@ class Slam // Establishing global variables gtsam::Values init_estimate_, result_; + gtsam::Values history_; gtsam::NonlinearFactorGraph graph_; gtsam::Pose3 previous_pose_; unsigned long curr_index_; diff --git a/igvc_perception/launch/slam.launch b/igvc_perception/launch/slam.launch index 98593fe30..95d8b7687 100644 --- a/igvc_perception/launch/slam.launch +++ b/igvc_perception/launch/slam.launch @@ -1,8 +1,9 @@ - + + \ No newline at end of file diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 2d67a02e6..9d062440b 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -35,6 +35,7 @@ void Slam::gpsCallback(const nav_msgs::Odometry &msg) gtsam::Point3 curr_point = Conversion::getPoint3FromOdom(msg); gtsam::GPSFactor gps_factor(X(curr_index_ + 1), curr_point, gps_noise_); graph_.add(gps_factor); + ROS_INFO_STREAM("Factor " << graph_.size() << ": GPSFactor on x" << curr_index_ + 1); addMagFactor(); @@ -83,7 +84,7 @@ void Slam::magCallback(const sensor_msgs::MagneticField &msg) */ void Slam::addMagFactor() { - MagPoseFactor mag_factor(X(curr_index_), curr_mag_reading_, scale_, local_mag_field_, gtsam::Point3(1e-9, 1e-9, 1e-9), + MagPoseFactor mag_factor(X(curr_index_ + 1), curr_mag_reading_, scale_, local_mag_field_, gtsam::Point3(1e-9, 1e-9, 1e-9), mag_noise_); graph_.add(mag_factor); } @@ -96,13 +97,13 @@ void Slam::integrateAndAddIMUFactor() if (accum_.preintMeasCov().trace() != 0) { // Add bias factor - auto factor = boost::make_shared >( - B(curr_index_), B(curr_index_ + 1), gtsam::imuBias::ConstantBias(), bias_noise_); - graph_.add(factor); - init_estimate_.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); +// auto factor = boost::make_shared >( +// B(curr_index_), B(curr_index_ + 1), gtsam::imuBias::ConstantBias(), bias_noise_); +// graph_.add(factor); +// init_estimate_.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); // Add imu factor - gtsam::ImuFactor imufac(X(curr_index_), V(curr_index_), X(curr_index_ + 1), V(curr_index_ + 1), B(curr_index_ + 1), + gtsam::ImuFactor imufac(X(curr_index_), V(curr_index_), X(curr_index_ + 1), V(curr_index_ + 1), B(0), accum_); graph_.add(imufac); @@ -127,14 +128,29 @@ void Slam::optimize() static int iteration = 0; ROS_WARN_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ << " curr_index: " << curr_index_); - graph_.print(); + + // Add initial estimates to history_ + history_.insert(init_estimate_); + + // Update ISAM graph with new factors and estimates isam_.update(graph_, init_estimate_); +// graph_.printErrors(history_); + +// ROS_INFO_STREAM("printErrors:"); +// isam_.getFactorsUnsafe().printErrors(history_); + +// ROS_INFO_STREAM("isam_ graph"); +// isam_.getFactorsUnsafe().print(); result_ = isam_.calculateEstimate(); +// graph_.printErrors(init_estimate_); + + // Update variables with optimized ones + history_.update(result_); + + // Clear graph and estimates graph_.resize(0); init_estimate_.clear(); - result_.print(); - auto curr_pose = result_.at(X(curr_index_)); // gtsam::Pose3 currPose location_pub_.publish(Conversion::getOdomFromPose3(curr_pose)); } @@ -164,18 +180,21 @@ void Slam::initializePriors() noiseDiagonal::shared_ptr poseNoise = noiseDiagonal::Sigmas((gtsam::Vector(6) << Vec3::Constant(0.1), Vec3::Constant(0.3)).finished()); graph_.push_back(gtsam::PriorFactor(X(0), priorPose, poseNoise)); + ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on x0"); init_estimate_.insert(X(0), priorPose); // Adding Initial Velocity (Pose + Covariance Matrix) Vec3 priorVel(0.0, 0.0, 0.0); noiseDiagonal::shared_ptr velNoise = noiseDiagonal::Sigmas(Vec3::Constant(0.1)); graph_.push_back(gtsam::PriorFactor(V(0), priorVel, velNoise)); + ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on v0"); init_estimate_.insert(V(0), priorVel); // Adding Bias Prior noiseDiagonal::shared_ptr biasNoise = noiseDiagonal::Sigmas(gtsam::Vector6::Constant(0.1)); gtsam::PriorFactor biasprior(B(0), gtsam::imuBias::ConstantBias(), biasNoise); graph_.push_back(biasprior); + ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on B0"); init_estimate_.insert(B(0), gtsam::imuBias::ConstantBias()); optimize(); diff --git a/igvc_perception/src/slam/type_conversions.cpp b/igvc_perception/src/slam/type_conversions.cpp index 96bf0d75a..6b855d62c 100644 --- a/igvc_perception/src/slam/type_conversions.cpp +++ b/igvc_perception/src/slam/type_conversions.cpp @@ -10,6 +10,7 @@ gtsam::Pose3 Conversion::getPose3FromOdom(const nav_msgs::Odometry &msg) nav_msgs::Odometry Conversion::getOdomFromPose3(const gtsam::Pose3 &pos) { nav_msgs::Odometry msg; + msg.header.stamp = ros::Time::now(); msg.child_frame_id = "/base_footprint"; msg.header.frame_id = "/odom"; msg.pose.pose.position.x = pos.x(); From 50e7fb1cb922799ccad77c8c88d75d3277ff1bcc Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Fri, 3 Apr 2020 00:39:06 -0400 Subject: [PATCH 19/55] Added debugging mode --- igvc_perception/include/slam/MagPoseFactor.h | 8 ---- igvc_perception/include/slam/slam.h | 9 +++- igvc_perception/src/slam/slam.cpp | 49 ++++++++++++-------- 3 files changed, 36 insertions(+), 30 deletions(-) diff --git a/igvc_perception/include/slam/MagPoseFactor.h b/igvc_perception/include/slam/MagPoseFactor.h index a8aa0be14..75f85019c 100644 --- a/igvc_perception/include/slam/MagPoseFactor.h +++ b/igvc_perception/include/slam/MagPoseFactor.h @@ -52,14 +52,6 @@ class MagPoseFactor : public gtsam::NoiseModelFactor1 } const auto error = hx - measured_; return error; -// std::cout << "> [MagPoseFactor] evaluateError\n"; -// std::cout << " hx: " << hx.transpose() << "\n"; -// std::cout << " measured_: " << measured_.transpose() << "\n"; -// std::cout << " error: " << error.transpose() << "\n"; -// const auto whitened = noiseModel_ ? noiseModel_->whiten(error) : error; -// std::cout << " whitened: " << whitened.transpose() << std::endl; -// std::cout << " tot error: " << whitened.norm() << std::endl; -// return gtsam::Vector3::Zero(); } void print(const std::string& s, const gtsam::KeyFormatter& keyFormatter) const override { diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index 880e27962..fcc0205c2 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -28,6 +28,9 @@ #include #include +// Uncomment to enable debugging and have additional print statements +//#define _DEBUG 1 + class Slam { public: @@ -58,11 +61,13 @@ class Slam typedef gtsam::noiseModel::Diagonal noiseDiagonal; typedef gtsam::Vector3 Vec3; - // Establishing global variables + // Defining some variables gtsam::Values init_estimate_, result_; +#if defined(_DEBUG) gtsam::Values history_; +#endif + gtsam::NonlinearFactorGraph graph_; - gtsam::Pose3 previous_pose_; unsigned long curr_index_; noiseDiagonal::shared_ptr gps_noise_, bias_noise_, mag_noise_; gtsam::Unit3 local_mag_field_; diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 9d062440b..6d890a3c0 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -10,7 +10,6 @@ using gtsam::symbol_shorthand::X; */ Slam::Slam() : pnh_{ "~" } { - // odom_sub_ = pnh_.subscribe("/wheel_odometry", 10, &Slam::OdomCallback, this); imu_sub_ = pnh_.subscribe("/imu", 100, &Slam::imuCallback, this); gps_sub_ = pnh_.subscribe("/odometry/gps", 1, &Slam::gpsCallback, this); mag_sub_ = pnh_.subscribe("/magnetometer_mag", 1, &Slam::magCallback, this); @@ -35,7 +34,6 @@ void Slam::gpsCallback(const nav_msgs::Odometry &msg) gtsam::Point3 curr_point = Conversion::getPoint3FromOdom(msg); gtsam::GPSFactor gps_factor(X(curr_index_ + 1), curr_point, gps_noise_); graph_.add(gps_factor); - ROS_INFO_STREAM("Factor " << graph_.size() << ": GPSFactor on x" << curr_index_ + 1); addMagFactor(); @@ -84,8 +82,8 @@ void Slam::magCallback(const sensor_msgs::MagneticField &msg) */ void Slam::addMagFactor() { - MagPoseFactor mag_factor(X(curr_index_ + 1), curr_mag_reading_, scale_, local_mag_field_, gtsam::Point3(1e-9, 1e-9, 1e-9), - mag_noise_); + MagPoseFactor mag_factor(X(curr_index_ + 1), curr_mag_reading_, scale_, + local_mag_field_, gtsam::Point3(1e-9, 1e-9, 1e-9),mag_noise_); graph_.add(mag_factor); } @@ -97,14 +95,14 @@ void Slam::integrateAndAddIMUFactor() if (accum_.preintMeasCov().trace() != 0) { // Add bias factor -// auto factor = boost::make_shared >( -// B(curr_index_), B(curr_index_ + 1), gtsam::imuBias::ConstantBias(), bias_noise_); -// graph_.add(factor); -// init_estimate_.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); + auto factor = boost::make_shared >( + B(curr_index_), B(curr_index_ + 1), gtsam::imuBias::ConstantBias(), bias_noise_); + graph_.add(factor); + init_estimate_.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); // Add imu factor - gtsam::ImuFactor imufac(X(curr_index_), V(curr_index_), X(curr_index_ + 1), V(curr_index_ + 1), B(0), - accum_); + gtsam::ImuFactor imufac(X(curr_index_), V(curr_index_), X(curr_index_ + 1), + V(curr_index_ + 1), B(curr_index_ + 1),accum_); graph_.add(imufac); auto newPoseEstimate = result_.at(X(curr_index_)); // gtsam::Pose3 newPoseEstimate @@ -126,26 +124,31 @@ void Slam::integrateAndAddIMUFactor() void Slam::optimize() { static int iteration = 0; - ROS_WARN_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ + ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ << " curr_index: " << curr_index_); + // Update ISAM graph with new factors and estimates + isam_.update(graph_, init_estimate_); + #if defined(_DEBUG) // Add initial estimates to history_ history_.insert(init_estimate_); + graph_.printErrors(history_); - // Update ISAM graph with new factors and estimates - isam_.update(graph_, init_estimate_); -// graph_.printErrors(history_); + ROS_INFO_STREAM("printErrors:"); + isam_.getFactorsUnsafe().printErrors(history_); -// ROS_INFO_STREAM("printErrors:"); -// isam_.getFactorsUnsafe().printErrors(history_); + ROS_INFO_STREAM("isam_ graph"); + isam_.getFactorsUnsafe().print(); + #endif -// ROS_INFO_STREAM("isam_ graph"); -// isam_.getFactorsUnsafe().print(); result_ = isam_.calculateEstimate(); -// graph_.printErrors(init_estimate_); + +#if defined(_DEBUG) + graph_.printErrors(init_estimate_); // Update variables with optimized ones history_.update(result_); +#endif // Clear graph and estimates graph_.resize(0); @@ -180,21 +183,27 @@ void Slam::initializePriors() noiseDiagonal::shared_ptr poseNoise = noiseDiagonal::Sigmas((gtsam::Vector(6) << Vec3::Constant(0.1), Vec3::Constant(0.3)).finished()); graph_.push_back(gtsam::PriorFactor(X(0), priorPose, poseNoise)); +#if defined(_DEBUG) ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on x0"); +#endif init_estimate_.insert(X(0), priorPose); // Adding Initial Velocity (Pose + Covariance Matrix) Vec3 priorVel(0.0, 0.0, 0.0); noiseDiagonal::shared_ptr velNoise = noiseDiagonal::Sigmas(Vec3::Constant(0.1)); graph_.push_back(gtsam::PriorFactor(V(0), priorVel, velNoise)); +#if defined(_DEBUG) ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on v0"); +#endif init_estimate_.insert(V(0), priorVel); // Adding Bias Prior noiseDiagonal::shared_ptr biasNoise = noiseDiagonal::Sigmas(gtsam::Vector6::Constant(0.1)); gtsam::PriorFactor biasprior(B(0), gtsam::imuBias::ConstantBias(), biasNoise); graph_.push_back(biasprior); +#if defined(_DEBUG) ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on B0"); +#endif init_estimate_.insert(B(0), gtsam::imuBias::ConstantBias()); optimize(); @@ -224,5 +233,5 @@ void Slam::initializeDirectionOfLocalMagField() pnh_.param("localMagneticField", std::vector{ 0.0000227095, 0.0000020783, -0.0000432753 }); local_mag_field_ = gtsam::Unit3(lmg[0], lmg[1], lmg[2]); scale_ = Vec3(lmg[0], lmg[1], lmg[2]).norm(); - ROS_WARN_STREAM("scale_:" << scale_); + ROS_INFO_STREAM("Initializing Direction of Local Magnetic Field"); } \ No newline at end of file From a49dcc46953345f023db0446b45fcce275ae974e Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Sun, 17 May 2020 21:09:30 -0400 Subject: [PATCH 20/55] working 3D localization, but teb is slow --- external/kindr_ros | 2 +- .../launch/3D_nav_simulation.launch | 29 ++++++++++++ igvc_perception/CMakeLists.txt | 9 ---- igvc_perception/include/slam/slam.h | 9 +++- igvc_perception/include/tests/SlamTests.h | 19 -------- igvc_perception/launch/slam.launch | 46 +++++++++++++++++++ igvc_perception/src/slam/slam.cpp | 17 ++++++- igvc_perception/src/tests/CMakeLists.txt | 10 ---- igvc_perception/src/tests/SlamTests.cpp | 5 -- igvc_perception/src/tests/main.cpp | 9 ---- 10 files changed, 100 insertions(+), 55 deletions(-) create mode 100644 igvc_navigation/launch/3D_nav_simulation.launch delete mode 100644 igvc_perception/include/tests/SlamTests.h delete mode 100644 igvc_perception/src/tests/CMakeLists.txt delete mode 100644 igvc_perception/src/tests/SlamTests.cpp delete mode 100644 igvc_perception/src/tests/main.cpp diff --git a/external/kindr_ros b/external/kindr_ros index dbc546619..4a9523640 160000 --- a/external/kindr_ros +++ b/external/kindr_ros @@ -1 +1 @@ -Subproject commit dbc546619309367aa10141a166b68205614e2a46 +Subproject commit 4a9523640582489d35ea061b3b3bc30c3c617562 diff --git a/igvc_navigation/launch/3D_nav_simulation.launch b/igvc_navigation/launch/3D_nav_simulation.launch new file mode 100644 index 000000000..960acb7c8 --- /dev/null +++ b/igvc_navigation/launch/3D_nav_simulation.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/igvc_perception/CMakeLists.txt b/igvc_perception/CMakeLists.txt index 66c57ba61..289bf5026 100644 --- a/igvc_perception/CMakeLists.txt +++ b/igvc_perception/CMakeLists.txt @@ -172,15 +172,6 @@ include_directories( # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # ) -############# -## Testing ## -############# - -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_subdirectory(src/tests) -endif () - add_subdirectory(src/slam) add_subdirectory(src/pointcloud_filter) add_subdirectory(src/robot_pose_type_converter) diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index fcc0205c2..1d86113df 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -17,7 +17,6 @@ // Custom Mag Factor based around Pose3 #include #include -#include #include #include #include @@ -27,6 +26,12 @@ #include #include #include +#include +#include +#include +#include +#include +#include // Uncomment to enable debugging and have additional print statements //#define _DEBUG 1 @@ -56,6 +61,7 @@ class Slam void initializeNoiseMatrices(); void initializeDirectionOfLocalMagField(); void addMagFactor(); + void updateTransform(const nav_msgs::Odometry &pos); // Defining some types typedef gtsam::noiseModel::Diagonal noiseDiagonal; @@ -68,6 +74,7 @@ class Slam #endif gtsam::NonlinearFactorGraph graph_; + tf2_ros::TransformBroadcaster world_transform_broadcaster_; unsigned long curr_index_; noiseDiagonal::shared_ptr gps_noise_, bias_noise_, mag_noise_; gtsam::Unit3 local_mag_field_; diff --git a/igvc_perception/include/tests/SlamTests.h b/igvc_perception/include/tests/SlamTests.h deleted file mode 100644 index 1edc62de3..000000000 --- a/igvc_perception/include/tests/SlamTests.h +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef SRC_SLAMTESTS_H -#define SRC_SLAMTESTS_H - -#include -#include -#include - -class SlamTests -{ -public: - SlamTests(); - -private: - ros::NodeHandle pnh_; - ros::Subscriber slam_sub_; - ros::Subscriber ground_truth_sub_; -}; - -#endif // SRC_SLAMTESTS_H diff --git a/igvc_perception/launch/slam.launch b/igvc_perception/launch/slam.launch index 95d8b7687..116e4243b 100644 --- a/igvc_perception/launch/slam.launch +++ b/igvc_perception/launch/slam.launch @@ -1,9 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 6d890a3c0..b81aa89c7 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -155,7 +155,22 @@ void Slam::optimize() init_estimate_.clear(); auto curr_pose = result_.at(X(curr_index_)); // gtsam::Pose3 currPose - location_pub_.publish(Conversion::getOdomFromPose3(curr_pose)); + auto odom_message = Conversion::getOdomFromPose3(curr_pose); + location_pub_.publish(odom_message); + updateTransform(odom_message); +} + +void Slam::updateTransform(const nav_msgs::Odometry &pos){ + geometry_msgs::TransformStamped tfLink; + tfLink.header.stamp = pos.header.stamp; + tfLink.header.frame_id = pos.header.frame_id; + tfLink.child_frame_id = pos.child_frame_id; + + tfLink.transform.translation.x = pos.pose.pose.position.x; + tfLink.transform.translation.y = pos.pose.pose.position.y; + tfLink.transform.translation.z = pos.pose.pose.position.z; + tfLink.transform.rotation = pos.pose.pose.orientation; + world_transform_broadcaster_.sendTransform(tfLink); } /** diff --git a/igvc_perception/src/tests/CMakeLists.txt b/igvc_perception/src/tests/CMakeLists.txt deleted file mode 100644 index 27949317f..000000000 --- a/igvc_perception/src/tests/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -add_executable(slam_tests main.cpp SlamTests.cpp) -add_dependencies(slam_tests ${catkin_EXPORTED_TARGETS}) -target_link_libraries(slam_tests ${catkin_LIBRARIES}) - -install( - TARGETS slam_tests - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) \ No newline at end of file diff --git a/igvc_perception/src/tests/SlamTests.cpp b/igvc_perception/src/tests/SlamTests.cpp deleted file mode 100644 index 4d03cbf86..000000000 --- a/igvc_perception/src/tests/SlamTests.cpp +++ /dev/null @@ -1,5 +0,0 @@ -#include "tests/SlamTests.h" - -SlamTests::SlamTests() -{ -} diff --git a/igvc_perception/src/tests/main.cpp b/igvc_perception/src/tests/main.cpp deleted file mode 100644 index 72c437f61..000000000 --- a/igvc_perception/src/tests/main.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include -#include - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "slam_test"); - SlamTests testing_node; - ros::spin(); -} \ No newline at end of file From 3224a2bd93c7f6fa257e6e9d93e55e33d464fee2 Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Mon, 18 May 2020 17:01:44 -0400 Subject: [PATCH 21/55] added twist to odom messages --- igvc_perception/include/slam/slam.h | 1 + .../include/slam/type_conversions.h | 7 ++-- igvc_perception/src/slam/slam.cpp | 24 +++++++++++--- igvc_perception/src/slam/type_conversions.cpp | 32 +++++++++++-------- 4 files changed, 43 insertions(+), 21 deletions(-) diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index 1d86113df..29841a191 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -62,6 +62,7 @@ class Slam void initializeDirectionOfLocalMagField(); void addMagFactor(); void updateTransform(const nav_msgs::Odometry &pos); + static nav_msgs::Odometry createOdomMsg(const gtsam::Pose3 &pos, const gtsam::Vector3 &vel, const gtsam::Vector3 &ang); // Defining some types typedef gtsam::noiseModel::Diagonal noiseDiagonal; diff --git a/igvc_perception/include/slam/type_conversions.h b/igvc_perception/include/slam/type_conversions.h index 462e7dcb9..e162a369c 100644 --- a/igvc_perception/include/slam/type_conversions.h +++ b/igvc_perception/include/slam/type_conversions.h @@ -10,9 +10,10 @@ class Conversion { public: - static gtsam::Pose3 getPose3FromOdom(const nav_msgs::Odometry &msg); - static gtsam::Point3 getPoint3FromOdom(const nav_msgs::Odometry &msg); - static nav_msgs::Odometry getOdomFromPose3(const gtsam::Pose3 &pos); + static gtsam::Pose3 odomMsgToGtsamPose3(const nav_msgs::Odometry &msg); + static gtsam::Point3 odomMsgToGtsamPoint3(const nav_msgs::Odometry &msg); + static geometry_msgs::Vector3 gtsamVector3ToVector3Msg(const gtsam::Vector3 &vec); + static geometry_msgs::Pose gtsamPose3ToPose3Msg(const gtsam::Pose3 &pos); }; #endif // SRC_TYPE_CONVERSIONS_H diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index b81aa89c7..03a93f6e7 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -31,7 +31,7 @@ Slam::Slam() : pnh_{ "~" } */ void Slam::gpsCallback(const nav_msgs::Odometry &msg) { - gtsam::Point3 curr_point = Conversion::getPoint3FromOdom(msg); + gtsam::Point3 curr_point = Conversion::odomMsgToGtsamPoint3(msg); gtsam::GPSFactor gps_factor(X(curr_index_ + 1), curr_point, gps_noise_); graph_.add(gps_factor); @@ -109,9 +109,9 @@ void Slam::integrateAndAddIMUFactor() newPoseEstimate = gtsam::Pose3(accum_.deltaRij() * newPoseEstimate.rotation(), accum_.deltaPij() + newPoseEstimate.translation()); init_estimate_.insert(X(curr_index_ + 1), newPoseEstimate); - Vec3 lastVel = result_.at(V(curr_index_)); - lastVel += accum_.deltaVij(); - init_estimate_.insert(V(curr_index_ + 1), lastVel); + Vec3 last_vel = result_.at(V(curr_index_)); + last_vel += accum_.deltaVij(); + init_estimate_.insert(V(curr_index_ + 1), last_vel); curr_index_++; optimize(); } @@ -155,7 +155,9 @@ void Slam::optimize() init_estimate_.clear(); auto curr_pose = result_.at(X(curr_index_)); // gtsam::Pose3 currPose - auto odom_message = Conversion::getOdomFromPose3(curr_pose); + Vec3 curr_vel = result_.at(V(curr_index_)); + Vec3 curr_ang = accum_.deltaRij().rpy()/accum_.deltaTij(); + auto odom_message = createOdomMsg(curr_pose, curr_vel, curr_ang); location_pub_.publish(odom_message); updateTransform(odom_message); } @@ -249,4 +251,16 @@ void Slam::initializeDirectionOfLocalMagField() local_mag_field_ = gtsam::Unit3(lmg[0], lmg[1], lmg[2]); scale_ = Vec3(lmg[0], lmg[1], lmg[2]).norm(); ROS_INFO_STREAM("Initializing Direction of Local Magnetic Field"); +} + +nav_msgs::Odometry Slam::createOdomMsg(const gtsam::Pose3 &pos, const gtsam::Vector3 &vel, const gtsam::Vector3 &ang) +{ + nav_msgs::Odometry msg; + msg.header.stamp = ros::Time::now(); + msg.child_frame_id = "/base_footprint"; + msg.header.frame_id = "/odom"; + msg.pose.pose = Conversion::gtsamPose3ToPose3Msg(pos); + msg.twist.twist.linear = Conversion::gtsamVector3ToVector3Msg(vel); + msg.twist.twist.angular = Conversion::gtsamVector3ToVector3Msg(ang); + return msg; } \ No newline at end of file diff --git a/igvc_perception/src/slam/type_conversions.cpp b/igvc_perception/src/slam/type_conversions.cpp index 6b855d62c..ee9472024 100644 --- a/igvc_perception/src/slam/type_conversions.cpp +++ b/igvc_perception/src/slam/type_conversions.cpp @@ -1,27 +1,33 @@ #include -gtsam::Pose3 Conversion::getPose3FromOdom(const nav_msgs::Odometry &msg) +gtsam::Pose3 Conversion::odomMsgToGtsamPose3(const nav_msgs::Odometry &msg) { return gtsam::Pose3(gtsam::Rot3(msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z), gtsam::Point3(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z)); } -nav_msgs::Odometry Conversion::getOdomFromPose3(const gtsam::Pose3 &pos) -{ - nav_msgs::Odometry msg; - msg.header.stamp = ros::Time::now(); - msg.child_frame_id = "/base_footprint"; - msg.header.frame_id = "/odom"; - msg.pose.pose.position.x = pos.x(); - msg.pose.pose.position.y = pos.y(); - msg.pose.pose.position.z = pos.z(); +geometry_msgs::Pose Conversion::gtsamPose3ToPose3Msg(const gtsam::Pose3 &pos){ + geometry_msgs::Pose gPos; + gPos.position.x = pos.x(); + gPos.position.y = pos.y(); + gPos.position.z = pos.z(); gtsam::Vector3 r = pos.rotation().xyz(); - msg.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(r.x(), r.y(), r.z()); - return msg; + gPos.orientation = tf::createQuaternionMsgFromRollPitchYaw(r.x(), r.y(), r.z()); + return gPos; +} + +geometry_msgs::Vector3 Conversion::gtsamVector3ToVector3Msg(const gtsam::Vector3 &vec){ + geometry_msgs::Vector3 gVec; + gVec.x = vec.x(); + gVec.y = vec.y(); + gVec.z = vec.z(); + return gVec; } -gtsam::Point3 Conversion::getPoint3FromOdom(const nav_msgs::Odometry &msg) + + +gtsam::Point3 Conversion::odomMsgToGtsamPoint3(const nav_msgs::Odometry &msg) { return gtsam::Point3(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z); } \ No newline at end of file From 5780b8b177ca4a9da6ccc1ac62980823c1ac1498 Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Mon, 1 Jun 2020 21:28:07 -0400 Subject: [PATCH 22/55] Change a param in igvc_control.launch The wheel radius was accidentally set to the wheel diameter causing the robot to move slowly. This commit will change the param to its correct value. --- igvc_gazebo/launch/igvc_control.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/igvc_gazebo/launch/igvc_control.launch b/igvc_gazebo/launch/igvc_control.launch index 3cb0f5cc8..6335ae228 100644 --- a/igvc_gazebo/launch/igvc_control.launch +++ b/igvc_gazebo/launch/igvc_control.launch @@ -17,7 +17,7 @@ - + From 746e8aa18fc8271081d1ed68e389d11883784325 Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Mon, 1 Jun 2020 23:26:55 -0400 Subject: [PATCH 23/55] change linear velocity to be in the local frame --- igvc_perception/src/slam/slam.cpp | 7 ++++++- igvc_perception/src/slam/type_conversions.cpp | 2 -- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 03a93f6e7..98c944870 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -124,7 +124,7 @@ void Slam::integrateAndAddIMUFactor() void Slam::optimize() { static int iteration = 0; - ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ + //ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ << " curr_index: " << curr_index_); // Update ISAM graph with new factors and estimates isam_.update(graph_, init_estimate_); @@ -156,6 +156,11 @@ void Slam::optimize() auto curr_pose = result_.at(X(curr_index_)); // gtsam::Pose3 currPose Vec3 curr_vel = result_.at(V(curr_index_)); + + // converts linear velocity to the local frame + auto global_to_local = curr_pose.rotation().toQuaternion(); + curr_vel = global_to_local._transformVector(curr_vel); + Vec3 curr_ang = accum_.deltaRij().rpy()/accum_.deltaTij(); auto odom_message = createOdomMsg(curr_pose, curr_vel, curr_ang); location_pub_.publish(odom_message); diff --git a/igvc_perception/src/slam/type_conversions.cpp b/igvc_perception/src/slam/type_conversions.cpp index ee9472024..e7370d360 100644 --- a/igvc_perception/src/slam/type_conversions.cpp +++ b/igvc_perception/src/slam/type_conversions.cpp @@ -25,8 +25,6 @@ geometry_msgs::Vector3 Conversion::gtsamVector3ToVector3Msg(const gtsam::Vector3 return gVec; } - - gtsam::Point3 Conversion::odomMsgToGtsamPoint3(const nav_msgs::Odometry &msg) { return gtsam::Point3(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z); From 91c0ffe995a4cf7791678b0d7bb424cc91090491 Mon Sep 17 00:00:00 2001 From: Andrew Yarovoi Date: Sat, 4 Jul 2020 14:57:31 -0400 Subject: [PATCH 24/55] Fix issue that caused the robot to move extremely slowly in the y direction --- igvc_perception/src/slam/slam.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 98c944870..fd53eb9a8 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -124,8 +124,8 @@ void Slam::integrateAndAddIMUFactor() void Slam::optimize() { static int iteration = 0; - //ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ - << " curr_index: " << curr_index_); +// ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ +// << " curr_index: " << curr_index_); // Update ISAM graph with new factors and estimates isam_.update(graph_, init_estimate_); @@ -158,7 +158,7 @@ void Slam::optimize() Vec3 curr_vel = result_.at(V(curr_index_)); // converts linear velocity to the local frame - auto global_to_local = curr_pose.rotation().toQuaternion(); + auto global_to_local = curr_pose.rotation().toQuaternion().inverse(); curr_vel = global_to_local._transformVector(curr_vel); Vec3 curr_ang = accum_.deltaRij().rpy()/accum_.deltaTij(); From 0a598ba760d2597c124eaa67eec3a0e1a5fd1ebf Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Fri, 16 Apr 2021 22:06:48 -0400 Subject: [PATCH 25/55] Changed frame_ids so can visualize odom in rviz. Added magnetometer readings into ekf_localization_node_params so initial orientation accounted for in robot_localization pose_ekf --- igvc_gazebo/nodes/ground_truth/main.cpp | 4 ++-- igvc_navigation/config/ekf_localization_node_params.yaml | 7 +++++++ igvc_navigation/launch/localization.launch | 2 +- igvc_perception/launch/slam.launch | 2 +- igvc_perception/src/slam/slam.cpp | 5 +++-- 5 files changed, 14 insertions(+), 6 deletions(-) diff --git a/igvc_gazebo/nodes/ground_truth/main.cpp b/igvc_gazebo/nodes/ground_truth/main.cpp index c733c7609..5b13c6cd8 100644 --- a/igvc_gazebo/nodes/ground_truth/main.cpp +++ b/igvc_gazebo/nodes/ground_truth/main.cpp @@ -43,8 +43,8 @@ void groundTruthCallback(const nav_msgs::Odometry::ConstPtr& msg) // set up the correct header result.header = msg->header; - result.child_frame_id = "/base_footprint"; - result.header.frame_id = "/odom"; + result.child_frame_id = "base_footprint"; + result.header.frame_id = "odom"; tf::Quaternion quat; tf::Vector3 pos; diff --git a/igvc_navigation/config/ekf_localization_node_params.yaml b/igvc_navigation/config/ekf_localization_node_params.yaml index 9396e976d..2a3320ba6 100644 --- a/igvc_navigation/config/ekf_localization_node_params.yaml +++ b/igvc_navigation/config/ekf_localization_node_params.yaml @@ -148,6 +148,13 @@ imu0_queue_size: 5 #imu0_twist_rejection_threshold: 0.8 # #imu0_linear_acceleration_rejection_threshold: 0.8 # +imu1: /magnetometer +imu1_config: [ false, false, false, + false, false, true, + false, false, false, + false, false, false, + false, false, false] + # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. imu0_remove_gravitational_acceleration: true diff --git a/igvc_navigation/launch/localization.launch b/igvc_navigation/launch/localization.launch index 6c49fe96e..2d43efe71 100644 --- a/igvc_navigation/launch/localization.launch +++ b/igvc_navigation/launch/localization.launch @@ -23,7 +23,7 @@ - + diff --git a/igvc_perception/launch/slam.launch b/igvc_perception/launch/slam.launch index 116e4243b..2e88e6019 100644 --- a/igvc_perception/launch/slam.launch +++ b/igvc_perception/launch/slam.launch @@ -18,7 +18,7 @@ - + diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index fd53eb9a8..4913b190d 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -163,6 +163,7 @@ void Slam::optimize() Vec3 curr_ang = accum_.deltaRij().rpy()/accum_.deltaTij(); auto odom_message = createOdomMsg(curr_pose, curr_vel, curr_ang); +// ROS_INFO_STREAM("IMU MSG: x: " << odom_message.pose.pose.position.x); location_pub_.publish(odom_message); updateTransform(odom_message); } @@ -262,8 +263,8 @@ nav_msgs::Odometry Slam::createOdomMsg(const gtsam::Pose3 &pos, const gtsam::Vec { nav_msgs::Odometry msg; msg.header.stamp = ros::Time::now(); - msg.child_frame_id = "/base_footprint"; - msg.header.frame_id = "/odom"; + msg.child_frame_id = "base_footprint"; + msg.header.frame_id = "odom"; msg.pose.pose = Conversion::gtsamPose3ToPose3Msg(pos); msg.twist.twist.linear = Conversion::gtsamVector3ToVector3Msg(vel); msg.twist.twist.angular = Conversion::gtsamVector3ToVector3Msg(ang); From f44172c0d7bc06062ae91d2cc724ded26d74ca20 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Fri, 16 Apr 2021 23:47:13 -0400 Subject: [PATCH 26/55] ekf works correctly now --- igvc_description/urdf/jessii.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/igvc_description/urdf/jessii.urdf.xacro b/igvc_description/urdf/jessii.urdf.xacro index ef0c16bbb..407423ef6 100644 --- a/igvc_description/urdf/jessii.urdf.xacro +++ b/igvc_description/urdf/jessii.urdf.xacro @@ -619,7 +619,7 @@ 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 - 1.57079632679 + 0 From c23efe0605fccba664094ed22b0d6e4b91f2f8d0 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Thu, 6 May 2021 18:39:32 -0400 Subject: [PATCH 27/55] fixed navsat problem but I have no clue why this works --- igvc_perception/launch/slam.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/igvc_perception/launch/slam.launch b/igvc_perception/launch/slam.launch index 2e88e6019..083ff2187 100644 --- a/igvc_perception/launch/slam.launch +++ b/igvc_perception/launch/slam.launch @@ -30,7 +30,7 @@ - + From cfa49d110e41fdd3149eaeb24fc78f7ed63243be Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Sun, 23 May 2021 13:26:28 -0400 Subject: [PATCH 28/55] Removed navsat_transform_node stuff and integrated raw gps and wheel odom measurements directly. Weird stuff happening with gps still. --- igvc_perception/include/slam/slam.h | 24 +++- .../include/slam/type_conversions.h | 2 + igvc_perception/launch/slam.launch | 48 +++---- igvc_perception/src/slam/CMakeLists.txt | 8 +- igvc_perception/src/slam/slam.cpp | 127 ++++++++++++++++-- igvc_perception/src/slam/type_conversions.cpp | 7 + 6 files changed, 175 insertions(+), 41 deletions(-) diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index 29841a191..aec9b1067 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -3,6 +3,7 @@ #include #include +#include // Pose2 == (Point3, Rot3) #include // PriorFactor == Initial Pose @@ -32,6 +33,9 @@ #include #include #include +// Convert between geodetic coordinates to local cartesian coordinates +#include +#include // Uncomment to enable debugging and have additional print statements //#define _DEBUG 1 @@ -48,13 +52,17 @@ class Slam ros::Subscriber imu_sub_; ros::Subscriber gps_sub_; ros::Subscriber mag_sub_; + ros::Subscriber wheel_sub_; ros::Publisher location_pub_; + ros::Publisher gps_location_pub_; - void gpsCallback(const nav_msgs::Odometry &msg); + void gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps); void imuCallback(const sensor_msgs::Imu &msg); void magCallback(const sensor_msgs::MagneticField &msg); void integrateAndAddIMUFactor(); + void wheelOdomCallback(const nav_msgs::Odometry &msg); + void addWheelOdomFactor(); void optimize(); void initializeImuParams(); void initializePriors(); @@ -63,6 +71,7 @@ class Slam void addMagFactor(); void updateTransform(const nav_msgs::Odometry &pos); static nav_msgs::Odometry createOdomMsg(const gtsam::Pose3 &pos, const gtsam::Vector3 &vel, const gtsam::Vector3 &ang); + geometry_msgs::TransformStamped getTransform(const std::string &frame, const ros::Time &stamp) const; // Defining some types typedef gtsam::noiseModel::Diagonal noiseDiagonal; @@ -74,17 +83,26 @@ class Slam gtsam::Values history_; #endif + // Transform from base_link to odom + tf2_ros::Buffer buffer_; + tf2_ros::TransformListener listener_ = tf2_ros::TransformListener(buffer_); + std::string target_frame_ = "odom"; + geometry_msgs::TransformStamped transformToOdom; + gtsam::NonlinearFactorGraph graph_; tf2_ros::TransformBroadcaster world_transform_broadcaster_; unsigned long curr_index_; - noiseDiagonal::shared_ptr gps_noise_, bias_noise_, mag_noise_; + noiseDiagonal::shared_ptr gps_noise_, bias_noise_, mag_noise_, odometryNoise; gtsam::Unit3 local_mag_field_; gtsam::Point3 curr_mag_reading_; + gtsam::Pose2 curr_wheelOdom_reading_; gtsam::ISAM2 isam_; gtsam::PreintegratedImuMeasurements accum_; ros::Time last_imu_measurement_; - bool imu_connected_, imu_update_available_; + bool imu_connected_, imu_update_available_, firstReading; double scale_; +// const GeographicLib::Geocentric& kWGS84; + GeographicLib::LocalCartesian origin_ENU; const double KGRAVITY = 9.81; }; diff --git a/igvc_perception/include/slam/type_conversions.h b/igvc_perception/include/slam/type_conversions.h index e162a369c..0b69105d5 100644 --- a/igvc_perception/include/slam/type_conversions.h +++ b/igvc_perception/include/slam/type_conversions.h @@ -5,12 +5,14 @@ #include // Pose2 == (Point3, Rot3) #include +#include #include class Conversion { public: static gtsam::Pose3 odomMsgToGtsamPose3(const nav_msgs::Odometry &msg); + static gtsam::Pose2 odomMsgToGtsamPose2(const nav_msgs::Odometry &msg); static gtsam::Point3 odomMsgToGtsamPoint3(const nav_msgs::Odometry &msg); static geometry_msgs::Vector3 gtsamVector3ToVector3Msg(const gtsam::Vector3 &vec); static geometry_msgs::Pose gtsamPose3ToPose3Msg(const gtsam::Pose3 &pos); diff --git a/igvc_perception/launch/slam.launch b/igvc_perception/launch/slam.launch index 083ff2187..03bea4a6a 100644 --- a/igvc_perception/launch/slam.launch +++ b/igvc_perception/launch/slam.launch @@ -4,41 +4,41 @@ - + - + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - - - + + + + - - + + diff --git a/igvc_perception/src/slam/CMakeLists.txt b/igvc_perception/src/slam/CMakeLists.txt index 3f1b169dd..b4784d8eb 100644 --- a/igvc_perception/src/slam/CMakeLists.txt +++ b/igvc_perception/src/slam/CMakeLists.txt @@ -8,6 +8,10 @@ list(APPEND NEEDED_LIBRARIES ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY} find_package(Eigen3 REQUIRED) include_directories(${EIGEN3_INCLUDE_DIR}) +# Find GeographicLib helpful: "sudo ln -s /usr/share/cmake/geographiclib/FindGeographicLib.cmake /usr/share/cmake-3.16/Modules/" +find_package(GeographicLib REQUIRED) +include_directories(${GeographicLib_INCLUDE_DIR}) + # Find GTSAM components find_package(GTSAM REQUIRED) # Uses installed package if(GTSAM_FOUND) @@ -20,8 +24,8 @@ if(GTSAM_FOUND) slam.cpp type_conversions.cpp) target_include_directories(slam PRIVATE ${catkin_INCLUDE_DIRS}) - target_link_libraries(slam gtsam ${catkin_LIBRARIES} Eigen3::Eigen ${Boost_LIBRARIES}) + target_link_libraries(slam gtsam ${catkin_LIBRARIES} Eigen3::Eigen ${Boost_LIBRARIES} ${GeographicLib_LIBRARIES}) add_dependencies(slam ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) else() message(WARNING "Could not find gtsam. Not compiling slam!") -endif() +endif() \ No newline at end of file diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 4913b190d..76621f8b2 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -1,9 +1,9 @@ #include // Using statements -using gtsam::symbol_shorthand::B; -using gtsam::symbol_shorthand::V; -using gtsam::symbol_shorthand::X; +using gtsam::symbol_shorthand::B; // bias measurements +using gtsam::symbol_shorthand::V; // velocity measurements +using gtsam::symbol_shorthand::X; // pose measurements /** * Initializes the factor graph and all the subscribers and publishers @@ -11,13 +11,18 @@ using gtsam::symbol_shorthand::X; Slam::Slam() : pnh_{ "~" } { imu_sub_ = pnh_.subscribe("/imu", 100, &Slam::imuCallback, this); - gps_sub_ = pnh_.subscribe("/odometry/gps", 1, &Slam::gpsCallback, this); - mag_sub_ = pnh_.subscribe("/magnetometer_mag", 1, &Slam::magCallback, this); + gps_sub_ = pnh_.subscribe("/fix", 1, &Slam::gpsCallback, this); + wheel_sub_ = pnh_.subscribe("/wheel_odometry", 1, &Slam::wheelOdomCallback, this); + mag_sub_ = pnh_.subscribe("/magnetometer_mag", 100, &Slam::magCallback, this); + location_pub_ = pnh_.advertise("/slam/position", 1); + gps_location_pub_ = pnh_.advertise("/slam_gps", 1); +// tf2_filter_ = tf2_ros::MessageFilter(buffer_, target_frame_, 10, pnh_); curr_index_ = 0; imu_connected_ = false; imu_update_available_ = false; + transformToOdom = getTransform(target_frame_, ros::Time::now()); initializeDirectionOfLocalMagField(); initializeNoiseMatrices(); @@ -27,12 +32,58 @@ Slam::Slam() : pnh_{ "~" } /** * The gpsCallback adds GPS measurements to the factor graph - * @param msg An odometery message derived from GPS measurements (published by navsat_transform_node) + * @param gps A NavSatFix message derived from GPS measurements (published by /fix) */ -void Slam::gpsCallback(const nav_msgs::Odometry &msg) -{ - gtsam::Point3 curr_point = Conversion::odomMsgToGtsamPoint3(msg); - gtsam::GPSFactor gps_factor(X(curr_index_ + 1), curr_point, gps_noise_); +void Slam::gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps) { + // get the starting location as the origin + double lat, lon, h, E, N, U; + lat = gps->latitude; + lon = gps->longitude; + h = gps->altitude; + ROS_INFO_STREAM("gps measurement number: " << gps->header.seq); + if (firstReading) { + const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274; + GeographicLib::LocalCartesian test_ENU = GeographicLib::LocalCartesian(lat0, lon0, h0, GeographicLib::Geocentric::WGS84()); + test_ENU.Forward(33.87071, -84.30482, 274, E, N, U); + ROS_INFO_STREAM("TEST gps_factor reading: " << E << ", " << N << ", " << U); + + origin_ENU = GeographicLib::LocalCartesian(lat, lon, h, GeographicLib::Geocentric::WGS84()); + firstReading = false; +// origin_ENU = GeographicLib::LocalCartesian(gps->latitude, gps->longitude, gps->altitude, GeographicLib::Geocentric::WGS84()); + } + origin_ENU.Forward(lat, lon, h, E, N, U); + + geometry_msgs::Quaternion quat = transformToOdom.transform.rotation; + gtsam::Rot3 global_to_local = gtsam::Rot3::Quaternion(quat.x, quat.y, quat.z, quat.w).inverse(); + +// gtsam::Rot3 global_to_local = gtsam::Rot3::Quaternion(0,0,-.707,.707); + +// Vec3 measured_gps = Vec3(E, N, U); +// Vec3 curr_gps = global_to_local * measured_gps; + gtsam::Point3 measured_gps = gtsam::Point3(E, N, U); + gtsam::Point3 curr_gps = global_to_local.rotate(measured_gps); + + nav_msgs::Odometry odom; + odom.header.stamp = ros::Time::now(); + odom.header.frame_id = "odom"; + + //set the position +// odom.pose.pose.position.x = curr_gps.x(); +// odom.pose.pose.position.y = curr_gps.y(); +// odom.pose.pose.position.z = curr_gps.z(); + odom.pose.pose.position.x = E; + odom.pose.pose.position.y = N; + odom.pose.pose.position.z = U; + gps_location_pub_.publish(odom); + +// origin_ENU.Forward(gps->latitude, gps->longitude, gps->altitude, E, N, U); +// gtsam::Point3 curr_point = Conversion::odomMsgToGtsamPoint3(msg); +// gtsam::GPSFactor gps_factor(X(curr_index_ + 1), curr_point, gps_noise_); +// ROS_INFO_STREAM("gps_latlon reading: " << lat << ", " << lon << ", " << h); +// ROS_INFO_STREAM("gps_pointer reading: " << gps->latitude << ", " << gps->longitude << ", " << gps->altitude); + ROS_INFO_STREAM("gps_factor reading: " << E << ", " << N << ", " << U); +// gtsam::GPSFactor gps_factor(X(curr_index_ + 1), gtsam::Point3(curr_gps.x(), curr_gps.y(), curr_gps.z()), gps_noise_); + gtsam::GPSFactor gps_factor(X(curr_index_ + 1), gtsam::Point3(E, N, U), gps_noise_); graph_.add(gps_factor); addMagFactor(); @@ -74,6 +125,9 @@ void Slam::imuCallback(const sensor_msgs::Imu &msg) */ void Slam::magCallback(const sensor_msgs::MagneticField &msg) { +// gtsam::Rot3 quat = gtsam::Rot3::Quaternion(0,0,.707,.707); +// gtsam::Point3 curr = gtsam::Point3(msg.magnetic_field.x, msg.magnetic_field.y, msg.magnetic_field.z); +// curr_mag_reading_ = quat.rotate(curr); curr_mag_reading_ = gtsam::Point3(msg.magnetic_field.x, msg.magnetic_field.y, msg.magnetic_field.z); } @@ -118,14 +172,37 @@ void Slam::integrateAndAddIMUFactor() accum_.resetIntegration(); } +/** + * The magCallback updates the curr_mag_reading_ with its most recent value + * @param msg An MagneticField sensor message (published by yostlab_driver_node) + */ +void Slam::wheelOdomCallback(const nav_msgs::Odometry &msg) +{ + curr_wheelOdom_reading_ = Conversion::odomMsgToGtsamPose2(msg); + ROS_INFO_STREAM("Wheel odom reading: " << curr_wheelOdom_reading_.x() << ", " << curr_wheelOdom_reading_.y() << ", " << curr_wheelOdom_reading_.theta()); +} + +/** + * Adds the magFactor to the factor graph + */ +void Slam::addWheelOdomFactor() +{ +// gtsam::BetweenFactor odomFactor = gtsam::BetweenFactor(X(curr_index_), X(curr_index_ + 1), curr_wheelOdom_reading_, odometryNoise); + graph_.add(gtsam::BetweenFactor(X(curr_index_), X(curr_index_ + 1), curr_wheelOdom_reading_, odometryNoise)); +// gtsam::OdometryFactorBase odomFactor = gtsam::OdometryFactorBase() +// MagPoseFactor mag_factor(X(curr_index_ + 1), curr_mag_reading_, scale_, +// local_mag_field_, gtsam::Point3(1e-9, 1e-9, 1e-9),mag_noise_); +// graph_.add(mag_factor); +} + /** * Triggers ISAM2 to optimize the current graph and publish the current estimated pose */ void Slam::optimize() { static int iteration = 0; -// ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ -// << " curr_index: " << curr_index_); + ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ + << " curr_index: " << curr_index_); // Update ISAM graph with new factors and estimates isam_.update(graph_, init_estimate_); @@ -210,6 +287,7 @@ void Slam::initializePriors() ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on x0"); #endif init_estimate_.insert(X(0), priorPose); + firstReading = true; // Adding Initial Velocity (Pose + Covariance Matrix) Vec3 priorVel(0.0, 0.0, 0.0); @@ -243,7 +321,9 @@ void Slam::initializeNoiseMatrices() double gps_z_noise = pnh_.param("gpsZNoiseConstant", 0.15); double mag_noise = pnh_.param("magNoiseConstant", 0.00000005); gps_noise_ = noiseDiagonal::Sigmas(Vec3(gps_xy_noise, gps_xy_noise, gps_z_noise)); +// gps_noise_ = gtsam::noiseModel::Isotropic::Sigma(3, 0.25); mag_noise_ = noiseDiagonal::Sigmas(Vec3::Constant(mag_noise)); + odometryNoise = noiseDiagonal::Sigmas(Vec3(0.2, 0.2, 0.1)); bias_noise_ = noiseDiagonal::Sigmas(gtsam::Vector6::Constant(bias_noise)); } @@ -269,4 +349,27 @@ nav_msgs::Odometry Slam::createOdomMsg(const gtsam::Pose3 &pos, const gtsam::Vec msg.twist.twist.linear = Conversion::gtsamVector3ToVector3Msg(vel); msg.twist.twist.angular = Conversion::gtsamVector3ToVector3Msg(ang); return msg; +} + +geometry_msgs::TransformStamped Slam::getTransform(const std::string &frame, const ros::Time &stamp) const +{ +// geometry_msgs::TransformStamped transformStamped; +// try{ +// transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", +// ros::Time(0)); +// } +// catch (tf2::TransformException &ex) { +// ROS_WARN("%s",ex.what()); +// ros::Duration(1.0).sleep(); +// continue; +// } + + if (!buffer_.canTransform("odom", frame, stamp, ros::Duration{ 1 })) + { + ROS_WARN_STREAM_THROTTLE(1.0, "Failed to find transform from frame 'odom' to frame 'base_link' within " + "timeout. Using latest transform..."); + return buffer_.lookupTransform("odom", frame, ros::Time{ 0 }, ros::Duration{ 1 }); + } + + return buffer_.lookupTransform("odom", frame, stamp, ros::Duration{ 1 }); } \ No newline at end of file diff --git a/igvc_perception/src/slam/type_conversions.cpp b/igvc_perception/src/slam/type_conversions.cpp index e7370d360..ca96adda3 100644 --- a/igvc_perception/src/slam/type_conversions.cpp +++ b/igvc_perception/src/slam/type_conversions.cpp @@ -7,6 +7,13 @@ gtsam::Pose3 Conversion::odomMsgToGtsamPose3(const nav_msgs::Odometry &msg) gtsam::Point3(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z)); } +gtsam::Pose2 Conversion::odomMsgToGtsamPose2(const nav_msgs::Odometry &msg) +{ + gtsam::Rot3 twist = gtsam::Rot3(msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z); + + return gtsam::Pose2(twist.yaw(), gtsam::Point2(msg.pose.pose.position.x, msg.pose.pose.position.y)); +} + geometry_msgs::Pose Conversion::gtsamPose3ToPose3Msg(const gtsam::Pose3 &pos){ geometry_msgs::Pose gPos; gPos.position.x = pos.x(); From feee90bf6339e53999914281c0665ab231c70465 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Thu, 3 Jun 2021 23:34:33 -0400 Subject: [PATCH 29/55] slam finally working! Realized that there was a problem with the hector gazebo plugin which was producing the wrong latitudes and longitudes in the simulation. The wrong behavior was that along the +X axis (East) latitude increased and along the +Y axis (North) longitude decreased. Instead we needed for +X to increase longitude and +Y to increase latitude. Thus I had to set the reference_heading in the jessii.urdf.xacro file to be 90 to fix this issue. Weird how its been going on for so long. Surprised nobody caught this for so long.:: --- igvc_description/urdf/jessii.urdf.xacro | 1 + igvc_perception/include/slam/slam.h | 7 --- igvc_perception/src/slam/slam.cpp | 70 ++----------------------- 3 files changed, 6 insertions(+), 72 deletions(-) diff --git a/igvc_description/urdf/jessii.urdf.xacro b/igvc_description/urdf/jessii.urdf.xacro index 407423ef6..5460288ae 100644 --- a/igvc_description/urdf/jessii.urdf.xacro +++ b/igvc_description/urdf/jessii.urdf.xacro @@ -644,6 +644,7 @@ /fix_velocity 33.774497 -84.405001 + 90 309.0 0.001 0.001 0.001 0.0001 0.0001 0.0001 diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index aec9b1067..c6f87fc7f 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -71,7 +71,6 @@ class Slam void addMagFactor(); void updateTransform(const nav_msgs::Odometry &pos); static nav_msgs::Odometry createOdomMsg(const gtsam::Pose3 &pos, const gtsam::Vector3 &vel, const gtsam::Vector3 &ang); - geometry_msgs::TransformStamped getTransform(const std::string &frame, const ros::Time &stamp) const; // Defining some types typedef gtsam::noiseModel::Diagonal noiseDiagonal; @@ -83,12 +82,6 @@ class Slam gtsam::Values history_; #endif - // Transform from base_link to odom - tf2_ros::Buffer buffer_; - tf2_ros::TransformListener listener_ = tf2_ros::TransformListener(buffer_); - std::string target_frame_ = "odom"; - geometry_msgs::TransformStamped transformToOdom; - gtsam::NonlinearFactorGraph graph_; tf2_ros::TransformBroadcaster world_transform_broadcaster_; unsigned long curr_index_; diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 76621f8b2..3156dccfa 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -18,11 +18,9 @@ Slam::Slam() : pnh_{ "~" } location_pub_ = pnh_.advertise("/slam/position", 1); gps_location_pub_ = pnh_.advertise("/slam_gps", 1); -// tf2_filter_ = tf2_ros::MessageFilter(buffer_, target_frame_, 10, pnh_); curr_index_ = 0; imu_connected_ = false; imu_update_available_ = false; - transformToOdom = getTransform(target_frame_, ros::Time::now()); initializeDirectionOfLocalMagField(); initializeNoiseMatrices(); @@ -40,49 +38,23 @@ void Slam::gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps) { lat = gps->latitude; lon = gps->longitude; h = gps->altitude; - ROS_INFO_STREAM("gps measurement number: " << gps->header.seq); +// ROS_INFO_STREAM("gps measurement number: " << gps->header.seq); if (firstReading) { - const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274; - GeographicLib::LocalCartesian test_ENU = GeographicLib::LocalCartesian(lat0, lon0, h0, GeographicLib::Geocentric::WGS84()); - test_ENU.Forward(33.87071, -84.30482, 274, E, N, U); - ROS_INFO_STREAM("TEST gps_factor reading: " << E << ", " << N << ", " << U); - origin_ENU = GeographicLib::LocalCartesian(lat, lon, h, GeographicLib::Geocentric::WGS84()); firstReading = false; -// origin_ENU = GeographicLib::LocalCartesian(gps->latitude, gps->longitude, gps->altitude, GeographicLib::Geocentric::WGS84()); } origin_ENU.Forward(lat, lon, h, E, N, U); - geometry_msgs::Quaternion quat = transformToOdom.transform.rotation; - gtsam::Rot3 global_to_local = gtsam::Rot3::Quaternion(quat.x, quat.y, quat.z, quat.w).inverse(); - -// gtsam::Rot3 global_to_local = gtsam::Rot3::Quaternion(0,0,-.707,.707); - -// Vec3 measured_gps = Vec3(E, N, U); -// Vec3 curr_gps = global_to_local * measured_gps; - gtsam::Point3 measured_gps = gtsam::Point3(E, N, U); - gtsam::Point3 curr_gps = global_to_local.rotate(measured_gps); - nav_msgs::Odometry odom; odom.header.stamp = ros::Time::now(); odom.header.frame_id = "odom"; - //set the position -// odom.pose.pose.position.x = curr_gps.x(); -// odom.pose.pose.position.y = curr_gps.y(); -// odom.pose.pose.position.z = curr_gps.z(); + // set the position odom.pose.pose.position.x = E; odom.pose.pose.position.y = N; odom.pose.pose.position.z = U; gps_location_pub_.publish(odom); -// origin_ENU.Forward(gps->latitude, gps->longitude, gps->altitude, E, N, U); -// gtsam::Point3 curr_point = Conversion::odomMsgToGtsamPoint3(msg); -// gtsam::GPSFactor gps_factor(X(curr_index_ + 1), curr_point, gps_noise_); -// ROS_INFO_STREAM("gps_latlon reading: " << lat << ", " << lon << ", " << h); -// ROS_INFO_STREAM("gps_pointer reading: " << gps->latitude << ", " << gps->longitude << ", " << gps->altitude); - ROS_INFO_STREAM("gps_factor reading: " << E << ", " << N << ", " << U); -// gtsam::GPSFactor gps_factor(X(curr_index_ + 1), gtsam::Point3(curr_gps.x(), curr_gps.y(), curr_gps.z()), gps_noise_); gtsam::GPSFactor gps_factor(X(curr_index_ + 1), gtsam::Point3(E, N, U), gps_noise_); graph_.add(gps_factor); @@ -125,9 +97,6 @@ void Slam::imuCallback(const sensor_msgs::Imu &msg) */ void Slam::magCallback(const sensor_msgs::MagneticField &msg) { -// gtsam::Rot3 quat = gtsam::Rot3::Quaternion(0,0,.707,.707); -// gtsam::Point3 curr = gtsam::Point3(msg.magnetic_field.x, msg.magnetic_field.y, msg.magnetic_field.z); -// curr_mag_reading_ = quat.rotate(curr); curr_mag_reading_ = gtsam::Point3(msg.magnetic_field.x, msg.magnetic_field.y, msg.magnetic_field.z); } @@ -179,7 +148,7 @@ void Slam::integrateAndAddIMUFactor() void Slam::wheelOdomCallback(const nav_msgs::Odometry &msg) { curr_wheelOdom_reading_ = Conversion::odomMsgToGtsamPose2(msg); - ROS_INFO_STREAM("Wheel odom reading: " << curr_wheelOdom_reading_.x() << ", " << curr_wheelOdom_reading_.y() << ", " << curr_wheelOdom_reading_.theta()); +// ROS_INFO_STREAM("Wheel odom reading: " << curr_wheelOdom_reading_.x() << ", " << curr_wheelOdom_reading_.y() << ", " << curr_wheelOdom_reading_.theta()); } /** @@ -187,12 +156,7 @@ void Slam::wheelOdomCallback(const nav_msgs::Odometry &msg) */ void Slam::addWheelOdomFactor() { -// gtsam::BetweenFactor odomFactor = gtsam::BetweenFactor(X(curr_index_), X(curr_index_ + 1), curr_wheelOdom_reading_, odometryNoise); graph_.add(gtsam::BetweenFactor(X(curr_index_), X(curr_index_ + 1), curr_wheelOdom_reading_, odometryNoise)); -// gtsam::OdometryFactorBase odomFactor = gtsam::OdometryFactorBase() -// MagPoseFactor mag_factor(X(curr_index_ + 1), curr_mag_reading_, scale_, -// local_mag_field_, gtsam::Point3(1e-9, 1e-9, 1e-9),mag_noise_); -// graph_.add(mag_factor); } /** @@ -201,8 +165,8 @@ void Slam::addWheelOdomFactor() void Slam::optimize() { static int iteration = 0; - ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ - << " curr_index: " << curr_index_); +// ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ +// << " curr_index: " << curr_index_); // Update ISAM graph with new factors and estimates isam_.update(graph_, init_estimate_); @@ -321,7 +285,6 @@ void Slam::initializeNoiseMatrices() double gps_z_noise = pnh_.param("gpsZNoiseConstant", 0.15); double mag_noise = pnh_.param("magNoiseConstant", 0.00000005); gps_noise_ = noiseDiagonal::Sigmas(Vec3(gps_xy_noise, gps_xy_noise, gps_z_noise)); -// gps_noise_ = gtsam::noiseModel::Isotropic::Sigma(3, 0.25); mag_noise_ = noiseDiagonal::Sigmas(Vec3::Constant(mag_noise)); odometryNoise = noiseDiagonal::Sigmas(Vec3(0.2, 0.2, 0.1)); bias_noise_ = noiseDiagonal::Sigmas(gtsam::Vector6::Constant(bias_noise)); @@ -349,27 +312,4 @@ nav_msgs::Odometry Slam::createOdomMsg(const gtsam::Pose3 &pos, const gtsam::Vec msg.twist.twist.linear = Conversion::gtsamVector3ToVector3Msg(vel); msg.twist.twist.angular = Conversion::gtsamVector3ToVector3Msg(ang); return msg; -} - -geometry_msgs::TransformStamped Slam::getTransform(const std::string &frame, const ros::Time &stamp) const -{ -// geometry_msgs::TransformStamped transformStamped; -// try{ -// transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", -// ros::Time(0)); -// } -// catch (tf2::TransformException &ex) { -// ROS_WARN("%s",ex.what()); -// ros::Duration(1.0).sleep(); -// continue; -// } - - if (!buffer_.canTransform("odom", frame, stamp, ros::Duration{ 1 })) - { - ROS_WARN_STREAM_THROTTLE(1.0, "Failed to find transform from frame 'odom' to frame 'base_link' within " - "timeout. Using latest transform..."); - return buffer_.lookupTransform("odom", frame, ros::Time{ 0 }, ros::Duration{ 1 }); - } - - return buffer_.lookupTransform("odom", frame, stamp, ros::Duration{ 1 }); } \ No newline at end of file From d5d2850f5f1aebec04170c659751d6b97c491776 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Fri, 4 Jun 2021 10:02:36 -0400 Subject: [PATCH 30/55] formatted using make format --- igvc_perception/include/slam/MagPoseFactor.h | 12 ++- igvc_perception/include/slam/slam.h | 7 +- .../include/slam/type_conversions.h | 2 +- igvc_perception/src/slam/slam.cpp | 95 +++++++++++-------- igvc_perception/src/slam/type_conversions.cpp | 11 ++- 5 files changed, 72 insertions(+), 55 deletions(-) diff --git a/igvc_perception/include/slam/MagPoseFactor.h b/igvc_perception/include/slam/MagPoseFactor.h index 75f85019c..650863a2a 100644 --- a/igvc_perception/include/slam/MagPoseFactor.h +++ b/igvc_perception/include/slam/MagPoseFactor.h @@ -46,7 +46,8 @@ class MagPoseFactor : public gtsam::NoiseModelFactor1 gtsam::Point3 hx = nRb.rotation().unrotate(nM_, H, boost::none) + bias_; // H is 3x3 from above unrotate operation, but needs to be 3x6, ie. set the 3x3 block on the right // to all zeros. - if (H) { + if (H) + { H->conservativeResize(3, 6); H->rightCols(3).setZero(); } @@ -54,10 +55,11 @@ class MagPoseFactor : public gtsam::NoiseModelFactor1 return error; } - void print(const std::string& s, const gtsam::KeyFormatter& keyFormatter) const override { - std::cout << s << "MagPoseFactor on " << keyFormatter(key()) << "\n"; - std::cout << s << " Mag measurement: " << measured_.transpose() << std::endl; - noiseModel_->print(" noise model: "); + void print(const std::string& s, const gtsam::KeyFormatter& keyFormatter) const override + { + std::cout << s << "MagPoseFactor on " << keyFormatter(key()) << "\n"; + std::cout << s << " Mag measurement: " << measured_.transpose() << std::endl; + noiseModel_->print(" noise model: "); } }; diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index c6f87fc7f..d5c7696f2 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -57,7 +57,7 @@ class Slam ros::Publisher location_pub_; ros::Publisher gps_location_pub_; - void gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps); + void gpsCallback(const sensor_msgs::NavSatFixConstPtr &gps); void imuCallback(const sensor_msgs::Imu &msg); void magCallback(const sensor_msgs::MagneticField &msg); void integrateAndAddIMUFactor(); @@ -70,7 +70,8 @@ class Slam void initializeDirectionOfLocalMagField(); void addMagFactor(); void updateTransform(const nav_msgs::Odometry &pos); - static nav_msgs::Odometry createOdomMsg(const gtsam::Pose3 &pos, const gtsam::Vector3 &vel, const gtsam::Vector3 &ang); + static nav_msgs::Odometry createOdomMsg(const gtsam::Pose3 &pos, const gtsam::Vector3 &vel, + const gtsam::Vector3 &ang); // Defining some types typedef gtsam::noiseModel::Diagonal noiseDiagonal; @@ -94,7 +95,7 @@ class Slam ros::Time last_imu_measurement_; bool imu_connected_, imu_update_available_, firstReading; double scale_; -// const GeographicLib::Geocentric& kWGS84; + // const GeographicLib::Geocentric& kWGS84; GeographicLib::LocalCartesian origin_ENU; const double KGRAVITY = 9.81; diff --git a/igvc_perception/include/slam/type_conversions.h b/igvc_perception/include/slam/type_conversions.h index 0b69105d5..7281e0ed3 100644 --- a/igvc_perception/include/slam/type_conversions.h +++ b/igvc_perception/include/slam/type_conversions.h @@ -14,7 +14,7 @@ class Conversion static gtsam::Pose3 odomMsgToGtsamPose3(const nav_msgs::Odometry &msg); static gtsam::Pose2 odomMsgToGtsamPose2(const nav_msgs::Odometry &msg); static gtsam::Point3 odomMsgToGtsamPoint3(const nav_msgs::Odometry &msg); - static geometry_msgs::Vector3 gtsamVector3ToVector3Msg(const gtsam::Vector3 &vec); + static geometry_msgs::Vector3 gtsamVector3ToVector3Msg(const gtsam::Vector3 &vec); static geometry_msgs::Pose gtsamPose3ToPose3Msg(const gtsam::Pose3 &pos); }; diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 3156dccfa..515cb6d2e 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -1,9 +1,9 @@ #include // Using statements -using gtsam::symbol_shorthand::B; // bias measurements -using gtsam::symbol_shorthand::V; // velocity measurements -using gtsam::symbol_shorthand::X; // pose measurements +using gtsam::symbol_shorthand::B; // bias measurements +using gtsam::symbol_shorthand::V; // velocity measurements +using gtsam::symbol_shorthand::X; // pose measurements /** * Initializes the factor graph and all the subscribers and publishers @@ -16,7 +16,7 @@ Slam::Slam() : pnh_{ "~" } mag_sub_ = pnh_.subscribe("/magnetometer_mag", 100, &Slam::magCallback, this); location_pub_ = pnh_.advertise("/slam/position", 1); - gps_location_pub_ = pnh_.advertise("/slam_gps", 1); + gps_location_pub_ = pnh_.advertise("/odometry/gps", 1); curr_index_ = 0; imu_connected_ = false; @@ -32,30 +32,34 @@ Slam::Slam() : pnh_{ "~" } * The gpsCallback adds GPS measurements to the factor graph * @param gps A NavSatFix message derived from GPS measurements (published by /fix) */ -void Slam::gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps) { - // get the starting location as the origin - double lat, lon, h, E, N, U; - lat = gps->latitude; - lon = gps->longitude; - h = gps->altitude; -// ROS_INFO_STREAM("gps measurement number: " << gps->header.seq); - if (firstReading) { - origin_ENU = GeographicLib::LocalCartesian(lat, lon, h, GeographicLib::Geocentric::WGS84()); - firstReading = false; - } - origin_ENU.Forward(lat, lon, h, E, N, U); - - nav_msgs::Odometry odom; - odom.header.stamp = ros::Time::now(); - odom.header.frame_id = "odom"; - - // set the position - odom.pose.pose.position.x = E; - odom.pose.pose.position.y = N; - odom.pose.pose.position.z = U; - gps_location_pub_.publish(odom); - - gtsam::GPSFactor gps_factor(X(curr_index_ + 1), gtsam::Point3(E, N, U), gps_noise_); +void Slam::gpsCallback(const sensor_msgs::NavSatFixConstPtr &gps) +{ + // get the starting location as the origin + double lat, lon, h, E, N, U; + lat = gps->latitude; + lon = gps->longitude; + h = gps->altitude; +#if defined(_DEBUG) + ROS_INFO_STREAM("gps measurement number: " << gps->header.seq); +#endif + if (firstReading) + { + origin_ENU = GeographicLib::LocalCartesian(lat, lon, h, GeographicLib::Geocentric::WGS84()); + firstReading = false; + } + origin_ENU.Forward(lat, lon, h, E, N, U); + + nav_msgs::Odometry odom; + odom.header.stamp = ros::Time::now(); + odom.header.frame_id = "odom"; + + // set the position + odom.pose.pose.position.x = E; + odom.pose.pose.position.y = N; + odom.pose.pose.position.z = U; + gps_location_pub_.publish(odom); + + gtsam::GPSFactor gps_factor(X(curr_index_ + 1), gtsam::Point3(E, N, U), gps_noise_); graph_.add(gps_factor); addMagFactor(); @@ -105,8 +109,8 @@ void Slam::magCallback(const sensor_msgs::MagneticField &msg) */ void Slam::addMagFactor() { - MagPoseFactor mag_factor(X(curr_index_ + 1), curr_mag_reading_, scale_, - local_mag_field_, gtsam::Point3(1e-9, 1e-9, 1e-9),mag_noise_); + MagPoseFactor mag_factor(X(curr_index_ + 1), curr_mag_reading_, scale_, local_mag_field_, + gtsam::Point3(1e-9, 1e-9, 1e-9), mag_noise_); graph_.add(mag_factor); } @@ -124,8 +128,8 @@ void Slam::integrateAndAddIMUFactor() init_estimate_.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); // Add imu factor - gtsam::ImuFactor imufac(X(curr_index_), V(curr_index_), X(curr_index_ + 1), - V(curr_index_ + 1), B(curr_index_ + 1),accum_); + gtsam::ImuFactor imufac(X(curr_index_), V(curr_index_), X(curr_index_ + 1), V(curr_index_ + 1), B(curr_index_ + 1), + accum_); graph_.add(imufac); auto newPoseEstimate = result_.at(X(curr_index_)); // gtsam::Pose3 newPoseEstimate @@ -147,8 +151,11 @@ void Slam::integrateAndAddIMUFactor() */ void Slam::wheelOdomCallback(const nav_msgs::Odometry &msg) { - curr_wheelOdom_reading_ = Conversion::odomMsgToGtsamPose2(msg); -// ROS_INFO_STREAM("Wheel odom reading: " << curr_wheelOdom_reading_.x() << ", " << curr_wheelOdom_reading_.y() << ", " << curr_wheelOdom_reading_.theta()); + curr_wheelOdom_reading_ = Conversion::odomMsgToGtsamPose2(msg); +#if defined(_DEBUG) + ROS_INFO_STREAM("Wheel odom reading: " << curr_wheelOdom_reading_.x() << ", " << curr_wheelOdom_reading_.y() << ", " + << curr_wheelOdom_reading_.theta()); +#endif } /** @@ -156,7 +163,8 @@ void Slam::wheelOdomCallback(const nav_msgs::Odometry &msg) */ void Slam::addWheelOdomFactor() { - graph_.add(gtsam::BetweenFactor(X(curr_index_), X(curr_index_ + 1), curr_wheelOdom_reading_, odometryNoise)); + graph_.add( + gtsam::BetweenFactor(X(curr_index_), X(curr_index_ + 1), curr_wheelOdom_reading_, odometryNoise)); } /** @@ -165,12 +173,12 @@ void Slam::addWheelOdomFactor() void Slam::optimize() { static int iteration = 0; -// ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ -// << " curr_index: " << curr_index_); + // ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ + // << " curr_index: " << curr_index_); // Update ISAM graph with new factors and estimates isam_.update(graph_, init_estimate_); - #if defined(_DEBUG) +#if defined(_DEBUG) // Add initial estimates to history_ history_.insert(init_estimate_); graph_.printErrors(history_); @@ -180,7 +188,7 @@ void Slam::optimize() ROS_INFO_STREAM("isam_ graph"); isam_.getFactorsUnsafe().print(); - #endif +#endif result_ = isam_.calculateEstimate(); @@ -202,14 +210,17 @@ void Slam::optimize() auto global_to_local = curr_pose.rotation().toQuaternion().inverse(); curr_vel = global_to_local._transformVector(curr_vel); - Vec3 curr_ang = accum_.deltaRij().rpy()/accum_.deltaTij(); + Vec3 curr_ang = accum_.deltaRij().rpy() / accum_.deltaTij(); auto odom_message = createOdomMsg(curr_pose, curr_vel, curr_ang); -// ROS_INFO_STREAM("IMU MSG: x: " << odom_message.pose.pose.position.x); +#if defined(_DEBUG) + ROS_INFO_STREAM("IMU MSG: x: " << odom_message.pose.pose.position.x); +#endif location_pub_.publish(odom_message); updateTransform(odom_message); } -void Slam::updateTransform(const nav_msgs::Odometry &pos){ +void Slam::updateTransform(const nav_msgs::Odometry &pos) +{ geometry_msgs::TransformStamped tfLink; tfLink.header.stamp = pos.header.stamp; tfLink.header.frame_id = pos.header.frame_id; diff --git a/igvc_perception/src/slam/type_conversions.cpp b/igvc_perception/src/slam/type_conversions.cpp index ca96adda3..f1f17ea4a 100644 --- a/igvc_perception/src/slam/type_conversions.cpp +++ b/igvc_perception/src/slam/type_conversions.cpp @@ -9,12 +9,14 @@ gtsam::Pose3 Conversion::odomMsgToGtsamPose3(const nav_msgs::Odometry &msg) gtsam::Pose2 Conversion::odomMsgToGtsamPose2(const nav_msgs::Odometry &msg) { - gtsam::Rot3 twist = gtsam::Rot3(msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z); + gtsam::Rot3 twist = gtsam::Rot3(msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z); - return gtsam::Pose2(twist.yaw(), gtsam::Point2(msg.pose.pose.position.x, msg.pose.pose.position.y)); + return gtsam::Pose2(twist.yaw(), gtsam::Point2(msg.pose.pose.position.x, msg.pose.pose.position.y)); } -geometry_msgs::Pose Conversion::gtsamPose3ToPose3Msg(const gtsam::Pose3 &pos){ +geometry_msgs::Pose Conversion::gtsamPose3ToPose3Msg(const gtsam::Pose3 &pos) +{ geometry_msgs::Pose gPos; gPos.position.x = pos.x(); gPos.position.y = pos.y(); @@ -24,7 +26,8 @@ geometry_msgs::Pose Conversion::gtsamPose3ToPose3Msg(const gtsam::Pose3 &pos){ return gPos; } -geometry_msgs::Vector3 Conversion::gtsamVector3ToVector3Msg(const gtsam::Vector3 &vec){ +geometry_msgs::Vector3 Conversion::gtsamVector3ToVector3Msg(const gtsam::Vector3 &vec) +{ geometry_msgs::Vector3 gVec; gVec.x = vec.x(); gVec.y = vec.y(); From 22959d3383186051ff977c1ee75a75db618d1b7c Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Fri, 4 Jun 2021 10:23:13 -0400 Subject: [PATCH 31/55] removed navsat transform stuff from launch --- igvc_perception/launch/slam.launch | 36 ------------------------------ 1 file changed, 36 deletions(-) diff --git a/igvc_perception/launch/slam.launch b/igvc_perception/launch/slam.launch index 03bea4a6a..6125d26b7 100644 --- a/igvc_perception/launch/slam.launch +++ b/igvc_perception/launch/slam.launch @@ -4,42 +4,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 25e9f0a7228d994c06e3e8b150f705ed39944a3b Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Fri, 4 Jun 2021 22:47:18 -0400 Subject: [PATCH 32/55] fixed waypoints on autonav course since previous gps coordinates were incorrect due to gazebo gps plugin bug --- igvc_gazebo/config/waypoints_autonav_0.csv | 10 +++++----- igvc_gazebo/config/waypoints_autonav_1.csv | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/igvc_gazebo/config/waypoints_autonav_0.csv b/igvc_gazebo/config/waypoints_autonav_0.csv index 164e7ba1e..a6bc5d29f 100644 --- a/igvc_gazebo/config/waypoints_autonav_0.csv +++ b/igvc_gazebo/config/waypoints_autonav_0.csv @@ -1,5 +1,5 @@ -33.7746360976, -84.4047548331 -33.7745538754, -84.4048924451 -33.7744382483, -84.4048926838 -33.7743553358, -84.4047560492 -33.7744950498, -84.4052488655 \ No newline at end of file +33.7742906118346, -84.40482946425239 +33.7744032479795, -84.40493470388343 +33.7744015516094, -84.40507763991296 +33.7742906402805, -84.40517679775147 +33.7747034526282, -84.40500116144553 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_autonav_1.csv b/igvc_gazebo/config/waypoints_autonav_1.csv index 8dfe5de29..f1b77a699 100644 --- a/igvc_gazebo/config/waypoints_autonav_1.csv +++ b/igvc_gazebo/config/waypoints_autonav_1.csv @@ -1,5 +1,5 @@ -33.7743553358, -84.4047560492 -33.7744382483, -84.4048926838 -33.7745538754, -84.4048924451 -33.7746360976, -84.4047548331 -33.7744950498, -84.4052488655 \ No newline at end of file +33.7742906402805, -84.40517679775147 +33.7744015516094, -84.40507763991296 +33.7744032479795, -84.40493470388343 +33.7742906118346, -84.40482946425239 +33.7747034526282, -84.40500116144553 \ No newline at end of file From 521423c0f73de21218964234bd0c51b891309089 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Fri, 4 Jun 2021 22:50:15 -0400 Subject: [PATCH 33/55] Fixed problem with navsat transform node --- igvc_navigation/launch/localization.launch | 2 +- igvc_perception/include/slam/slam.h | 1 - igvc_perception/launch/slam.launch | 37 ++++++++++++++++++++++ 3 files changed, 38 insertions(+), 2 deletions(-) diff --git a/igvc_navigation/launch/localization.launch b/igvc_navigation/launch/localization.launch index 2d43efe71..6c49fe96e 100644 --- a/igvc_navigation/launch/localization.launch +++ b/igvc_navigation/launch/localization.launch @@ -23,7 +23,7 @@ - + diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index d5c7696f2..5059d4477 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -95,7 +95,6 @@ class Slam ros::Time last_imu_measurement_; bool imu_connected_, imu_update_available_, firstReading; double scale_; - // const GeographicLib::Geocentric& kWGS84; GeographicLib::LocalCartesian origin_ENU; const double KGRAVITY = 9.81; diff --git a/igvc_perception/launch/slam.launch b/igvc_perception/launch/slam.launch index 6125d26b7..a6a1735ee 100644 --- a/igvc_perception/launch/slam.launch +++ b/igvc_perception/launch/slam.launch @@ -4,6 +4,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From bca0dcde2985315f4976d2384232f7b307291465 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Sat, 5 Jun 2021 14:29:10 -0400 Subject: [PATCH 34/55] Added dependencies for slam to run --- .gitmodules | 3 +++ external/gtsam | 1 + igvc_perception/package.xml | 1 + 3 files changed, 5 insertions(+) create mode 160000 external/gtsam diff --git a/.gitmodules b/.gitmodules index 3a75aee72..15dad9275 100644 --- a/.gitmodules +++ b/.gitmodules @@ -14,3 +14,6 @@ path = external/elevation_mapping url = https://github.com/RoboJackets/elevation_mapping.git +[submodule "external/gtsam"] + path = external/gtsam + url = https://github.com/borglab/gtsam.git diff --git a/external/gtsam b/external/gtsam new file mode 160000 index 000000000..5c9e4136b --- /dev/null +++ b/external/gtsam @@ -0,0 +1 @@ +Subproject commit 5c9e4136b7deeb32b92e635e1fc03987d3f633a3 diff --git a/igvc_perception/package.xml b/igvc_perception/package.xml index 2815f2fec..3d4d78f7a 100644 --- a/igvc_perception/package.xml +++ b/igvc_perception/package.xml @@ -46,6 +46,7 @@ grid_map_core grid_map_ros grid_map_msgs + geographiclib image_transport cv_bridge From e6ad030183e298f024e2de42545aa3e6dc1a865f Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Sat, 5 Jun 2021 15:22:49 -0400 Subject: [PATCH 35/55] removed gtsam submodule --- .gitmodules | 3 --- external/gtsam | 1 - 2 files changed, 4 deletions(-) delete mode 160000 external/gtsam diff --git a/.gitmodules b/.gitmodules index 15dad9275..3a75aee72 100644 --- a/.gitmodules +++ b/.gitmodules @@ -14,6 +14,3 @@ path = external/elevation_mapping url = https://github.com/RoboJackets/elevation_mapping.git -[submodule "external/gtsam"] - path = external/gtsam - url = https://github.com/borglab/gtsam.git diff --git a/external/gtsam b/external/gtsam deleted file mode 160000 index 5c9e4136b..000000000 --- a/external/gtsam +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 5c9e4136b7deeb32b92e635e1fc03987d3f633a3 From 568cc464e4bb147a0ef3423f0fe05d908231fdeb Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Sat, 5 Jun 2021 15:35:08 -0400 Subject: [PATCH 36/55] Added gtsam and geographiclib installation commands --- install_dependencies.sh | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/install_dependencies.sh b/install_dependencies.sh index 79b3669b4..9866f68c4 100755 --- a/install_dependencies.sh +++ b/install_dependencies.sh @@ -12,3 +12,13 @@ fi rosdep install -iy --from-paths ../../src --skip-keys='kindr serial' pip3 install --no-cache-dir torch torchvision + +## GTSAM +# Add PPA +sudo add-apt-repository ppa:borglab/gtsam-release-4.0 +sudo apt update # not necessary since Bionic +# Install: +sudo apt install libgtsam-dev libgtsam-unstable-dev + +## GeographicLib +sudo apt-get install -y libgeographic-dev \ No newline at end of file From a2d274cf25275b57bc4b402fb6b1240527194b98 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Sat, 5 Jun 2021 15:39:19 -0400 Subject: [PATCH 37/55] Added link command because geographic lib doesnt install in the correct location --- install_dependencies.sh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/install_dependencies.sh b/install_dependencies.sh index 9866f68c4..52a0218ec 100755 --- a/install_dependencies.sh +++ b/install_dependencies.sh @@ -21,4 +21,5 @@ sudo apt update # not necessary since Bionic sudo apt install libgtsam-dev libgtsam-unstable-dev ## GeographicLib -sudo apt-get install -y libgeographic-dev \ No newline at end of file +sudo apt-get install -y libgeographic-dev +sudo ln -s /usr/share/cmake/geographiclib/FindGeographicLib.cmake /usr/share/cmake-3.16/Modules/ \ No newline at end of file From c3bcd38209756ed695b7596ff40c5b9fe8d3f327 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Mon, 19 Jul 2021 21:49:50 -0400 Subject: [PATCH 38/55] removed redundancies --- igvc_perception/package.xml | 1 - .../src/projection/.idea/workspace.xml | 94 ------------------- 2 files changed, 95 deletions(-) delete mode 100644 igvc_perception/src/projection/.idea/workspace.xml diff --git a/igvc_perception/package.xml b/igvc_perception/package.xml index 7fe98f5fa..3d4d78f7a 100644 --- a/igvc_perception/package.xml +++ b/igvc_perception/package.xml @@ -61,7 +61,6 @@ cv_bridge pcl_conversions image_geometry - geographiclib robot_localization pluginlib python-pytorch-pip diff --git a/igvc_perception/src/projection/.idea/workspace.xml b/igvc_perception/src/projection/.idea/workspace.xml deleted file mode 100644 index 4f1955cb6..000000000 --- a/igvc_perception/src/projection/.idea/workspace.xml +++ /dev/null @@ -1,94 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1572483525476 - - - - - - - - - \ No newline at end of file From 015b87f539f3d5d93eafe15ae278726d701e02f8 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Wed, 21 Jul 2021 01:08:26 -0400 Subject: [PATCH 39/55] remove install geographiclib --- external/velodyne | 1 - install_dependencies.sh | 6 +----- 2 files changed, 1 insertion(+), 6 deletions(-) delete mode 160000 external/velodyne diff --git a/external/velodyne b/external/velodyne deleted file mode 160000 index 720829573..000000000 --- a/external/velodyne +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 720829573076bf7d7edd4b1de344924fad909049 diff --git a/install_dependencies.sh b/install_dependencies.sh index 0254767a4..ae5f92ff2 100755 --- a/install_dependencies.sh +++ b/install_dependencies.sh @@ -19,8 +19,4 @@ pip3 install --no-cache-dir torch torchvision sudo add-apt-repository ppa:borglab/gtsam-release-4.0 sudo apt update # not necessary since Bionic # Install: -sudo apt install libgtsam-dev libgtsam-unstable-dev - -## GeographicLib -sudo apt-get install -y libgeographic-dev -sudo ln -s /usr/share/cmake/geographiclib/FindGeographicLib.cmake /usr/share/cmake-3.16/Modules/ \ No newline at end of file +sudo apt install libgtsam-dev libgtsam-unstable-dev \ No newline at end of file From 20aa54c7da9e5f81a9c867f4f042c9177c5d235c Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Thu, 22 Jul 2021 23:55:41 -0400 Subject: [PATCH 40/55] modified gtsam install --- install_dependencies.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/install_dependencies.sh b/install_dependencies.sh index ae5f92ff2..87c77ffd3 100755 --- a/install_dependencies.sh +++ b/install_dependencies.sh @@ -19,4 +19,4 @@ pip3 install --no-cache-dir torch torchvision sudo add-apt-repository ppa:borglab/gtsam-release-4.0 sudo apt update # not necessary since Bionic # Install: -sudo apt install libgtsam-dev libgtsam-unstable-dev \ No newline at end of file +sudo apt install libgtsam-dev libgtsam-unstable-dev -y \ No newline at end of file From fbd823a1b8827bcefd72b45e9fd169644dd4ef77 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Fri, 23 Jul 2021 00:03:17 -0400 Subject: [PATCH 41/55] modified gtsam install --- install_dependencies.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/install_dependencies.sh b/install_dependencies.sh index 87c77ffd3..48b58ba44 100755 --- a/install_dependencies.sh +++ b/install_dependencies.sh @@ -16,7 +16,7 @@ pip3 install --no-cache-dir torch torchvision ## GTSAM # Add PPA -sudo add-apt-repository ppa:borglab/gtsam-release-4.0 +sudo add-apt-repository -y ppa:borglab/gtsam-release-4.0 sudo apt update # not necessary since Bionic # Install: -sudo apt install libgtsam-dev libgtsam-unstable-dev -y \ No newline at end of file +sudo apt install -y libgtsam-dev libgtsam-unstable-dev \ No newline at end of file From 4084bbd1d999f6763b7e03c4745e7e7d97730f4c Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Fri, 23 Jul 2021 00:16:34 -0400 Subject: [PATCH 42/55] Modified CMakeLists to account for non standard geographiclib location --- igvc_perception/src/slam/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/igvc_perception/src/slam/CMakeLists.txt b/igvc_perception/src/slam/CMakeLists.txt index b4784d8eb..cce444459 100644 --- a/igvc_perception/src/slam/CMakeLists.txt +++ b/igvc_perception/src/slam/CMakeLists.txt @@ -9,6 +9,8 @@ find_package(Eigen3 REQUIRED) include_directories(${EIGEN3_INCLUDE_DIR}) # Find GeographicLib helpful: "sudo ln -s /usr/share/cmake/geographiclib/FindGeographicLib.cmake /usr/share/cmake-3.16/Modules/" +# Geographiclib installs FindGeographicLib.cmake to this non-standard location +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") find_package(GeographicLib REQUIRED) include_directories(${GeographicLib_INCLUDE_DIR}) From 6c87ce8a8d9833b5f439408a31fb75beadfb075a Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Sat, 24 Jul 2021 00:18:18 -0400 Subject: [PATCH 43/55] Fixed GPS waypoints --- igvc_gazebo/config/ramp_lane.csv | 2 +- igvc_gazebo/config/waypoints_qual_0.csv | 4 ++-- igvc_gazebo/config/waypoints_qual_1.csv | 4 ++-- igvc_gazebo/config/waypoints_qual_10.csv | 2 +- igvc_gazebo/config/waypoints_qual_2.csv | 2 +- igvc_gazebo/config/waypoints_qual_3.csv | 4 +++- igvc_gazebo/config/waypoints_qual_4.csv | 2 +- igvc_gazebo/config/waypoints_qual_5.csv | 2 +- igvc_gazebo/config/waypoints_qual_6.csv | 2 +- igvc_gazebo/config/waypoints_qual_7.csv | 2 +- igvc_gazebo/config/waypoints_qual_8.csv | 1 + igvc_gazebo/config/waypoints_qual_9.csv | 2 +- igvc_gazebo/launch/qualification.launch | 6 +++--- 13 files changed, 19 insertions(+), 16 deletions(-) diff --git a/igvc_gazebo/config/ramp_lane.csv b/igvc_gazebo/config/ramp_lane.csv index 754f29810..12e8078bf 100644 --- a/igvc_gazebo/config/ramp_lane.csv +++ b/igvc_gazebo/config/ramp_lane.csv @@ -1 +1 @@ -33.7746465553, -84.4049956986 +33.7744970036, -84.4048174628 diff --git a/igvc_gazebo/config/waypoints_qual_0.csv b/igvc_gazebo/config/waypoints_qual_0.csv index ff7d58f9e..1658be81a 100644 --- a/igvc_gazebo/config/waypoints_qual_0.csv +++ b/igvc_gazebo/config/waypoints_qual_0.csv @@ -1,2 +1,2 @@ -33.7744970057, -84.4052924784 -33.7747806719, -84.4052299548 +33.7747404520, -84.4050010479 +33.7746932055, -84.4046567163 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_1.csv b/igvc_gazebo/config/waypoints_qual_1.csv index 28f8812fa..c0015a95f 100644 --- a/igvc_gazebo/config/waypoints_qual_1.csv +++ b/igvc_gazebo/config/waypoints_qual_1.csv @@ -1,2 +1,2 @@ -33.774496988, -84.4052330973 -33.774781346, -84.4051866739 +33.774690857, -84.4050010059 +33.774655721, -84.4046558027 diff --git a/igvc_gazebo/config/waypoints_qual_10.csv b/igvc_gazebo/config/waypoints_qual_10.csv index 8723c3eca..ec587bc4d 100644 --- a/igvc_gazebo/config/waypoints_qual_10.csv +++ b/igvc_gazebo/config/waypoints_qual_10.csv @@ -1 +1 @@ -33.7744969822, -84.4047958812 +33.7743347316, -84.4046771296 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_2.csv b/igvc_gazebo/config/waypoints_qual_2.csv index 28b45bc39..3d4aa93bb 100644 --- a/igvc_gazebo/config/waypoints_qual_2.csv +++ b/igvc_gazebo/config/waypoints_qual_2.csv @@ -1 +1 @@ -33.7744969834, -84.405184529 +33.7746187315, -84.404655538 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_3.csv b/igvc_gazebo/config/waypoints_qual_3.csv index afbbf34c6..d8647ff8f 100644 --- a/igvc_gazebo/config/waypoints_qual_3.csv +++ b/igvc_gazebo/config/waypoints_qual_3.csv @@ -1 +1,3 @@ -33.774496984, -84.4051467422 +33.774623239, -84.4051035645 +33.774623238, -84.4048984439 +33.774623236, -84.4046771293 diff --git a/igvc_gazebo/config/waypoints_qual_4.csv b/igvc_gazebo/config/waypoints_qual_4.csv index fa82f3a13..b7a016e68 100644 --- a/igvc_gazebo/config/waypoints_qual_4.csv +++ b/igvc_gazebo/config/waypoints_qual_4.csv @@ -1 +1 @@ -33.7744969817, -84.4051089608 +33.7745781594, -84.4046771290 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_5.csv b/igvc_gazebo/config/waypoints_qual_5.csv index 1384e5ef5..6fbb07f1f 100644 --- a/igvc_gazebo/config/waypoints_qual_5.csv +++ b/igvc_gazebo/config/waypoints_qual_5.csv @@ -1 +1 @@ -33.774496985, -84.4050387849 +33.774528570, -84.4046771297 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_6.csv b/igvc_gazebo/config/waypoints_qual_6.csv index 1f37e34be..883f2011a 100644 --- a/igvc_gazebo/config/waypoints_qual_6.csv +++ b/igvc_gazebo/config/waypoints_qual_6.csv @@ -1 +1 @@ -33.7744969811, -84.4049826404 +33.7744970156, -84.4046771288 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_7.csv b/igvc_gazebo/config/waypoints_qual_7.csv index ec17128de..bcb4d72b2 100644 --- a/igvc_gazebo/config/waypoints_qual_7.csv +++ b/igvc_gazebo/config/waypoints_qual_7.csv @@ -1 +1 @@ -33.774496983,-84.4049524208 +33.774469970, -84.4046771283 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_8.csv b/igvc_gazebo/config/waypoints_qual_8.csv index e69de29bb..e96153ae2 100644 --- a/igvc_gazebo/config/waypoints_qual_8.csv +++ b/igvc_gazebo/config/waypoints_qual_8.csv @@ -0,0 +1 @@ +33.77441587480856, -84.40467713087766 \ No newline at end of file diff --git a/igvc_gazebo/config/waypoints_qual_9.csv b/igvc_gazebo/config/waypoints_qual_9.csv index 90bb1d363..b3ae0d6a9 100644 --- a/igvc_gazebo/config/waypoints_qual_9.csv +++ b/igvc_gazebo/config/waypoints_qual_9.csv @@ -1 +1 @@ -33.7744969833, -84.4048390622 +33.7743662886, -84.4046771292 \ No newline at end of file diff --git a/igvc_gazebo/launch/qualification.launch b/igvc_gazebo/launch/qualification.launch index c4ed94d61..28a478b54 100644 --- a/igvc_gazebo/launch/qualification.launch +++ b/igvc_gazebo/launch/qualification.launch @@ -19,11 +19,11 @@ - - + + - + From 0c4b513f733ce2a5d769d25e7457cb2e4f6ef159 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Mon, 26 Jul 2021 23:44:09 -0400 Subject: [PATCH 44/55] Fixed graph initialization issue Initially the graph was initialized with a default orientation of 0 which really screwed stuff up with the mapping before the position estimate was able to converge. This commit uses the orientation from the imu as the initial heading and fixes the problems when the robot initially at a different heading. --- igvc_perception/include/slam/slam.h | 1 + igvc_perception/src/slam/slam.cpp | 21 ++++++++++++++++++--- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index 5059d4477..8ddbe0429 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -96,6 +96,7 @@ class Slam bool imu_connected_, imu_update_available_, firstReading; double scale_; GeographicLib::LocalCartesian origin_ENU; + gtsam::Rot3 initOrientation; const double KGRAVITY = 9.81; }; diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 515cb6d2e..41a50b849 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -80,6 +80,7 @@ void Slam::imuCallback(const sensor_msgs::Imu &msg) ros::Time curr_time = ros::Time::now(); Vec3 measured_acc = Vec3(msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z); Vec3 measured_omega = Vec3(msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z); + if (!imu_connected_) { imu_connected_ = true; @@ -173,8 +174,10 @@ void Slam::addWheelOdomFactor() void Slam::optimize() { static int iteration = 0; - // ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ - // << " curr_index: " << curr_index_); +#if defined(_DEBUG) + ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ + << " curr_index: " << curr_index_); +#endif // Update ISAM graph with new factors and estimates isam_.update(graph_, init_estimate_); @@ -254,7 +257,19 @@ void Slam::initializeImuParams() void Slam::initializePriors() { // Adding Initial Position (Pose + Covariance Matrix) - gtsam::Pose3 priorPose; + sensor_msgs::Imu initImuMsg; + sensor_msgs::ImuConstPtr msg = ros::topic::waitForMessage("/magnetometer", ros::Duration(1)); + if (msg == NULL) + { + ROS_INFO("No messages received"); + } + else + { + initImuMsg = *msg; + } + initOrientation = gtsam::Rot3::Quaternion(initImuMsg.orientation.w, initImuMsg.orientation.x, + initImuMsg.orientation.y, initImuMsg.orientation.z); + gtsam::Pose3 priorPose = gtsam::Pose3(initOrientation, gtsam::Point3()); noiseDiagonal::shared_ptr poseNoise = noiseDiagonal::Sigmas((gtsam::Vector(6) << Vec3::Constant(0.1), Vec3::Constant(0.3)).finished()); graph_.push_back(gtsam::PriorFactor(X(0), priorPose, poseNoise)); From d9df73271a2d76c566b6bf204a846739e9caa35e Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Sun, 15 Aug 2021 22:33:55 -0400 Subject: [PATCH 45/55] Fixed problems with wheel odom Added initial yaw orientation for wheel odometer and cleaned up includes in cpp and h files --- .../src/wheel_odometer/Odometer.cpp | 19 +++++++++++++------ igvc_navigation/src/wheel_odometer/Odometer.h | 3 +++ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/igvc_navigation/src/wheel_odometer/Odometer.cpp b/igvc_navigation/src/wheel_odometer/Odometer.cpp index 80114e39f..7503185bd 100644 --- a/igvc_navigation/src/wheel_odometer/Odometer.cpp +++ b/igvc_navigation/src/wheel_odometer/Odometer.cpp @@ -1,9 +1,4 @@ #include "Odometer.h" -#include -#include -#include -#include -#include "Odometer.h" double wheel_separation; @@ -99,7 +94,19 @@ Odometer::Odometer(ros::NodeHandle& nh) // initialize position - map published is relative to position at time t=0 x = 0; y = 0; - yaw = 0; + + sensor_msgs::Imu initImuMsg; + sensor_msgs::ImuConstPtr msg = ros::topic::waitForMessage("/magnetometer", ros::Duration(1)); + if (msg == NULL) + { + ROS_INFO("No messages received"); + } + else + { + initImuMsg = *msg; + } + tf::Quaternion orientation(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w); + yaw = tf::getYaw(orientation); } int main(int argc, char** argv) diff --git a/igvc_navigation/src/wheel_odometer/Odometer.h b/igvc_navigation/src/wheel_odometer/Odometer.h index 6c441ee3b..6e2b5ee1c 100644 --- a/igvc_navigation/src/wheel_odometer/Odometer.h +++ b/igvc_navigation/src/wheel_odometer/Odometer.h @@ -3,6 +3,9 @@ #include #include #include +#include +#include +#include class Odometer { From 6644a8a600614023c5329e1a3ce07d5c204a6e77 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Wed, 18 Aug 2021 23:55:08 -0400 Subject: [PATCH 46/55] Wheel odometry integrated --- igvc_perception/include/slam/slam.h | 7 ++- igvc_perception/src/slam/slam.cpp | 87 +++++++++++++++++++---------- 2 files changed, 63 insertions(+), 31 deletions(-) diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index 8ddbe0429..f9ec1a622 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -17,15 +17,14 @@ #include // Custom Mag Factor based around Pose3 #include +#include #include #include #include #include -#include #include #include #include -#include #include #include #include @@ -89,7 +88,7 @@ class Slam noiseDiagonal::shared_ptr gps_noise_, bias_noise_, mag_noise_, odometryNoise; gtsam::Unit3 local_mag_field_; gtsam::Point3 curr_mag_reading_; - gtsam::Pose2 curr_wheelOdom_reading_; + gtsam::Pose3 curr_wheelOdom_reading_; gtsam::ISAM2 isam_; gtsam::PreintegratedImuMeasurements accum_; ros::Time last_imu_measurement_; @@ -99,6 +98,8 @@ class Slam gtsam::Rot3 initOrientation; const double KGRAVITY = 9.81; + size_t n_discard_frames = 20; + }; #endif // SRC_SLAM_H diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 41a50b849..3d55d484a 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -29,7 +29,8 @@ Slam::Slam() : pnh_{ "~" } } /** - * The gpsCallback adds GPS measurements to the factor graph + * The gpsCallback adds GPS measurements to the factor graph, publishes GPS odom measurements. + * Also adds magnetometer factors, wheel odom measurements, and imu factors, then * @param gps A NavSatFix message derived from GPS measurements (published by /fix) */ void Slam::gpsCallback(const sensor_msgs::NavSatFixConstPtr &gps) @@ -63,6 +64,7 @@ void Slam::gpsCallback(const sensor_msgs::NavSatFixConstPtr &gps) graph_.add(gps_factor); addMagFactor(); + addWheelOdomFactor(); if (imu_update_available_) { @@ -116,7 +118,38 @@ void Slam::addMagFactor() } /** - * If there are IMU measurements in the accumulator, this adds them as a single factor to the factor graph. + * The wheelOdomCallback updates the curr_mag_reading_ with its most recent value + * @param msg An odometry sensor message + */ +void Slam::wheelOdomCallback(const nav_msgs::Odometry &msg) +{ + // curr_wheelOdom_reading_ = Conversion::odomMsgToGtsamPose2(msg); + // odometryNoise = noiseDiagonal::Sigmas( + // (gtsam::Vector(3) << msg.pose.covariance[0], msg.pose.covariance[7], msg.pose.covariance[35]).finished()); + curr_wheelOdom_reading_ = Conversion::odomMsgToGtsamPose3(msg); + odometryNoise = noiseDiagonal::Sigmas( + (gtsam::Vector(6) << msg.pose.covariance[0], msg.pose.covariance[7], msg.pose.covariance[14], + msg.pose.covariance[21], msg.pose.covariance[28], msg.pose.covariance[35]).finished()); + +#if defined(_DEBUG) + ROS_INFO_STREAM("Wheel odom reading: " << curr_wheelOdom_reading_.x() << ", " << curr_wheelOdom_reading_.y() << ", " + << curr_wheelOdom_reading_.theta()); +#endif +} + +/** + * Adds the wheelOdom measurement to the factor graph + */ +void Slam::addWheelOdomFactor() +{ + // auto factor = boost::make_shared >(W(curr_index_), W(curr_index_ + 1), curr_wheelOdom_reading_, odometryNoise); + // graph_.add(factor); + auto factor = boost::make_shared >(X(curr_index_), X(curr_index_ + 1), curr_wheelOdom_reading_, odometryNoise); + graph_.add(factor); +} + +/** + * If there are IMU measurements in the accumulator, this adds them as a single factor to the factor graph and optimizes graph. */ void Slam::integrateAndAddIMUFactor() { @@ -137,44 +170,25 @@ void Slam::integrateAndAddIMUFactor() newPoseEstimate = gtsam::Pose3(accum_.deltaRij() * newPoseEstimate.rotation(), accum_.deltaPij() + newPoseEstimate.translation()); init_estimate_.insert(X(curr_index_ + 1), newPoseEstimate); + Vec3 last_vel = result_.at(V(curr_index_)); last_vel += accum_.deltaVij(); init_estimate_.insert(V(curr_index_ + 1), last_vel); + curr_index_++; optimize(); } accum_.resetIntegration(); } -/** - * The magCallback updates the curr_mag_reading_ with its most recent value - * @param msg An MagneticField sensor message (published by yostlab_driver_node) - */ -void Slam::wheelOdomCallback(const nav_msgs::Odometry &msg) -{ - curr_wheelOdom_reading_ = Conversion::odomMsgToGtsamPose2(msg); -#if defined(_DEBUG) - ROS_INFO_STREAM("Wheel odom reading: " << curr_wheelOdom_reading_.x() << ", " << curr_wheelOdom_reading_.y() << ", " - << curr_wheelOdom_reading_.theta()); -#endif -} - -/** - * Adds the magFactor to the factor graph - */ -void Slam::addWheelOdomFactor() -{ - graph_.add( - gtsam::BetweenFactor(X(curr_index_), X(curr_index_ + 1), curr_wheelOdom_reading_, odometryNoise)); -} - /** * Triggers ISAM2 to optimize the current graph and publish the current estimated pose */ void Slam::optimize() { - static int iteration = 0; + #if defined(_DEBUG) + static int iteration = 0; ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ << " curr_index: " << curr_index_); #endif @@ -202,10 +216,17 @@ void Slam::optimize() history_.update(result_); #endif - // Clear graph and estimates + // Clear the objects holding new factors and node values for the next iteration graph_.resize(0); init_estimate_.clear(); + // Discard first frames, while we wait for pose to converge + if (curr_index_ < n_discard_frames) + { + ROS_INFO("Discarding initial poses..."); + return; + } + auto curr_pose = result_.at(X(curr_index_)); // gtsam::Pose3 currPose Vec3 curr_vel = result_.at(V(curr_index_)); @@ -218,6 +239,7 @@ void Slam::optimize() #if defined(_DEBUG) ROS_INFO_STREAM("IMU MSG: x: " << odom_message.pose.pose.position.x); #endif + location_pub_.publish(odom_message); updateTransform(odom_message); } @@ -277,7 +299,14 @@ void Slam::initializePriors() ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on x0"); #endif init_estimate_.insert(X(0), priorPose); - firstReading = true; + +// // Adding Initial Position for wheel odometry (Pose + Covariance Matrix) +// gtsam::Pose2 priorPose2 = gtsam::Pose2(initOrientation.yaw(), gtsam::Point2()); +// graph_.push_back(gtsam::PriorFactor(W(0), priorPose2, noiseDiagonal::Sigmas(Vec3::Constant(0.1)))); +// #if defined(_DEBUG) +// ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on w0"); +// #endif +// init_estimate_.insert(W(0), priorPose2); // Adding Initial Velocity (Pose + Covariance Matrix) Vec3 priorVel(0.0, 0.0, 0.0); @@ -297,6 +326,9 @@ void Slam::initializePriors() #endif init_estimate_.insert(B(0), gtsam::imuBias::ConstantBias()); + // Set flag for first reading + firstReading = true; + optimize(); ROS_INFO_STREAM("Priors Initialized."); } @@ -312,7 +344,6 @@ void Slam::initializeNoiseMatrices() double mag_noise = pnh_.param("magNoiseConstant", 0.00000005); gps_noise_ = noiseDiagonal::Sigmas(Vec3(gps_xy_noise, gps_xy_noise, gps_z_noise)); mag_noise_ = noiseDiagonal::Sigmas(Vec3::Constant(mag_noise)); - odometryNoise = noiseDiagonal::Sigmas(Vec3(0.2, 0.2, 0.1)); bias_noise_ = noiseDiagonal::Sigmas(gtsam::Vector6::Constant(bias_noise)); } From 6c7b6dd71ae15525144cedbf03876bd7c097dda0 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Thu, 19 Aug 2021 00:31:07 -0400 Subject: [PATCH 47/55] Cleaned up stuff --- igvc_perception/src/slam/slam.cpp | 35 ++++++++++--------------------- 1 file changed, 11 insertions(+), 24 deletions(-) diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 3d55d484a..134eeb70d 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -123,9 +123,6 @@ void Slam::addMagFactor() */ void Slam::wheelOdomCallback(const nav_msgs::Odometry &msg) { - // curr_wheelOdom_reading_ = Conversion::odomMsgToGtsamPose2(msg); - // odometryNoise = noiseDiagonal::Sigmas( - // (gtsam::Vector(3) << msg.pose.covariance[0], msg.pose.covariance[7], msg.pose.covariance[35]).finished()); curr_wheelOdom_reading_ = Conversion::odomMsgToGtsamPose3(msg); odometryNoise = noiseDiagonal::Sigmas( (gtsam::Vector(6) << msg.pose.covariance[0], msg.pose.covariance[7], msg.pose.covariance[14], @@ -142,8 +139,6 @@ void Slam::wheelOdomCallback(const nav_msgs::Odometry &msg) */ void Slam::addWheelOdomFactor() { - // auto factor = boost::make_shared >(W(curr_index_), W(curr_index_ + 1), curr_wheelOdom_reading_, odometryNoise); - // graph_.add(factor); auto factor = boost::make_shared >(X(curr_index_), X(curr_index_ + 1), curr_wheelOdom_reading_, odometryNoise); graph_.add(factor); } @@ -159,7 +154,7 @@ void Slam::integrateAndAddIMUFactor() auto factor = boost::make_shared >( B(curr_index_), B(curr_index_ + 1), gtsam::imuBias::ConstantBias(), bias_noise_); graph_.add(factor); - init_estimate_.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); + graphValues.insert(B(curr_index_ + 1), gtsam::imuBias::ConstantBias()); // Add imu factor gtsam::ImuFactor imufac(X(curr_index_), V(curr_index_), X(curr_index_ + 1), V(curr_index_ + 1), B(curr_index_ + 1), @@ -169,11 +164,11 @@ void Slam::integrateAndAddIMUFactor() auto newPoseEstimate = result_.at(X(curr_index_)); // gtsam::Pose3 newPoseEstimate newPoseEstimate = gtsam::Pose3(accum_.deltaRij() * newPoseEstimate.rotation(), accum_.deltaPij() + newPoseEstimate.translation()); - init_estimate_.insert(X(curr_index_ + 1), newPoseEstimate); + graphValues.insert(X(curr_index_ + 1), newPoseEstimate); Vec3 last_vel = result_.at(V(curr_index_)); last_vel += accum_.deltaVij(); - init_estimate_.insert(V(curr_index_ + 1), last_vel); + graphValues.insert(V(curr_index_ + 1), last_vel); curr_index_++; optimize(); @@ -192,12 +187,12 @@ void Slam::optimize() ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ << " curr_index: " << curr_index_); #endif - // Update ISAM graph with new factors and estimates - isam_.update(graph_, init_estimate_); + // Adds new factors, updating the ISAM solution and relinearizing as needed. + isam_.update(graph_, graphValues); #if defined(_DEBUG) // Add initial estimates to history_ - history_.insert(init_estimate_); + history_.insert(graphValues); graph_.printErrors(history_); ROS_INFO_STREAM("printErrors:"); @@ -210,7 +205,7 @@ void Slam::optimize() result_ = isam_.calculateEstimate(); #if defined(_DEBUG) - graph_.printErrors(init_estimate_); + graph_.printErrors(graphValues); // Update variables with optimized ones history_.update(result_); @@ -218,7 +213,7 @@ void Slam::optimize() // Clear the objects holding new factors and node values for the next iteration graph_.resize(0); - init_estimate_.clear(); + graphValues.clear(); // Discard first frames, while we wait for pose to converge if (curr_index_ < n_discard_frames) @@ -298,15 +293,7 @@ void Slam::initializePriors() #if defined(_DEBUG) ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on x0"); #endif - init_estimate_.insert(X(0), priorPose); - -// // Adding Initial Position for wheel odometry (Pose + Covariance Matrix) -// gtsam::Pose2 priorPose2 = gtsam::Pose2(initOrientation.yaw(), gtsam::Point2()); -// graph_.push_back(gtsam::PriorFactor(W(0), priorPose2, noiseDiagonal::Sigmas(Vec3::Constant(0.1)))); -// #if defined(_DEBUG) -// ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on w0"); -// #endif -// init_estimate_.insert(W(0), priorPose2); + graphValues.insert(X(0), priorPose); // Adding Initial Velocity (Pose + Covariance Matrix) Vec3 priorVel(0.0, 0.0, 0.0); @@ -315,7 +302,7 @@ void Slam::initializePriors() #if defined(_DEBUG) ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on v0"); #endif - init_estimate_.insert(V(0), priorVel); + graphValues.insert(V(0), priorVel); // Adding Bias Prior noiseDiagonal::shared_ptr biasNoise = noiseDiagonal::Sigmas(gtsam::Vector6::Constant(0.1)); @@ -324,7 +311,7 @@ void Slam::initializePriors() #if defined(_DEBUG) ROS_INFO_STREAM("Factor " << graph_.size() << ": PriorFactor on B0"); #endif - init_estimate_.insert(B(0), gtsam::imuBias::ConstantBias()); + graphValues.insert(B(0), gtsam::imuBias::ConstantBias()); // Set flag for first reading firstReading = true; From 775967bc5a4c92bbb18ab85cdab58401aeca2f91 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Thu, 19 Aug 2021 00:34:01 -0400 Subject: [PATCH 48/55] formatting --- igvc_perception/include/slam/slam.h | 3 +-- igvc_perception/src/slam/slam.cpp | 18 ++++++++++-------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index f9ec1a622..bb83e0386 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -77,7 +77,7 @@ class Slam typedef gtsam::Vector3 Vec3; // Defining some variables - gtsam::Values init_estimate_, result_; + gtsam::Values graphValues, result_; #if defined(_DEBUG) gtsam::Values history_; #endif @@ -99,7 +99,6 @@ class Slam const double KGRAVITY = 9.81; size_t n_discard_frames = 20; - }; #endif // SRC_SLAM_H diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 134eeb70d..f0723c4d6 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -30,7 +30,7 @@ Slam::Slam() : pnh_{ "~" } /** * The gpsCallback adds GPS measurements to the factor graph, publishes GPS odom measurements. - * Also adds magnetometer factors, wheel odom measurements, and imu factors, then + * Also adds magnetometer factors, wheel odom measurements, and imu factors, then * @param gps A NavSatFix message derived from GPS measurements (published by /fix) */ void Slam::gpsCallback(const sensor_msgs::NavSatFixConstPtr &gps) @@ -124,9 +124,10 @@ void Slam::addMagFactor() void Slam::wheelOdomCallback(const nav_msgs::Odometry &msg) { curr_wheelOdom_reading_ = Conversion::odomMsgToGtsamPose3(msg); - odometryNoise = noiseDiagonal::Sigmas( - (gtsam::Vector(6) << msg.pose.covariance[0], msg.pose.covariance[7], msg.pose.covariance[14], - msg.pose.covariance[21], msg.pose.covariance[28], msg.pose.covariance[35]).finished()); + odometryNoise = noiseDiagonal::Sigmas((gtsam::Vector(6) << msg.pose.covariance[0], msg.pose.covariance[7], + msg.pose.covariance[14], msg.pose.covariance[21], msg.pose.covariance[28], + msg.pose.covariance[35]) + .finished()); #if defined(_DEBUG) ROS_INFO_STREAM("Wheel odom reading: " << curr_wheelOdom_reading_.x() << ", " << curr_wheelOdom_reading_.y() << ", " @@ -139,12 +140,14 @@ void Slam::wheelOdomCallback(const nav_msgs::Odometry &msg) */ void Slam::addWheelOdomFactor() { - auto factor = boost::make_shared >(X(curr_index_), X(curr_index_ + 1), curr_wheelOdom_reading_, odometryNoise); + auto factor = boost::make_shared >(X(curr_index_), X(curr_index_ + 1), + curr_wheelOdom_reading_, odometryNoise); graph_.add(factor); } /** - * If there are IMU measurements in the accumulator, this adds them as a single factor to the factor graph and optimizes graph. + * If there are IMU measurements in the accumulator, this adds them as a single factor to the factor graph and optimizes + * graph. */ void Slam::integrateAndAddIMUFactor() { @@ -181,13 +184,12 @@ void Slam::integrateAndAddIMUFactor() */ void Slam::optimize() { - #if defined(_DEBUG) static int iteration = 0; ROS_INFO_STREAM("SLAM: Iteration:" << iteration++ << " Imu_updated: " << imu_update_available_ << " curr_index: " << curr_index_); #endif - // Adds new factors, updating the ISAM solution and relinearizing as needed. + // Adds new factors, updating the ISAM solution and relinearizing as needed. isam_.update(graph_, graphValues); #if defined(_DEBUG) From 83ef186c686917968d76e5e29cfe1f1566089049 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Wed, 1 Sep 2021 21:10:41 -0400 Subject: [PATCH 49/55] Fixed wheel odometry integration --- igvc_perception/include/slam/slam.h | 9 ++++++++ igvc_perception/src/slam/slam.cpp | 33 +++++++++++++++++++++++------ 2 files changed, 35 insertions(+), 7 deletions(-) diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index bb83e0386..c71572e9b 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -99,6 +99,15 @@ class Slam const double KGRAVITY = 9.81; size_t n_discard_frames = 20; + + double g_x = 0; + double g_y = 0; + double g_theta = 0; + double g_xVar = 0; + double g_yVar = 0; + double g_zVar = 0; + double g_thetaVariance = 0; + ros::Time g_last_time; // Last time callback was called (to calculate delta t) }; #endif // SRC_SLAM_H diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index f0723c4d6..1e5fd3a7f 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -123,12 +123,28 @@ void Slam::addMagFactor() */ void Slam::wheelOdomCallback(const nav_msgs::Odometry &msg) { - curr_wheelOdom_reading_ = Conversion::odomMsgToGtsamPose3(msg); - odometryNoise = noiseDiagonal::Sigmas((gtsam::Vector(6) << msg.pose.covariance[0], msg.pose.covariance[7], - msg.pose.covariance[14], msg.pose.covariance[21], msg.pose.covariance[28], - msg.pose.covariance[35]) - .finished()); + if (g_last_time.sec == 0) + { + g_last_time = ros::Time::now(); + } + ros::Duration delta_t = msg.header.stamp - g_last_time; + double dt = delta_t.toSec(); + + // the local frame velocities + double vx = msg.twist.twist.linear.x; + double vy = msg.twist.twist.linear.y; // currently zero with jessi but that will change with swervi + // update the relative position from the previous factor + g_x += vx*dt*cos(g_theta) - vy*dt*sin(g_theta); + g_y += vx*dt*sin(g_theta) + vy*dt*cos(g_theta); + + g_theta += dt * msg.twist.twist.angular.z; + g_xVar += dt * msg.twist.covariance[0]; + g_yVar += dt * msg.twist.covariance[7]; + g_zVar += dt * msg.twist.covariance[14]; + g_thetaVariance += dt * msg.twist.covariance[35]; + + g_last_time = msg.header.stamp; #if defined(_DEBUG) ROS_INFO_STREAM("Wheel odom reading: " << curr_wheelOdom_reading_.x() << ", " << curr_wheelOdom_reading_.y() << ", " << curr_wheelOdom_reading_.theta()); @@ -140,8 +156,11 @@ void Slam::wheelOdomCallback(const nav_msgs::Odometry &msg) */ void Slam::addWheelOdomFactor() { - auto factor = boost::make_shared >(X(curr_index_), X(curr_index_ + 1), - curr_wheelOdom_reading_, odometryNoise); + gtsam::Pose3 betweenPose(gtsam::Rot3::Rz(g_theta), gtsam::Point3(g_x, g_y, 0.0)); + + auto factor = gtsam::BetweenFactor(X(curr_index_), X(curr_index_ + 1), betweenPose, noiseDiagonal::Sigmas( + (gtsam::Vector(6) << g_thetaVariance*2, g_thetaVariance*2, g_thetaVariance, g_xVar, g_yVar, g_zVar).finished())); + graph_.add(factor); } From 7189e2a299ff722c6c8c55902c4e2e2f71294556 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Wed, 1 Sep 2021 21:26:48 -0400 Subject: [PATCH 50/55] formatting --- igvc_perception/include/slam/slam.h | 2 +- igvc_perception/src/slam/slam.cpp | 15 +++++++++------ 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index c71572e9b..a694b4b05 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -107,7 +107,7 @@ class Slam double g_yVar = 0; double g_zVar = 0; double g_thetaVariance = 0; - ros::Time g_last_time; // Last time callback was called (to calculate delta t) + ros::Time g_last_time; // Last time callback was called (to calculate delta t) }; #endif // SRC_SLAM_H diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 1e5fd3a7f..e4df77815 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -132,11 +132,11 @@ void Slam::wheelOdomCallback(const nav_msgs::Odometry &msg) // the local frame velocities double vx = msg.twist.twist.linear.x; - double vy = msg.twist.twist.linear.y; // currently zero with jessi but that will change with swervi + double vy = msg.twist.twist.linear.y; // currently zero with jessi but that will change with swervi // update the relative position from the previous factor - g_x += vx*dt*cos(g_theta) - vy*dt*sin(g_theta); - g_y += vx*dt*sin(g_theta) + vy*dt*cos(g_theta); + g_x += vx * dt * cos(g_theta) - vy * dt * sin(g_theta); + g_y += vx * dt * sin(g_theta) + vy * dt * cos(g_theta); g_theta += dt * msg.twist.twist.angular.z; g_xVar += dt * msg.twist.covariance[0]; @@ -158,9 +158,12 @@ void Slam::addWheelOdomFactor() { gtsam::Pose3 betweenPose(gtsam::Rot3::Rz(g_theta), gtsam::Point3(g_x, g_y, 0.0)); - auto factor = gtsam::BetweenFactor(X(curr_index_), X(curr_index_ + 1), betweenPose, noiseDiagonal::Sigmas( - (gtsam::Vector(6) << g_thetaVariance*2, g_thetaVariance*2, g_thetaVariance, g_xVar, g_yVar, g_zVar).finished())); - + auto factor = gtsam::BetweenFactor( + X(curr_index_), X(curr_index_ + 1), betweenPose, + noiseDiagonal::Sigmas( + (gtsam::Vector(6) << g_thetaVariance * 2, g_thetaVariance * 2, g_thetaVariance, g_xVar, g_yVar, g_zVar) + .finished())); + graph_.add(factor); } From 92ced4a99276961dc8bda5c844892e9bb44236eb Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Thu, 2 Sep 2021 00:17:41 -0400 Subject: [PATCH 51/55] forgot to reset wheel factor vals --- igvc_perception/src/slam/slam.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index e4df77815..ed1d2fdbb 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -165,6 +165,14 @@ void Slam::addWheelOdomFactor() .finished())); graph_.add(factor); + + g_x = 0; + g_y = 0; + g_theta = 0; + g_xVar = 0; + g_yVar = 0; + g_zVar = 0; + g_thetaVariance = 0; } /** @@ -226,7 +234,7 @@ void Slam::optimize() isam_.getFactorsUnsafe().print(); #endif - result_ = isam_.calculateEstimate(); + result_ = isam_.calculateBestEstimate(); #if defined(_DEBUG) graph_.printErrors(graphValues); From f472e93a7df63581778d000a046262c1c221dc79 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Thu, 2 Sep 2021 00:18:35 -0400 Subject: [PATCH 52/55] keeping calculateEstimate for now --- igvc_perception/src/slam/slam.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index ed1d2fdbb..cab21f2aa 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -234,7 +234,7 @@ void Slam::optimize() isam_.getFactorsUnsafe().print(); #endif - result_ = isam_.calculateBestEstimate(); + result_ = isam_.calculateEstimate(); #if defined(_DEBUG) graph_.printErrors(graphValues); From 7022b7672aa51580ddba658a1c322a9bbbed3761 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Thu, 2 Sep 2021 00:19:22 -0400 Subject: [PATCH 53/55] format --- igvc_perception/src/slam/slam.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index cab21f2aa..44db6c0e2 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -165,7 +165,7 @@ void Slam::addWheelOdomFactor() .finished())); graph_.add(factor); - + g_x = 0; g_y = 0; g_theta = 0; From 4cebf3ecb0a3d2f8ebde5a3f163caee8435cd16f Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Thu, 2 Sep 2021 01:28:09 -0400 Subject: [PATCH 54/55] Added in covariance matrices to pose estimate --- igvc_perception/include/slam/slam.h | 4 +- .../include/slam/type_conversions.h | 2 +- igvc_perception/src/slam/slam.cpp | 11 ++-- igvc_perception/src/slam/type_conversions.cpp | 50 ++++++++++++++++--- 4 files changed, 55 insertions(+), 12 deletions(-) diff --git a/igvc_perception/include/slam/slam.h b/igvc_perception/include/slam/slam.h index a694b4b05..2ea9114fd 100644 --- a/igvc_perception/include/slam/slam.h +++ b/igvc_perception/include/slam/slam.h @@ -69,8 +69,8 @@ class Slam void initializeDirectionOfLocalMagField(); void addMagFactor(); void updateTransform(const nav_msgs::Odometry &pos); - static nav_msgs::Odometry createOdomMsg(const gtsam::Pose3 &pos, const gtsam::Vector3 &vel, - const gtsam::Vector3 &ang); + static nav_msgs::Odometry createOdomMsg(const gtsam::Pose3 &pos, const gtsam::Vector3 &vel, const gtsam::Vector3 &ang, + const gtsam::Matrix &cov); // Defining some types typedef gtsam::noiseModel::Diagonal noiseDiagonal; diff --git a/igvc_perception/include/slam/type_conversions.h b/igvc_perception/include/slam/type_conversions.h index 7281e0ed3..2fc98ffb5 100644 --- a/igvc_perception/include/slam/type_conversions.h +++ b/igvc_perception/include/slam/type_conversions.h @@ -15,7 +15,7 @@ class Conversion static gtsam::Pose2 odomMsgToGtsamPose2(const nav_msgs::Odometry &msg); static gtsam::Point3 odomMsgToGtsamPoint3(const nav_msgs::Odometry &msg); static geometry_msgs::Vector3 gtsamVector3ToVector3Msg(const gtsam::Vector3 &vec); - static geometry_msgs::Pose gtsamPose3ToPose3Msg(const gtsam::Pose3 &pos); + static geometry_msgs::PoseWithCovariance gtsamPose3ToPose3Msg(const gtsam::Pose3 &pos, const gtsam::Matrix &cov); }; #endif // SRC_TYPE_CONVERSIONS_H diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 44db6c0e2..451b576a7 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -236,6 +236,8 @@ void Slam::optimize() result_ = isam_.calculateEstimate(); + gtsam::Matrix covariance = isam_.marginalCovariance(X(curr_index_)); + #if defined(_DEBUG) graph_.printErrors(graphValues); @@ -262,7 +264,7 @@ void Slam::optimize() curr_vel = global_to_local._transformVector(curr_vel); Vec3 curr_ang = accum_.deltaRij().rpy() / accum_.deltaTij(); - auto odom_message = createOdomMsg(curr_pose, curr_vel, curr_ang); + auto odom_message = createOdomMsg(curr_pose, curr_vel, curr_ang, covariance); #if defined(_DEBUG) ROS_INFO_STREAM("IMU MSG: x: " << odom_message.pose.pose.position.x); #endif @@ -378,14 +380,17 @@ void Slam::initializeDirectionOfLocalMagField() ROS_INFO_STREAM("Initializing Direction of Local Magnetic Field"); } -nav_msgs::Odometry Slam::createOdomMsg(const gtsam::Pose3 &pos, const gtsam::Vector3 &vel, const gtsam::Vector3 &ang) +nav_msgs::Odometry Slam::createOdomMsg(const gtsam::Pose3 &pos, const gtsam::Vector3 &vel, const gtsam::Vector3 &ang, + const gtsam::Matrix &cov) { nav_msgs::Odometry msg; msg.header.stamp = ros::Time::now(); msg.child_frame_id = "base_footprint"; msg.header.frame_id = "odom"; - msg.pose.pose = Conversion::gtsamPose3ToPose3Msg(pos); + msg.pose = Conversion::gtsamPose3ToPose3Msg(pos, cov); msg.twist.twist.linear = Conversion::gtsamVector3ToVector3Msg(vel); msg.twist.twist.angular = Conversion::gtsamVector3ToVector3Msg(ang); + + msg.twist.covariance = msg.pose.covariance; return msg; } \ No newline at end of file diff --git a/igvc_perception/src/slam/type_conversions.cpp b/igvc_perception/src/slam/type_conversions.cpp index f1f17ea4a..dfc76989a 100644 --- a/igvc_perception/src/slam/type_conversions.cpp +++ b/igvc_perception/src/slam/type_conversions.cpp @@ -15,14 +15,52 @@ gtsam::Pose2 Conversion::odomMsgToGtsamPose2(const nav_msgs::Odometry &msg) return gtsam::Pose2(twist.yaw(), gtsam::Point2(msg.pose.pose.position.x, msg.pose.pose.position.y)); } -geometry_msgs::Pose Conversion::gtsamPose3ToPose3Msg(const gtsam::Pose3 &pos) +geometry_msgs::PoseWithCovariance Conversion::gtsamPose3ToPose3Msg(const gtsam::Pose3 &pos, const gtsam::Matrix &cov) { - geometry_msgs::Pose gPos; - gPos.position.x = pos.x(); - gPos.position.y = pos.y(); - gPos.position.z = pos.z(); + geometry_msgs::PoseWithCovariance gPos; + gPos.pose.position.x = pos.x(); + gPos.pose.position.y = pos.y(); + gPos.pose.position.z = pos.z(); gtsam::Vector3 r = pos.rotation().xyz(); - gPos.orientation = tf::createQuaternionMsgFromRollPitchYaw(r.x(), r.y(), r.z()); + gPos.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(r.x(), r.y(), r.z()); + + gPos.covariance[0] = cov(0); + gPos.covariance[1] = cov(1); + gPos.covariance[2] = cov(2); + gPos.covariance[3] = cov(3); + gPos.covariance[4] = cov(4); + gPos.covariance[5] = cov(5); + gPos.covariance[6] = cov(6); + gPos.covariance[7] = cov(7); + gPos.covariance[8] = cov(8); + gPos.covariance[9] = cov(9); + gPos.covariance[10] = cov(10); + gPos.covariance[11] = cov(11); + gPos.covariance[12] = cov(12); + gPos.covariance[13] = cov(13); + gPos.covariance[14] = cov(14); + gPos.covariance[15] = cov(15); + gPos.covariance[16] = cov(16); + gPos.covariance[17] = cov(17); + gPos.covariance[18] = cov(18); + gPos.covariance[19] = cov(19); + gPos.covariance[20] = cov(20); + gPos.covariance[21] = cov(21); + gPos.covariance[22] = cov(22); + gPos.covariance[23] = cov(23); + gPos.covariance[24] = cov(24); + gPos.covariance[25] = cov(25); + gPos.covariance[26] = cov(26); + gPos.covariance[27] = cov(27); + gPos.covariance[28] = cov(28); + gPos.covariance[29] = cov(29); + gPos.covariance[30] = cov(30); + gPos.covariance[31] = cov(31); + gPos.covariance[32] = cov(32); + gPos.covariance[33] = cov(33); + gPos.covariance[34] = cov(34); + gPos.covariance[35] = cov(35); + return gPos; } From 8eec6ac2d0ab6b9b1eb712575dd6ae4187d87c78 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Thu, 2 Sep 2021 11:55:00 -0400 Subject: [PATCH 55/55] proper wheel odom noise --- igvc_perception/src/slam/slam.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/igvc_perception/src/slam/slam.cpp b/igvc_perception/src/slam/slam.cpp index 451b576a7..8f6e7c96e 100644 --- a/igvc_perception/src/slam/slam.cpp +++ b/igvc_perception/src/slam/slam.cpp @@ -161,7 +161,7 @@ void Slam::addWheelOdomFactor() auto factor = gtsam::BetweenFactor( X(curr_index_), X(curr_index_ + 1), betweenPose, noiseDiagonal::Sigmas( - (gtsam::Vector(6) << g_thetaVariance * 2, g_thetaVariance * 2, g_thetaVariance, g_xVar, g_yVar, g_zVar) + (gtsam::Vector(6) << g_xVar, g_yVar, g_zVar, g_thetaVariance * 2, g_thetaVariance * 2, g_thetaVariance) .finished())); graph_.add(factor);