i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-12 14:30:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

rgbd_dataset: added support for rgbd_bonn ground truth format

Browse Source
This commit is contained in:
matlabbe
2025-09-20 18:08:38 -07:00
parent cb0cc9ed18
commit c774ef641a
6 changed files with 70 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Graph.h
+1 -1
View File
@@ -56,7 +56,7 @@ bool RTABMAP_CORE_EXPORT exportPoses(
bool RTABMAP_CORE_EXPORT importPoses(
const std::string & filePath,
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
std::map<int, Transform> & poses,
std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
std::map<int, double> * stamps = 0); // optional for format 1 and 9
corelib/include/rtabmap/core/camera/CameraImages.h
+2 -2
View File
@@ -94,14 +94,14 @@ public:
_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
}
// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
// 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
void setOdometryPath(const std::string & filePath, int format = 0)
{
_odometryPath = filePath;
_odometryFormat = format;
}
// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
// 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
void setGroundTruthPath(const std::string & filePath, int format = 0)
{
_groundTruthPath = filePath;
corelib/src/Graph.cpp
+16 -2
View File
@@ -196,7 +196,7 @@ bool exportPoses(
bool importPoses(
const std::string & filePath,
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
std::map<int, Transform> & poses,
std::multimap<int, Link> * constraints, // optional for formats 3 and 4
std::map<int, double> * stamps) // optional for format 1 and 9
@@ -440,7 +440,7 @@ bool importPoses(
UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y, found %d)", str.c_str(), (int)strList.size());
}
}
else if(format == 1 || format==10 || format==11) // rgbd-slam format
else if(format == 1 || format==10 || format==11 || format==12) // rgbd-slam format
{
std::list<std::string> strList = uSplit(str);
if((strList.size() >= 8 && format!=11) || (strList.size() == 9 && format==11))
@@ -481,6 +481,20 @@ bool importPoses(
1, 0, 0, 0);
pose = t*pose;
}
else if(format == 12)
{
// See https://www.ipb.uni-bonn.de/data/rgbd-dynamic-dataset/index.html
Transform T_ros(-1, 0, 0, 0,
0, 0, 1, 0,
0, 1, 0, 0);
Transform T_m(
1.0157, 0.1828, -0.2389, 0.0113,
0.0009, -0.8431, -0.6413, -0.00980,
-0.3009, 0.6147, -0.8085, 0.0111);
// we remove the optical rotation
pose = T_ros*pose*T_ros*T_m*CameraModel::opticalRotation().inverse();
}
poses.insert(std::make_pair(id, pose));
}
}
corelib/src/camera/CameraImages.cpp
+3 -3
View File
@@ -523,19 +523,19 @@ bool CameraImages::readPoses(
UERROR("Cannot read pose file \"%s\".", filePath.c_str());
return false;
}
else if((format != 1 && format != 10 && format != 5 && format != 6 && format != 7 && format != 9) && poses.size() != this->imagesCount())
else if((format != 1 && format != 10 && format != 12 && format != 5 && format != 6 && format != 7 && format != 9) && poses.size() != this->imagesCount())
{
UERROR("The pose count is not the same as the images (%d vs %d)! Please remove "
"the pose file path if you don't want to use it (current file path=%s).",
(int)poses.size(), this->imagesCount(), filePath.c_str());
return false;
}
else if((format == 1 || format == 10 || format == 5 || format == 6 || format == 7 || format == 9) && (inOutStamps.empty() && stamps.size()!=poses.size()))
else if((format == 1 || format == 10 || format == 12 || format == 5 || format == 6 || format == 7 || format == 9) && (inOutStamps.empty() && stamps.size()!=poses.size()))
{
UERROR("When using RGBD-SLAM, GPS, MALAGA, ST LUCIA and EuRoC MAV formats, images must have timestamps!");
return false;
}
else if(format == 1 || format == 10 || format == 5 || format == 6 || format == 7 || format == 9)
else if(format == 1 || format == 10 || format == 12 || format == 5 || format == 6 || format == 7 || format == 9)
{
UDEBUG("");
//Match ground truth values with images
guilib/src/ui/preferencesDialog.ui
+13 -3
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-1145</y>
<y>0</y>
<width>713</width>
<height>4705</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>9</number>
<number>5</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -3227,7 +3227,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="2" column="1">
<widget class="QLabel" name="label_42">
<property name="text">
<string>Local transform from /base_link to /camera_link. Mouse over the box to show formats. If odometry sensor is enabled, it is the transform to odometry sensor.</string>
<string>Local transform from /base_link to /camera_link (without optical transform). Mouse over the box to show formats. If odometry sensor is enabled, it is the transform to odometry sensor.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -7437,6 +7437,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>RGBD-SLAM + ID</string>
</property>
</item>
<item>
<property name="text">
<string>rgbd_bonn</string>
</property>
</item>
</widget>
</item>
<item row="11" column="0">
@@ -7582,6 +7587,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>RGBD-SLAM + ID</string>
</property>
</item>
<item>
<property name="text">
<string>rgbd_bonn</string>
</property>
</item>
</widget>
</item>
<item row="6" column="0">
tools/RgbdDataset/main.cpp
+35 -4
View File
@@ -58,6 +58,7 @@ void showUsage()
" --output Output directory. By default, results are saved in \"path\".\n"
" --output_name Output database name (default \"rtabmap\").\n"
" --skip # Skip X frames.\n"
" --max_time_diff #.# Maximum time difference with frame to attribute a valid ground truth pose (default 0.02 s).\n"
" --quiet Don't show log messages and iteration updates.\n"
"%s\n"
"Example:\n\n"
@@ -92,6 +93,7 @@ int main(int argc, char * argv[])
std::string output;
std::string outputName = "rtabmap";
int skipFrames = 0;
float maxTimeDiff = 0.02f;
bool quiet = false;
if(argc < 2)
{
@@ -114,6 +116,11 @@ int main(int argc, char * argv[])
skipFrames = atoi(argv[++i]);
UASSERT(skipFrames > 0);
}
else if(std::strcmp(argv[i], "--max_time_diff") == 0)
{
maxTimeDiff = atof(argv[++i]);
UASSERT(maxTimeDiff > 0.0f);
}
else if(std::strcmp(argv[i], "--quiet") == 0)
{
quiet = true;
@@ -158,14 +165,16 @@ int main(int argc, char * argv[])
" Depth path: %s\n"
" Output: %s\n"
" Output name: %s\n"
" Skip frames: %d\n",
" Skip frames: %d\n"
" Max time diff: %f\n",
seq.c_str(),
path.c_str(),
pathRgbImages.c_str(),
pathDepthImages.c_str(),
output.c_str(),
outputName.c_str(),
skipFrames);
skipFrames,
maxTimeDiff);
if(!pathGt.empty())
{
printf(" groundtruth.txt: %s\n", pathGt.c_str());
@@ -185,6 +194,7 @@ int main(int argc, char * argv[])
std::string sequenceName = UFile(path).getName();
Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
float depthFactor = 5.0f;
int gtPoseFormat = 1;
if(sequenceName.find("freiburg1") != std::string::npos)
{
model = CameraModel(outputName+"_calib", 517.3, 516.5, 318.6, 255.3, opticalRotation, 0, cv::Size(640,480));
@@ -193,10 +203,30 @@ int main(int argc, char * argv[])
{
model = CameraModel(outputName+"_calib", 520.9, 521.0, 325.1, 249.7, opticalRotation, 0, cv::Size(640,480));
}
else //if(sequenceName.find("freiburg3") != std::string::npos)
else if(sequenceName.find("freiburg3") != std::string::npos)
{
model = CameraModel(outputName+"_calib", 535.4, 539.2, 320.1, 247.6, opticalRotation, 0, cv::Size(640,480));
}
else if(sequenceName.find("rgbd_bonn") != std::string::npos)
{
cv::Mat K = cv::Mat::eye(3,3,CV_64FC1);
K.at<double>(0,0) = 542.822841; // fx
K.at<double>(1,1) = 542.576870; // fy
K.at<double>(0,2) = 315.593520; // cx
K.at<double>(1,2) = 237.756098; // cy
cv::Mat D = cv::Mat::eye(1,5,CV_64FC1);
D.at<double>(0,0) = 0.039903;
D.at<double>(0,1) = -0.099343;
D.at<double>(0,2) = -0.000730;
D.at<double>(0,3) = -0.000144;
D.at<double>(0,4) = 0.000000;
model = CameraModel(outputName+"_calib", cv::Size(640,480), K, D, cv::Mat(), cv::Mat(), opticalRotation);
gtPoseFormat = 12;
}
else {
printf("ERROR: Dataset %s is not supported. Update rgbd_dataset tool to include the right calibration parameter for this dataset!\n", sequenceName.c_str());
}
//parameters.insert(ParametersPair(Parameters::kg2oBaseline(), uNumber2Str(40.0f/model.fx())));
model.save(path);
@@ -209,7 +239,8 @@ int main(int argc, char * argv[])
((CameraRGBDImages*)cameraThread.camera())->setTimestamps(true, "", false);
if(!pathGt.empty())
{
((CameraRGBDImages*)cameraThread.camera())->setGroundTruthPath(pathGt, 1);
((CameraRGBDImages*)cameraThread.camera())->setGroundTruthPath(pathGt, gtPoseFormat);
((CameraRGBDImages*)cameraThread.camera())->setMaxPoseTimeDiff(maxTimeDiff);
}
bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();