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

/* ************************************************************************* */
double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
                    OptionalJacobian<1, 3> H2) const {
  Matrix36 D_local_pose;
  Matrix3 D_local_point;
  Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
  if (!H1 && !H2) {
    return local.norm();
  } else {
    Matrix13 D_r_local;
    const double r = local.norm(D_r_local);
    if (H1) *H1 = D_r_local * D_local_pose;
    if (H2) *H2 = D_r_local * D_local_point;
    return r;
  }
}

double ComputeRayAngle(Point2 p, Point2 q, const Camera& cam1, const Camera& cam2)
{
    const Point3 p3n = cam1.GetIntrinsicMatrix().inverse() * EuclideanToHomogenous(p);
    const Point3 q3n = cam2.GetIntrinsicMatrix().inverse() * EuclideanToHomogenous(q);

    const Point2 pn = p3n.head<2>() / p3n.z();
    const Point2 qn = q3n.head<2>() / q3n.z();

    const Point3 p_w = cam1.m_R.transpose() * Point3{pn.x(), pn.y(), -1.0};
    const Point3 q_w = cam2.m_R.transpose() * Point3{qn.x(), qn.y(), -1.0};

    // Compute the angle between the rays
    const double dot = p_w.dot(q_w);
    const double mag = p_w.norm() * q_w.norm();

    return acos(util::clamp((dot / mag), (-1.0 + 1.0e-8), (1.0 - 1.0e-8)));
}

//*************************************************************************
double norm_proxy(const Point3& point) {
  return double(point.norm());
}

#ifndef BTL_AO_NORM_POSE_HEADER
#define BTL_AO_NORM_POSE_HEADER

#include "common/OtherUtil.hpp"
#include <limits>
#include <Eigen/Dense>
#include "NormalAOPoseAdapter.hpp"
#include "AbsoluteOrientation.hpp"

using namespace Eigen;
using namespace std;

template<typename POSE_T, typename POINT_T>
Matrix<POSE_T, 3, 1> find_opt_cc(NormalAOPoseAdapter<POSE_T, POINT_T>& adapter)
{
	//the R has been fixed, we need to find optimal cc, camera center, given n pairs of 2-3 correspondences
	//Slabaugh, G., Schafer, R., & Livingston, M. (2001). Optimal Ray Intersection For Computing 3D Points From N -View Correspondences.
	typedef Matrix<POSE_T, 3, 1> V3;
	typedef Matrix<POSE_T, 3, 3> M3;

	M3 Rwc = adapter.getRcw().inverse().matrix();
	M3 AA; AA.setZero();
	V3 bb; bb.setZero();
	for (int i = 0; i < adapter.getNumberCorrespondences(); i++)
	{
		if (adapter.isInlier23(i)){
			V3 vr_w = Rwc * adapter.getBearingVector(i).template cast<POSE_T>();
			M3 A;
			A(0,0) = 1 - vr_w(0)*vr_w(0);
			A(1,0) = A(0,1) = - vr_w(0)*vr_w(1);
			A(2,0) = A(0,2) = - vr_w(0)*vr_w(2);
			A(1,1) = 1 - vr_w(1)*vr_w(1);
			A(2,1) = A(1,2) = - vr_w(1)*vr_w(2);
			A(2,2) = 1 - vr_w(2)*vr_w(2);
			V3 b = A * adapter.getPointGlob(i).template cast<POSE_T>();
			AA += A;
			bb += b;
		}
	}
	V3 c_w;
	if (fabs(AA.determinant()) < POSE_T(0.0001))
		c_w = V3(numeric_limits<POSE_T>::quiet_NaN(), numeric_limits<POSE_T>::quiet_NaN(), numeric_limits<POSE_T>::quiet_NaN());
	else
		c_w = AA.jacobiSvd(ComputeFullU | ComputeFullV).solve(bb);
	return c_w;
}

template< typename POSE_T, typename POINT_T >
bool assign_sample(const NormalAOPoseAdapter<POSE_T, POINT_T>& adapter, 
	const vector<int>& selected_cols_, 
	Matrix<POINT_T, Dynamic, Dynamic>* p_X_w_, Matrix<POINT_T, Dynamic, Dynamic>* p_N_w_, 
	Matrix<POINT_T, Dynamic, Dynamic>* p_X_c_, Matrix<POINT_T, Dynamic, Dynamic>* p_N_c_, Matrix<POINT_T, Dynamic, Dynamic>* p_bv_){
	
	int K = (int)selected_cols_.size() - 1;
	bool use_shinji = false;
	int nValid = 0;
	for (int nSample = 0; nSample < K; nSample++) {
		p_X_w_->col(nSample) = adapter.getPointGlob(selected_cols_[nSample]);
		p_N_w_->col(nSample) = adapter.getNormalGlob(selected_cols_[nSample]);
		p_bv_->col(nSample) = adapter.getBearingVector(selected_cols_[nSample]);
		if (adapter.isValid(selected_cols_[nSample])){
			p_X_c_->col(nSample) = adapter.getPointCurr(selected_cols_[nSample]);
			p_N_c_->col(nSample) = adapter.getNormalCurr(selected_cols_[nSample]);
			nValid++;
		}
	}
	if (nValid == K)
		use_shinji = true;
	//assign the fourth elements for 
	p_X_w_->col(3) = adapter.getPointGlob(selected_cols_[3]);
	p_N_w_->col(3) = adapter.getNormalGlob(selected_cols_[3]);
	p_bv_->col(3) = adapter.getBearingVector(selected_cols_[3]);

	return use_shinji;
}

template<typename POSE_T, typename POINT_T>
void nl_2p( const Matrix<POINT_T,3,1>& pt1_c, const Matrix<POINT_T,3,1>& nl1_c, const Matrix<POINT_T,3,1>& pt2_c, 
			const Matrix<POINT_T,3,1>& pt1_w, const Matrix<POINT_T,3,1>& nl1_w, const Matrix<POINT_T,3,1>& pt2_w, 
			SE3Group<POSE_T>* p_solution){
	//Drost, B., Ulrich, M., Navab, N., & Ilic, S. (2010). Model globally, match locally: Efficient and robust 3D object recognition. In CVPR (pp. 998?005). Ieee. http://doi.org/10.1109/CVPR.2010.5540108
	typedef Matrix<POINT_T, Dynamic, Dynamic> MatrixX;
	typedef Matrix<POSE_T, 3, 1> V3;
	typedef SO3Group<POSE_T> ROTATION;
	typedef SE3Group<POSE_T> RT;

	V3 c_w = pt1_w.template cast<POSE_T>(); // c_w is the origin of coordinate g w.r.t. world

	POSE_T alpha = acos(nl1_w(0)); // rotation nl1_c to x axis (1,0,0)
	V3 axis( 0, nl1_w(2), -nl1_w(1)); //rotation axis between nl1_c to x axis (1,0,0) i.e. cross( nl1_w, x );
	axis.normalized();

	//verify quaternion and rotation matrix
	Quaternion<POSE_T> q_g_f_w(AngleAxis<POSE_T>(alpha, axis));
	//cout << q_g_f_w << endl;
	ROTATION R_g_f_w(q_g_f_w);
	//cout << R_g_f_w << endl;

	V3 nl_x = R_g_f_w * nl1_w.template cast<POSE_T>();
	axis.normalized();
//.........这里部分代码省略.........