Straightening a trajectory

[dagata-mining]

Hi,
I'm trying to straighten a trajectory so that there are almost no deviation in x and y but only movement in z. As you can see in the picture

I have found an approach but its not perfect and I'm still getting deviations (going from 1.25% to 0.25%) but I'm looking for better results. I don't know if my approach is suitable and maybe you could help me on this one. Here's the steps I'm taking

1. Create key prior poses at each x amount of meters with very small variance
2. Set the key prior poses Transform so that there is no deviation
3. Add factors between the prior poses with very small variance
4. Add values of the initial state poses and factors with big variance
5. Optimize solution

void generateGraph(PointCloudSignatureList pointCloudSignatureList, float vDistance, float epsilon , int iterations)
    {
        gtsam::NonlinearFactorGraph graph;

        // Add Poses
        gtsam::Values initialEstimate;
        for (size_t i = 0 ; i < pointCloudSignatureList.m_signatures.size(); i++)
        {
            std::shared_ptr<PointCloudSignature> signature = pointCloudSignatureList.m_signatures[i];
            initialEstimate.insert(signature->m_id,gtsam::Pose3(signature->m_transform.toEigen4d()));
        }
        std::cout<< "Pose Added" <<std::endl;

        // Add Priors
        gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
        (gtsam::Vector(6) <<
                          1e-7,  1e-7,  1e-7,
                1e-7, 1e-7,  1e-7).finished());
        gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(
                (gtsam::Vector(6) << 0.1,0.1,0.1,0.01,0.01,0.01).finished());

        std::shared_ptr<PointCloudSignature>  prevSignature;

        int verticalId = pointCloudSignatureList.getBiggestId() + 1;
        int prevId;
        for (size_t i = 0 ; i < pointCloudSignatureList.m_signatures.size(); i++)
        {
            std::shared_ptr<PointCloudSignature> signature = pointCloudSignatureList.m_signatures[i];
//            gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(
//                    (gtsam::Vector(6) <<
//                                      signature->m_covariance.at<double>(0,0), signature->m_covariance.at<double>(1,1), signature->m_covariance.at<double>(2,2),
//                            signature->m_covariance.at<double>(3,3), signature->m_covariance.at<double>(4,4), signature->m_covariance.at<double>(5,5)).finished());
            if (i == 0)
            {
                prevSignature = signature;
                Transform priorPose = signature->m_transform;

                // Add prior pose
                graph.add(gtsam::PriorFactor<gtsam::Pose3>(signature->m_id, gtsam::Pose3(priorPose.toEigen4d()), priorNoise));

                // Add Link to pior pose
                prevSignature = signature;
                prevId = verticalId;
                verticalId ++;
            }
            else if ( i + 1 == pointCloudSignatureList.m_signatures.size() || prevSignature->m_transform.getDistance(signature->m_transform) > vDistance)
            {
                // Adding new Prior
                Transform priorPose = Transform(0,0,signature->m_transform.z(),0,0,0) ;
                Transform betweenFactor = priorPose * signature->m_transform.inverse();
                betweenFactor = Transform(0,0,0,0,0,0);

                //Prior Pose
                initialEstimate.insert(verticalId,gtsam::Pose3(signature->m_transform.toEigen4d()));
                graph.add(gtsam::BetweenFactor<gtsam::Pose3>(pointCloudSignatureList.m_signatures.front()->m_id, verticalId , gtsam::Pose3(signature->m_transform.toEigen4d()),priorNoise));
                //graph.add(gtsam::BetweenFactor<gtsam::Pose3>(i, verticalId , gtsam::Pose3(Transform(0,0,0,0,0,0).toEigen4d()),priorNoise));
                graph.add(gtsam::PriorFactor<gtsam::Pose3>(verticalId, gtsam::Pose3(priorPose.toEigen4d()), priorNoise));

                prevSignature = signature;
                prevId = verticalId;
                verticalId ++;
            }
        }

        // Generate vertical Priors from know poses with only Z translation no rotation and X,Y
        std::cout<< "Priors Added" <<std::endl;

        // Set covariances between factors
        std::shared_ptr<PointCloudSignature> prevFactor = pointCloudSignatureList.m_signatures.front();

        for (size_t  i = 0 ; i < pointCloudSignatureList.m_signatures.size(); i++)
        {
            std::shared_ptr<PointCloudSignature> signature = pointCloudSignatureList.m_signatures[i];
            if (i != 0)
            {
//                gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(
//                        (gtsam::Vector(6) <<
//                                          signature->m_covariance.at<double>(0,0), signature->m_covariance.at<double>(1,1), signature->m_covariance.at<double>(2,2),
//                                signature->m_covariance.at<double>(3,3), signature->m_covariance.at<double>(4,4), signature->m_covariance.at<double>(5,5)).finished());

                Transform t = signature->m_transform * prevFactor->m_transform.inverse();
                graph.add(gtsam::BetweenFactor<gtsam::Pose3>(pointCloudSignatureList.m_signatures.front()->m_id, signature->m_id , gtsam::Pose3(signature->m_transform.toEigen4d()),model));
            }
            prevFactor = signature;
        }
        std::cout<< "Factor Added" <<std::endl;

        //Generate optimizer
        gtsam::NonlinearOptimizer * optimize;
        gtsam::LevenbergMarquardtParams parameters;
        parameters.relativeErrorTol = epsilon;
        parameters.maxIterations = iterations;
        optimize = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters);

        int it = 0;
        double lastError = optimize->error();
        for(int i=0; i<iterations; ++i)
        {
            try
            {
                optimize->iterate();
                ++it;
            }
            catch(gtsam::IndeterminantLinearSystemException & e)
            {
                std::cout <<"GTSAM exception caught:" << e.what() << std::endl;
                delete optimize;
                return;
            }
            double error = optimize->error();
            std::cout << "iteration "<<std::to_string(i+1)<< " error " << error;
            double errorDelta = lastError - error;
            if(i>0 && errorDelta < epsilon)
            {
                if(errorDelta < 0)
                {
                    std::cout<< "Negative improvement?! Ignore and continue optimizing..." << std::endl;
                }
                else
                {
                    std::cout<<"Stop optimizing, not enough improvement"<< std::endl;
                    break;
                }
            }
            else if(i==0 && error < epsilon)
            {
                std::cout << "Stop optimizing, error is already under epsilon"<<std::endl;
                break;
            }
            lastError = error;
        }
        std::cout<< "Get poses" <<std::endl;
        // Get the poses
        for(gtsam::Values::const_iterator iter=optimize->values().begin(); iter!=optimize->values().end(); ++iter)
        {
            int key = (int)iter->key;
            if (iter->value.dim() > 1)
            {
                if(key > 0)
                {
                    gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
                    std::shared_ptr<PointCloudSignature> signature;
                    for (size_t  i = 0 ; i < pointCloudSignatureList.m_signatures.size(); i++)
                    {
                        signature = pointCloudSignatureList.m_signatures[i];
                        if (signature -> m_id == key)
                        {
                            signature-> m_transform = Transform::fromEigen4d(p.matrix());
                            break;
                        }
                    }
                }
            }
        }
        delete optimize;
        return;
    }

Small help here would be much appreciated

[ProfFan]

You are probably looking for a constrained optimization method, try looking at the constrained noise model?

[dagata-mining]

Will try that Asap! Thanks for the help. I'll keep you guys updated.

[dagata-mining]

Worked like a charm!! Many Thanks!