Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Problems during calculation of loop constraints #28

Open
SlamCabbage opened this issue Apr 5, 2023 · 0 comments
Open

Problems during calculation of loop constraints #28

SlamCabbage opened this issue Apr 5, 2023 · 0 comments

Comments

@SlamCabbage
Copy link

float x, y, z, roll, pitch, yaw;
Eigen::Affine3f correctionLidarFrame;
correctionLidarFrame = icp.getFinalTransformation();
// transform from world origin to wrong pose
Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);
// transform from world origin to corrected pose
Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed frame
pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);
gtsam::Vector Vector6(6);
float noiseScore = icp.getFitnessScore();
Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);

On line 676 in mapOptmization.cpp, the code uses
Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;
As I understand it, if the current pointcloud is used as the source and the loop pointcloud is used as the target in ICP, then the resulting pose transformation should be $T_{correct}^{wrong}$, which should be in the form of right multiplication.
tWrong is $T_{wrong}^{world}$ ,
tCorrect is $T_{correct}^{world}$ .
we can launch:
$T_{correct}^{world} = T_{wrong}^{world} \cdot T_{correct}^{wrong}$
Eigen::Affine3f tCorrect = tWrong * correctionLidarFrame;

Sorry for taking up your time, could you help me with my doubts?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant