Closed weiningwei closed 3 years ago
In the function of sceneRepresentation.cpp bool sceneRepresentation::updateScene(list<PointFeature *> matched_pt, list<LineFeature *> matched_ls), after pose = pose + x_aux;, I added the following code: ` string savePath = "trajout.txt"; ofstream f; f.open(savePath.c_str(), ios::app); f << fixed; Eigen::Vector3d poseTran; Eigen::Vector3d t(pose.x(), pose.y(), pose.z()); poseTran = t; //cout << pose << endl;
bool sceneRepresentation::updateScene(list<PointFeature *> matched_pt, list<LineFeature *> matched_ls)
pose = pose + x_aux;
Eigen::Matrix3d rotMat = pose.getRotationMatrix(); f << setprecision(7) << rotMat(0, 0) << " " << rotMat(0, 1) << " " << rotMat(0, 2) << " " << poseTran.x() << " " << rotMat(1, 0) << " " << rotMat(1, 1) << " " << rotMat(1, 2) << " " << poseTran.y() << " " << rotMat(2, 0) << " " << rotMat(2, 1) << " " << rotMat(2, 2) << " " << poseTran.z() << endl; f << flush; f.close();`
But the estimated trajectory has a large error from the groundtruth.
In the function of sceneRepresentation.cpp
bool sceneRepresentation::updateScene(list<PointFeature *> matched_pt, list<LineFeature *> matched_ls)
, afterpose = pose + x_aux;
, I added the following code: ` string savePath = "trajout.txt"; ofstream f; f.open(savePath.c_str(), ios::app); f << fixed; Eigen::Vector3d poseTran; Eigen::Vector3d t(pose.x(), pose.y(), pose.z()); poseTran = t; //cout << pose << endl;But the estimated trajectory has a large error from the groundtruth.