problem with return type of gtsam::noiseModels

[tmcg0]

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.

[tmcg0]

Here's an example output of that unit test. The i variables should be isotropic noise models, and the d variables should be diagonal noise models. But when you print them using i->print(), etc., you get the following:

[tmcg0]

Went back in my old files, here's what testNoiseModels.cpp looked like:

// test noise models of gtsam

#include <gtsam/base/Vector.h>
#include <gtsam/linear/NoiseModel.h>
#include <testutils.h>

int main(){
    // test 1: are diagonal and isotropic models the same when you use a one vector?
    gtsam::Vector3 v1=gtsam::Vector3::Ones();
    gtsam::SharedNoiseModel i1=gtsam::noiseModel::Isotropic::Sigmas(v1,true);
    gtsam::SharedNoiseModel d1=gtsam::noiseModel::Diagonal::Sigmas(v1,true);
    i1->print("i1=");
    d1->print("d1=");
    bool test1=i1->equals(*d1);
    if(!test1){
        throw std::runtime_error("test 1 failed.");
    }
    // test 2: are diagonal and isotropic models the same when you use a constant vector?
    double a=testutils::dRand();
    gtsam::Vector3 v2=gtsam::Vector3(a,a,a);
    gtsam::SharedNoiseModel i2=gtsam::noiseModel::Isotropic::Sigmas(v2,true); i2->print("i2=");
    gtsam::SharedNoiseModel d2=gtsam::noiseModel::Diagonal::Sigmas(v2,true); d2->print("d2=");
    bool test2=i2->equals(*d2);
    if(!test2){
        throw std::runtime_error("test 2 failed.");
    }
    // test 3: are diagonal and isotropic models the same when you use a random vector?
    gtsam::Vector3 v3=testutils::randomVector3();
    gtsam::SharedNoiseModel i3=gtsam::noiseModel::Isotropic::Sigmas(v3,true);
    gtsam::SharedNoiseModel d3=gtsam::noiseModel::Diagonal::Sigmas(v3,true);
    i3->print("i3=");
    d3->print("d3=");
    bool test3=i3->equals(*d3);
    if(!test3){
        throw std::runtime_error("test 3 failed.");
    }
    return 0;
}