ElasticFusion Slam algorithm running with pose graph on TUM RGB-D benchmark - algorithm

Question

I downloaded Freiburg desk dataset from TUM RGB-D SLAM Dataset and Benchmark and converted it to '.klg' which is custom format of slam algorithm . I loaded this klg file to ElasticFusion and runned the SLAM algorithm. The 3d reconstruction output seems good enough while doing it.
Now i want to build 3d reconstruction by already built trajectory information.
I retrieved trajectory data from previous run from '.freibrug' and converted it to desired format by ElasticFusion. I just changed timestamp from seconds to microsenconds by multiplying it to 1000000. And split the variables using "," instead of " " space .
I run the algorithm this time with "-p" flag and path information to trajectory file. Below is my running command.
/path_to_EF/./ElasticFusion -l /path_to_data/rgbd_dataset_freiburg1_desk/test2.klg -p /path_to_data/rgbd_dataset_freiburg1_desk/modified_freiburg.txt
I am expecting to get the same point cloud. But the result i am getting with given data far from the expected.
As you see its accuracy and reconstruction level far worse than previous run .
I do not have problem with trajectory. The below graph shows that trajectory I retrieved from the previous run is close to the groundtruth data provided by TUM RGB-D Benchmark.
Even when I am running it with groundtruth data, it does not build nice 3d reconstruction. What can be the reason and the missing points for such result?
Good suggestions and answers will be appreciated.

Answer 1

I successfully ran the code with trajectory file
(your timestamp should be integer and separate all parameter with space)
modify the ElasticFusion/GUI/src/Tools/GroundTruthOdometry.cpp
void GroundTruthOdometry::loadTrajectory(const std::string & filename)
{
std::ifstream file;
std::string line;
file.open(filename.c_str());
while (!file.eof())
{
unsigned long long int utime;
float x, y, z, qx, qy, qz, qw;
std::getline(file, line);
int n = sscanf(line.c_str(), "%llu %f %f %f %f %f %f %f", &utime, &x, &y, &z, &qx, &qy, &qz, &qw);
if(file.eof())
break;
assert(n == 8);
Eigen::Quaternionf q(qw, qx, qy, qz);
Eigen::Vector3f t(x, y, z);
Eigen::Isometry3f T;
T.setIdentity();
T.pretranslate(t).rotate(q);
camera_trajectory[utime] = T;
}
}
Eigen::Matrix4f GroundTruthOdometry::getTransformation(uint64_t timestamp)
{
Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
if(last_utime != 0)
{
std::map<uint64_t, Eigen::Isometry3f>::const_iterator it = camera_trajectory.find(last_utime);
if (it == camera_trajectory.end())
{
last_utime = timestamp;
return pose;
}
pose = camera_trajectory[timestamp].matrix();
}
else
{
std::map<uint64_t, Eigen::Isometry3f>::const_iterator it = camera_trajectory.find(timestamp);
Eigen::Isometry3f ident = it->second;
pose = Eigen::Matrix4f::Identity();
camera_trajectory[last_utime] = ident;
}
last_utime = timestamp;
return pose;
}
Basically just disable the M matrix, you can try it out

Answer 2

I took 3 scans: left-to-right, down-to-up and back-to-front. I observed that thought trajectory file seems correct , the building is going wrong. When I move the camera on x axis , on EF it moves in z axis and similar situation for the others. I tried to found transformation matrix manually. I applied this transformation to translation and rotation. It started to work afterwards.