C++ Vector4d::transpose方法代码示例

void BezierBoundarySolver::_IterateCurvatureReduction(BezierBoundaryProblem* pProblem,Eigen::Vector4d& params)
{
    double epsilon = 0.0001;
    //create a jacobian for the parameters by perturbing them
    Eigen::Vector4d Jt; //transpose of the jacobian
    BezierBoundaryProblem origProblem = *pProblem;
    double maxK = _GetMaximumCurvature(pProblem);
    for(int ii = 0; ii < 4 ; ii++){
        Eigen::Vector4d epsilonParams = params;
        epsilonParams[ii] += epsilon;
        _Get5thOrderBezier(pProblem,epsilonParams);
        _Sample5thOrderBezier(pProblem);
        double kPlus = _GetMaximumCurvature(pProblem);

        epsilonParams[ii] -= 2*epsilon;
        _Get5thOrderBezier(pProblem,epsilonParams);
        _Sample5thOrderBezier(pProblem);
        double kMinus = _GetMaximumCurvature(pProblem);
        Jt[ii] = (kPlus-kMinus)/(2*epsilon);
    }

    //now that we have Jt, we can calculate JtJ
    Eigen::Matrix4d JtJ = Jt*Jt.transpose();
    //thikonov regularization
    JtJ += Eigen::Matrix4d::Identity();

    Eigen::Vector4d deltaParams = JtJ.inverse() * Jt*maxK;
    params -= deltaParams;
    _Get5thOrderBezier(pProblem,params);
    _Sample5thOrderBezier(pProblem);
    //double finalMaxK = _GetMaximumCurvature(pProblem);

    //dout("2D Iteration took k from " << maxK << " to " << finalMaxK);
}

    void cloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud)
    {
        if( ( !cloud_updated ) && ( goal_completion_time + ros::Duration(2.0) < cloud->header.stamp ) )
        {
            try
            {
                // update the pose
                listener.waitForTransform( "/arm_kinect_frame", "/map", cloud->header.stamp, ros::Duration(5.0));
                listener.lookupTransform( "/map", "/arm_kinect_frame", cloud->header.stamp, kinect2map);

                listener.waitForTransform( "/arm_kinect_optical_frame", "/map", cloud->header.stamp, ros::Duration(5.0));
                listener.lookupTransform( "/map", "/arm_kinect_optical_frame", cloud->header.stamp, optical2map);

                tf::Vector3 position_( kinect2map.getOrigin() );
                position.x() = position_.x();
                position.y() = position_.y();
                position.z() = position_.z();

                tf::Quaternion orientation_( kinect2map.getRotation() );
                orientation.x() = orientation_.x();
                orientation.y() = orientation_.y();
                orientation.z() = orientation_.z();
                orientation.w() = orientation_.w();

                ROS_INFO_STREAM("position = " << position.transpose() );
                ROS_INFO_STREAM("orientation = " << orientation.transpose() );

                // update the cloud
                pcl::copyPointCloud(*cloud, *xyz_cld_ptr);	// Do I need to copy it?
                //xyz_cld_ptr = cloud;
                cloud_updated = true;
            }
            catch (tf::TransformException ex) {
                ROS_ERROR("%s", ex.what());
            }

        }
    }

const CPoint3DWORLD CLandmark::_getOptimizedLandmarkSTEREOUV( const UIDFrame& p_uFrame, const CPoint3DWORLD& p_vecInitialGuess )
{
    //ds initial values
    Eigen::Matrix4d matH( Eigen::Matrix4d::Zero( ) );
    Eigen::Vector4d vecB( Eigen::Vector4d::Zero( ) );
    CPoint3DHomogenized vecX( CMiniVisionToolbox::getHomogeneous( p_vecInitialGuess ) );
    //Eigen::Matrix2d matOmega( Eigen::Matrix2d::Identity( ) );
    double dErrorSquaredTotalPixelsPREVIOUS = 0.0;

    //ds iterations (break-out if convergence reached early)
    for( uint32_t uIteration = 0; uIteration < CLandmark::uCapIterations; ++uIteration )
    {
        //ds counts
        double dErrorSquaredTotalPixels = 0.0;
        uint32_t uInliers               = 0;

        //ds initialize setup
        matH.setZero( );
        vecB.setZero( );

        //ds do calibration over all recorded values
        for( const CMeasurementLandmark* pMeasurement: m_vecMeasurements )
        {
            //ds apply the projection to the transformed point
            const Eigen::Vector3d vecABCLEFT  = pMeasurement->matProjectionWORLDtoLEFT*vecX;
            const Eigen::Vector3d vecABCRIGHT = pMeasurement->matProjectionWORLDtoRIGHT*vecX;

            //ds buffer c value
            const double dCLEFT  = vecABCLEFT.z( );
            const double dCRIGHT = vecABCRIGHT.z( );

            //ds compute error
            const Eigen::Vector2d vecUVLEFT( vecABCLEFT.x( )/dCLEFT, vecABCLEFT.y( )/dCLEFT );
            const Eigen::Vector2d vecUVRIGHT( vecABCRIGHT.x( )/dCRIGHT, vecABCRIGHT.y( )/dCRIGHT );
            const Eigen::Vector4d vecError( vecUVLEFT.x( )-pMeasurement->ptUVLEFT.x,
                                            vecUVLEFT.y( )-pMeasurement->ptUVLEFT.y,
                                            vecUVRIGHT.x( )-pMeasurement->ptUVRIGHT.x,
                                            vecUVRIGHT.y( )-pMeasurement->ptUVRIGHT.y );

            //ds current error
            const double dErrorSquaredPixels = vecError.transpose( )*vecError;

            //std::printf( "[%06lu][%04u] error: %4.2f %4.2f %4.2f %4.2f (squared: %4.2f)\n", uID, uIteration, vecError(0), vecError(1), vecError(2), vecError(3) , dErrorSquaredPixels );

            //ds check if outlier
            double dWeight = 1.0;
            if( dKernelMaximumErrorSquaredPixels < dErrorSquaredPixels )
            {
                dWeight = dKernelMaximumErrorSquaredPixels/dErrorSquaredPixels;
            }
            else
            {
                ++uInliers;
            }
            dErrorSquaredTotalPixels += dWeight*dErrorSquaredPixels;

            //ds jacobian of the homogeneous division
            Eigen::Matrix< double, 2, 3 > matJacobianLEFT;
            matJacobianLEFT << 1/dCLEFT,          0, -vecABCLEFT.x( )/( dCLEFT*dCLEFT ),
                                      0,   1/dCLEFT, -vecABCLEFT.y( )/( dCLEFT*dCLEFT );

            Eigen::Matrix< double, 2, 3 > matJacobianRIGHT;
            matJacobianRIGHT << 1/dCRIGHT,           0, -vecABCRIGHT.x( )/( dCRIGHT*dCRIGHT ),
                                        0,   1/dCRIGHT, -vecABCRIGHT.y( )/( dCRIGHT*dCRIGHT );

            //ds final jacobian
            Eigen::Matrix< double, 4, 4 > matJacobian;
            matJacobian.setZero( );
            matJacobian.block< 2,4 >(0,0) = matJacobianLEFT*pMeasurement->matProjectionWORLDtoLEFT;
            matJacobian.block< 2,4 >(2,0) = matJacobianRIGHT*pMeasurement->matProjectionWORLDtoRIGHT;

            //ds precompute transposed
            const Eigen::Matrix< double, 4, 4 > matJacobianTransposed( matJacobian.transpose( ) );

            //ds accumulate
            matH += dWeight*matJacobianTransposed*matJacobian;
            vecB += dWeight*matJacobianTransposed*vecError;
        }

        //ds solve constrained system (since dx(3) = 0.0) and update x solution
        vecX.block< 3,1 >(0,0) += matH.block< 4, 3 >(0,0).householderQr( ).solve( -vecB );

        //std::printf( "[%06lu][%04u]: %6.2f %6.2f %6.2f %6.2f (delta 2norm: %f inliers: %u)\n", uID, uIteration, vecX.x( ), vecX.y( ), vecX.z( ), vecX(3), vecDeltaX.squaredNorm( ), uInliers );

        //std::fprintf( m_pFilePosition, "%04lu %06lu %03u %03lu %03u %6.2f\n", p_uFrame, uID, uIteration, m_vecMeasurements.size( ), uInliers, dRSSCurrent );

        //ds check if we have converged
        if( CLandmark::dConvergenceDelta > std::fabs( dErrorSquaredTotalPixelsPREVIOUS-dErrorSquaredTotalPixels ) )
        {
            //ds compute average error
            const double dErrorSquaredAveragePixels = dErrorSquaredTotalPixels/m_vecMeasurements.size( );

            //ds if acceptable inlier/outlier ratio
            if( CLandmark::dMinimumRatioInliersToOutliers < static_cast< double >( uInliers )/m_vecMeasurements.size( ) )
            {
                //ds success
                ++uOptimizationsSuccessful;

                //ds update average
                dCurrentAverageSquaredError = dErrorSquaredAveragePixels;
//.........这里部分代码省略.........