i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

RegVis: using toSignature camera model for guess reprojection on optical flow mode (#947)

Browse Source
This commit is contained in:
matlabbe
2022-12-20 20:51:56 -08:00
parent 4a6a765cd1
commit f12cc83fc2
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
corelib/src/RegistrationVis.cpp
View File

@@ -490,9 +490,9 @@ Transform RegistrationVis::computeTransformationImpl(
bool guessSet = !guess.isIdentity() && !guess.isNull(); bool guessSet = !guess.isIdentity() && !guess.isNull();
if(guessSet) if(guessSet)
{ {
if(fromSignature.sensorData().cameraModels().size() == 1 || fromSignature.sensorData().cameraModels().size() == 1) if(toSignature.sensorData().cameraModels().size() == 1 || toSignature.sensorData().stereoCameraModels().size() == 1)
{ {
Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModels()[0].left().localTransform(); Transform localTransform = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].localTransform():toSignature.sensorData().stereoCameraModels()[0].left().localTransform();
Transform guessCameraRef = (guess * localTransform).inverse(); Transform guessCameraRef = (guess * localTransform).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) << cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(), (double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
@@ -501,7 +501,7 @@ Transform RegistrationVis::computeTransformationImpl(
cv::Mat rvec(1,3, CV_64FC1); cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec); cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z()); cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModels()[0].left().K(); cv::Mat K = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].K():toSignature.sensorData().stereoCameraModels()[0].left().K();
cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo); cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
} }
else else