Some confusion about merge map optimization

[xiaoxueshengyao]

Thanks for your awesome work, I was deeply inspired. While I had a little doubt about the code. 1、In mergeMap() function, you put the last pose as odomtryFactor out of the loop. The paper just put them all. Why dose you do that noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());

gtsam::Pose3 poseFrom = pclPointTogtsamPose3(temporaryCloudKeyPoses6D->points[tempSize - 2 ]);

gtsam::Pose3 poseTo = pclPointTogtsamPose3(temporaryCloudKeyPoses6D->points[tempSize - 1 ]);

gtSAMgraphTM.add(BetweenFactor<Pose3>(tempSize - 2 , tempSize -1, poseFrom.between(poseTo), odometryNoise));

2、There is a IndoorKeyframe in your code, what dose it do?

[xiaoxueshengyao]

Sorry about that, I made a mistake. while, I still have another question. Is it necessary to update isam after added between factor every time? It's still in the mergeMap() function.

for (int i = priorNode; i < tempSize - 2; i++)
        {
            noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) <<1e-4, 1e-4, 1e-4, 1e-3, 1e-3, 1e-3 ).finished());

            gtsam::Pose3 poseFrom = pclPointTogtsamPose3(temporaryCloudKeyPoses6D->points[i]);

            gtsam::Pose3 poseTo   = pclPointTogtsamPose3(temporaryCloudKeyPoses6D->points[i+1]);

            gtSAMgraphTM.add(BetweenFactor<Pose3>(i,i+1, poseFrom.between(poseTo), odometryNoise));

            initialEstimateTM.insert(i+1, poseTo);

            // update iSAM
            isamTM->update(gtSAMgraphTM, initialEstimateTM);

            isamTM->update();

            gtSAMgraphTM.resize(0);

            initialEstimateTM.clear();

        }