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)
-
-
-
assigned issue to
-
assigned issue to
-
- changed status to invalid
ILS problem with graph, not GTSAM
- Log in to comment
I think there are 2 problems with your code.
initial pose and prior Pose3(Eigen::Matrix4d::Zero()) does not have a valid rotation matrix. Maybe you meant Pose3(Eigen::Matrix4d:Identity()) ?
priorModel is all zeros. Noise generally has to be > 0, else you'll get divide by zero errors.