Factor Graph SLAM with GTSAM

igvc_description/urdf/jessii.urdf.xacro

@@ -619,7 +619,7 @@
       <headingDrift>0.0 0.0 0.0</headingDrift>
       <headingDriftFrequency>0.0 0.0 0.0</headingDriftFrequency>
       <headingGaussianNoise>0.0 0.0 0.0</headingGaussianNoise>
-      <yawOffset>1.57079632679</yawOffset>
+      <yawOffset>0</yawOffset>
     </plugin>
 
     <plugin name="magnetometer" filename="libhector_gazebo_ros_magnetic.so">
@@ -644,6 +644,7 @@
       <velocityTopicName>/fix_velocity</velocityTopicName>
       <referenceLatitude>33.774497</referenceLatitude>
       <referenceLongitude>-84.405001</referenceLongitude>
+        <referenceHeading>90</referenceHeading>
       <referenceAltitude>309.0</referenceAltitude>
       <drift>0.001 0.001 0.001</drift>
       <gaussianNoise>0.0001 0.0001 0.0001</gaussianNoise>

igvc_gazebo/config/ramp_lane.csv

@@ -1 +1 @@
-33.7746465553, -84.4049956986
+33.7744970036, -84.4048174628

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
+33.7742906118346, -84.40482946425239
+33.7744032479795, -84.40493470388343
+33.7744015516094, -84.40507763991296
+33.7742906402805, -84.40517679775147
+33.7747034526282, -84.40500116144553

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
+33.7742906402805, -84.40517679775147
+33.7744015516094, -84.40507763991296
+33.7744032479795, -84.40493470388343
+33.7742906118346, -84.40482946425239
+33.7747034526282, -84.40500116144553

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

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

igvc_gazebo/config/waypoints_qual_10.csv

@@ -1 +1 @@
-33.7744969822, -84.4047958812
+33.7743347316, -84.4046771296

igvc_gazebo/config/waypoints_qual_2.csv

@@ -1 +1 @@
-33.7744969834, -84.405184529
+33.7746187315, -84.404655538

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

igvc_gazebo/config/waypoints_qual_4.csv

@@ -1 +1 @@
-33.7744969817, -84.4051089608
+33.7745781594, -84.4046771290

igvc_gazebo/config/waypoints_qual_5.csv

@@ -1 +1 @@
-33.774496985, -84.4050387849
+33.774528570, -84.4046771297

igvc_gazebo/config/waypoints_qual_6.csv

@@ -1 +1 @@
-33.7744969811, -84.4049826404
+33.7744970156, -84.4046771288

igvc_gazebo/config/waypoints_qual_7.csv

@@ -1 +1 @@
-33.774496983,-84.4049524208
+33.774469970, -84.4046771283

igvc_gazebo/config/waypoints_qual_8.csv

@@ -0,0 +1 @@
+33.77441587480856, -84.40467713087766

igvc_gazebo/config/waypoints_qual_9.csv

@@ -1 +1 @@
-33.7744969833, -84.4048390622
+33.7743662886, -84.4046771292

igvc_gazebo/launch/igvc_control.launch

@@ -17,7 +17,7 @@
       <param name="speed_D_right" value="0.3"/>
       <param name="speed_I_left" value="0.000"/>
       <param name="speed_I_right" value="0.000"/>
-      <param name="wheel_radius" value="0.3429"/>
+      <param name="wheel_radius" value="0.175"/>
       <param name="max_effort" value="8.0"/>
       <param name="rate" value="60.0"/>
   </node>

igvc_gazebo/launch/qualification.launch

@@ -19,11 +19,11 @@
     <arg name="y" value="13.5" if="$(eval track == 3)"/>
     <arg name="y" value="9" if="$(eval track == 4)"/>
     <arg name="y" value="3.5" if="$(eval track == 5)"/>
