Question: NonDeterminedLinearSystem Pose3D

Hi,

I am having a lot of trouble using GTSAM for loop closure of a 3D problem. I feel like I am setting up the problem correctly but it crashed on GaussNewton Optimization. If I use Levenberg it wont crash but the result will be the same as my input. I have been trying to base it on examples. I am not sure if this is a bug or just usage. It does not crash if I add no prior. But the result is then unchanged.

Any advice on this would be greatly appreciated. The following is what I am doing:

To describe my problem. I have a camera on a rotating platform capturing point cloud data. I know approximately (to within 1-2 degrees) the rotation around the y-axis between data captures to be 50 degrees. I then fit a loop closure transform from vertex 6 back to 0 (using a piece of software to get the transform). I want to then optimise my transforms based on my estimates. I store my relative transforms between the vertices in a text file which I load (loop closure is the last transform). The contents of this file are at the bottom.

Values initial;
NonlinearFactorGraph graph;

std::ifstream input(inputFile);

Eigen::Matrix4d globalPosePosition = Eigen::Matrix4d::Identity();
std::vector<Eigen::Matrix4d> edgeTransforms; //Relative poses - Transform to apply to v1 to get to v2

//Read in edges 
for (int i = 0; i < 7; i++)
{
    std::string line;
    std::getline(input, line);

    std::vector<std::string> splitLine;
    boost::split(splitLine, line, boost::is_any_of(","));

    //Create transform (gtsam just uses eigen).
    Eigen::Matrix4d transform;

    transform <<    std::stof(splitLine[0]), std::stof(splitLine[1]), std::stof(splitLine[2]), std::stof(splitLine[3]),
            std::stof(splitLine[4]), std::stof(splitLine[5]), std::stof(splitLine[6]), std::stof(splitLine[7]),
            std::stof(splitLine[8]), std::stof(splitLine[9]), std::stof(splitLine[10]), std::stof(splitLine[11]),
            std::stof(splitLine[12]), std::stof(splitLine[13]), std::stof(splitLine[14]), std::stof(splitLine[15]);

    edgeTransforms.push_back(transform);
}

//Add first vertex
initial.insert(0, gtsam::Pose3(Eigen::Matrix4d::Zero()));

for (int e = 0; e < 6; e++)
{
    globalPosePosition = edgeTransforms[e] * globalPosePosition;

    //Add to graph the pose transform
    initial.insert((e+1), gtsam::Pose3(globalPosePosition));
}

noiseModel::Diagonal::shared_ptr priorModel = //
        noiseModel::Diagonal::Variances((Vector(6) << 0, 0, 0, 0, 0, 0));

    noiseModel::Diagonal::shared_ptr noiseModel = //
        noiseModel::Diagonal::Variances((Vector(6) << 2.0, 2.0, 2.0, 2.0, 2.0, 2.0));

//Add prior
graph.add(PriorFactor<Pose3>(0, Pose3(Eigen::Matrix4d::Zero()), priorModel));

graph.add(BetweenFactor<Pose3>(0, 1, Pose3(edgeTransforms[0]), noiseModel));
graph.add(BetweenFactor<Pose3>(1, 2, Pose3(edgeTransforms[1]), noiseModel));
graph.add(BetweenFactor<Pose3>(2, 3, Pose3(edgeTransforms[2]), noiseModel));
graph.add(BetweenFactor<Pose3>(3, 4, Pose3(edgeTransforms[3]), noiseModel));
graph.add(BetweenFactor<Pose3>(4, 5, Pose3(edgeTransforms[4]), noiseModel));
graph.add(BetweenFactor<Pose3>(5, 6, Pose3(edgeTransforms[5]), noiseModel));
graph.add(BetweenFactor<Pose3>(6, 0, Pose3(edgeTransforms[6]), noiseModel));

Values results = GaussNewtonOptimizer(graph, initial).optimize();

Contents of pose file:

0.644124,0,0.764921,0,0,1,0,0,-0.764921,0,0.644124,0,0,0,0,1
0.644124,0,0.764921,0,0,1,0,0,-0.764921,0,0.644124,0,0,0,0,1
0.644124,0,0.764921,0,0,1,0,0,-0.764921,0,0.644124,0,0,0,0,1
0.644124,0,0.764921,0,0,1,0,0,-0.764921,0,0.644124,0,0,0,0,1
0.644124,0,0.764921,0,0,1,0,0,-0.764921,0,0.644124,0,0,0,0,1
0.644124,0,0.764921,0,0,1,0,0,-0.764921,0,0.644124,0,0,0,0,1
0.995018005371,0.099690653384,0.000996547169,-0.086787238717,-0.099335893989,0.992226421833,-0.074960812926,0.035765063018,-0.008461693302,0.074488364160,0.997186005116,-0.075587257743,0.000000000000,0.000000000000,0.000000000000,1.000000000000

Comments (3)