See testNoiseModels.cpp

For some reason, all of my noise models calls (i.e., gtsam::noiseModel::Diagonal::Sigmas) are all returning shared pointers to gtsam isotropic noise models or constrained noise models (i.e., the wrong type--we want diagonal noise models). It seems to be working okay in lowerbodypoesestimator though. Figure this problem out.