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

const double CMiniVisionToolbox::getTransformationDelta( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo )
{
    //ds compute pose change
    const Eigen::Isometry3d matTransformChange( p_matTransformationTo*p_matTransformationFrom.inverse( ) );

    //ds check point
    const Eigen::Vector4d vecSamplePoint( 40.2, -1.25, 2.5, 1.0 );
    double dNorm( vecSamplePoint.norm( ) );
    const Eigen::Vector4d vecDifference( vecSamplePoint-matTransformChange*vecSamplePoint );
    dNorm = ( dNorm + vecDifference.norm( ) )/2;

    //ds return norm
    return ( std::fabs( vecDifference(0) ) + std::fabs( vecDifference(1) ) + std::fabs( vecDifference(2) ) )/dNorm;
}

 inline sm::kinematics::Transformation transformationFromPropertyTree(const sm::ConstPropertyTree & config)
 {
     Eigen::Vector4d q = sm::eigen::quaternionFromPropertyTree( sm::ConstPropertyTree(config, "q_a_b") );
     q = q / q.norm();
     Eigen::Vector3d t = sm::eigen::vector3FromPropertyTree( sm::ConstPropertyTree(config, "t_a_b_a" ) );
     return sm::kinematics::Transformation(q,t);
 }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW /* Structures containing 
				       "fixed-sized vectorizable" Eigen classes
				       need proper memory alignment */
    Ray(const Eigen::Vector4d &orig, const Eigen::Vector4d &direction, const int fromObj=ID_AIR) :
	origin(orig), dir(direction), fromObj(fromObj) {
	BOOST_ASSERT_MSG(orig[3] == 1, 
			 "Origin of ray must have 4th coord equal to 1");
	BOOST_ASSERT_MSG(direction[3] == 0, 
			 "Direction of ray must have 4th coord equal to 0");
	BOOST_ASSERT_MSG(std::abs(direction.norm() - 1) < EPSILON, "Require unit direction vector!");
    }

void randomUnitQuaternion(Eigen::Vector4d &quat) {
    static boost::random::mt19937 rng(time(NULL));
    static boost::random::normal_distribution<> normal;

    do {
        quat(0) = normal(rng);
        quat(1) = normal(rng);
        quat(2) = normal(rng);
        quat(3) = normal(rng);
    } while (quat.norm() < 0.00001);
    quat.normalize();
}

bool WHeadPositionCorrection::checkMovementThreshold( const WLEMDHPI::TransformationT& trans )
{
    const Eigen::Vector4d diffTrans = m_transExc.data().col( 3 ) - trans.data().col( 3 );
    if( diffTrans.norm() > m_movementThreshold )
    {
        m_transExc = trans;
        return true;
    }
    // TODO(pieloth): check rotation, e.g. with 1 or more key points
    // initial: pick 6 key points from dipoles min/max (x,0,0); (0,y,0); (0,0,z)
    // compare max distance
    // keyPoint 3x6
    // (trans * keyPoints).colwise().norm() - (m_transExc * keyPoints).colwise().norm()).maxCoeff(), mind homog. coord.
    return false;
}

TEST(UncertainHomogeneousPointTestSuite, testUav)
{
  try {
    using namespace sm::kinematics;
    Eigen::Vector4d p;
    p.setRandom();
    p = p/p.norm();
    Eigen::Matrix3d U;
    U = sm::eigen::randomCovariance<3>();
    
    UncertainHomogeneousPoint uhp(p,U);
    
    sm::eigen::assertNear(U, uhp.U_av_form(), 1e-10, SM_SOURCE_FILE_POS, "Checking that I can recover the av/form point uncertainty");
  } 
  catch(std::exception const & e)
    {
      FAIL() << e.what();
    }
}

/*
 * @brief Normalize the given quaternion to unit quaternion.
 */
inline void quaternionNormalize(Eigen::Vector4d& q) {
  double norm = q.norm();
  q = q / norm;
  return;
}

void calculateOverlappingFOV(boost::shared_ptr<CAMERA_1_T> camera1,
                             boost::shared_ptr<CAMERA_1_T> camera2,
                             const sm::kinematics::Transformation& T_cam1_cam2,
                             cameras::ImageMask& outMask1,
                             cameras::ImageMask& outMask2,
                             double& outOverlappingRatio1,
                             double& outOverlappingRatio2,
                             const Eigen::VectorXi& sampleDistances,
                             double scale) {
  // get dimensions
  int width1 = (int) camera1->projection().ru() * scale;
  int height1 = (int) camera1->projection().rv() * scale;
  int width2 = (int) camera2->projection().ru() * scale;
  int height2 = (int) camera2->projection().rv() * scale;

  // clear incoming masks
  cv::Mat outMask1CV = outMask1.getMask();
  cv::Mat outMask2CV = outMask2.getMask();
  outMask1CV = cv::Mat::zeros(height1, width1, CV_8UC1);
  outMask2CV = cv::Mat::zeros(height2, width2, CV_8UC1);
  outMask1.setScale(scale);
  outMask2.setScale(scale);

  Eigen::Vector4d point;

  // build outMask1
  outOverlappingRatio1 = 0;
  for (int x = 0; x < width1; x++) {
    for (int y = 0; y < height1; y++) {
      // if visible
      // TODO: BB: what about rounding mistakes?
      if (camera1->projection().keypointToHomogeneous(
          Eigen::Vector2d((int) x / scale, (int) y / scale), point)) {
        double norm = point.norm();
        int i = 0;
        // check points on the beam until one is found or no other points to check
        while (i < sampleDistances.size()
            && outMask1CV.at<unsigned char>(y, x) == 0) {
          // Send point to distance sampleDistances(i) on the beam
          Eigen::Vector4d tempPoint = point;
          tempPoint(3) = norm / (norm + sampleDistances(i));

          // Project point to other camera frame
          tempPoint = T_cam1_cam2.inverse() * tempPoint;

          Eigen::Vector2d keypointLocation;
          if (camera2->projection().homogeneousToKeypoint(tempPoint,
                                                          keypointLocation)) {
            outMask1CV.at<unsigned char>(y, x) = 255;
            outOverlappingRatio1++;
          }  // if
          i++;
        }  // while
      }  // if
    }  // for
  }  // for
  outOverlappingRatio1 = outOverlappingRatio1 / (width1 * height1);

  // build outMask2
  outOverlappingRatio2 = 0;
  for (int x = 0; x < width2; x++) {
    for (int y = 0; y < height2; y++) {
      // if visible
      // TODO: BB: what about rounding mistakes?
      if (camera2->projection().keypointToHomogeneous(
          Eigen::Vector2d((int) x / scale, (int) y / scale), point)) {
        double norm = point.norm();
        int i = 0;
        // check points on the beam until one is found or no other points to check
        while (i < sampleDistances.size()
            && outMask2CV.at<unsigned char>(y, x) == 0) {
          // Send point to distance sampleDistances(i) on the beam
          Eigen::Vector4d tempPoint = point;
          tempPoint(3) = norm / (norm + sampleDistances(i));

          // Project point to other camera frame
          tempPoint = T_cam1_cam2 * tempPoint;

          Eigen::Vector2d keypointLocation;
          if (camera1->projection().homogeneousToKeypoint(tempPoint,
                                                          keypointLocation)) {
            outMask2CV.at<unsigned char>(y, x) = 255;
            outOverlappingRatio2++;
          }  // if
          i++;
        }  // while
      }  // if
    }  // for
  }  // for
  outOverlappingRatio2 = outOverlappingRatio2 / (width2 * height2);
}