-    <arg name="y" value="-1" if="$(eval track == 6)"/>
-    <arg name="y" value="-4.5" if="$(eval track == 7)"/>
+    <arg name="y" value="0" if="$(eval track == 6)"/>
+    <arg name="y" value="-3" if="$(eval track == 7)"/>
     <arg name="y" value="-9" if="$(eval track == 8)"/>
     <arg name="y" value="-14.5" if="$(eval track == 9)"/>
-    <arg name="y" value="-19" if="$(eval track == 10)"/>
+    <arg name="y" value="-18" if="$(eval track == 10)"/>
 
     <include file="$(find igvc_gazebo)/launch/simulation.launch">
         <arg name="world_name" value="$(find igvc_description)/urdf/worlds/qualification.world"/>

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;

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

igvc_navigation/launch/3D_nav_simulation.launch

@@ -0,0 +1,29 @@
+<launch>
+    <arg name="read_from_file" default="true"/>
+
+    <!-- Localization -->
+    <include file="$(find igvc_perception)/launch/slam.launch" />
+
+    <!-- Launch navigation (simulation version) -->
+    <include file="$(find igvc_navigation)/launch/mbf_navigation.launch" />
+
+    <!-- Launch navigation server -->
+    <include file="$(find igvc_navigation)/launch/navigation_server.launch"/>
+
+    <!-- Launch navigation client -->
+    <include file="$(find igvc_navigation)/launch/navigation_client.launch">
+        <arg name="read_from_file" value="$(arg read_from_file)"/>
+    </include>
+
+    <!-- Launch differential drive controller -->
+    <include file="$(find igvc_navigation)/launch/differential_drive.launch" />
+
+    <!-- pointcloud filter -->
+    <include file="$(find igvc_perception)/launch/pointcloud_filter.launch" />
+
+    <!-- elevation mapping -->
+    <include file="$(find igvc_perception)/launch/elevation_mapping.launch" />
+
+    <!-- traversability filter -->
+    <include file="$(find igvc_perception)/launch/slope_filter.launch"/>
+</launch>

igvc_navigation/src/wheel_odometer/Odometer.cpp

@@ -1,9 +1,4 @@
 #include "Odometer.h"
-#include <igvc_msgs/velocity_pair.h>
-#include <math.h>
-#include <nav_msgs/Odometry.h>
-#include <ros/ros.h>
-#include "Odometer.h"
 
 double wheel_separation;
 
@@ -78,7 +73,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
@@ -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<sensor_msgs::Imu>("/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)

igvc_navigation/src/wheel_odometer/Odometer.h

@@ -3,6 +3,9 @@
 #include <igvc_msgs/velocity_pair.h>
 #include <ros/ros.h>
 #include <tf/transform_broadcaster.h>
+#include <sensor_msgs/Imu.h>
+#include <math.h>
+#include <nav_msgs/Odometry.h>
 
 class Odometer
 {

igvc_perception/CMakeLists.txt

@@ -171,15 +171,7 @@ 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)
 add_subdirectory(src/slope_filter)

igvc_perception/include/slam/MagPoseFactor.h

@@ -0,0 +1,66 @@
+#ifndef SRC_MAGPOSEFACTOR_H
+#define SRC_MAGPOSEFACTOR_H
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/geometry/Pose3.h>
+
+class MagPoseFactor : public gtsam::NoiseModelFactor1<gtsam::Pose3>
+{
+  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<gtsam::Pose3>(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>(
+        gtsam::NonlinearFactor::shared_ptr(new MagPoseFactor(*this)));
+  }
+
+  /**
+   * @brief vector of errors
+   */
+  gtsam::Vector evaluateError(const gtsam::Pose3& nRb, boost::optional<gtsam::Matrix&> H) const override
+  {
+    // measured bM = nRbÕ * nM + b
+    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;
+  }
+
+  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: ");
+  }
+};
+
+#endif  // SRC_MAGPOSEFACTOR_H