コード例 #1
1
Eigen::Vector3d CalibrateData::calibrate(Eigen::Vector3d data,
                                         Eigen::Matrix3d alignment_matrix,
                                         Eigen::Matrix3d sensitivity_matrix,
                                         Eigen::Vector3d offset_vector)
{
  Eigen::Vector3d calibrated_data;
  calibrated_data = alignment_matrix.inverse() *
      sensitivity_matrix.inverse() *
      (data - offset_vector);
  return calibrated_data;
}
void GreenStrain_LIMSolver3D::prepareProblemData(std::vector<int>& hessRowIdx, std::vector<int>& hessColIdx)
{
  const int numNodes = mesh->InitalVertices->rows();

  // Compute deformation gradients
  int numTets = mesh->Tetrahedra->rows();
  Ms.resize(4,3*numTets);
  MMTs.resize(4,4*numTets);

  Eigen::Matrix<double,4,3> SelectorM;
  SelectorM.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
  SelectorM.row(3) = Eigen::Vector3d::Ones()*-1;
  
  for(int t=0;t<numTets;t++)
  {
    Eigen::VectorXi indices = TetrahedronVertexIdx.col(t);

    Eigen::Vector3d A = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,0)).cast<double>();
    Eigen::Vector3d B = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,1)).cast<double>();
    Eigen::Vector3d C = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,2)).cast<double>();
    Eigen::Vector3d D = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,3)).cast<double>();

    Eigen::Matrix3d V;
    V << A-D,B-D,C-D;
    
    Eigen::Matrix<double,4,3> Mtemp = SelectorM*V.inverse().cast<double>();
    Ms.block<4,3>(0,3*t) = Mtemp;
    MMTs.block<4,4>(0,4*t) = Mtemp*Mtemp.transpose();
  }
}
コード例 #3
0
/*
 * Given the line parameters  [n_x,n_y,a_x,a_y] check if
 * [n_x, n_y] dot [data.x-a_x, data.y-a_y] < m_delta
 */
bool RSTEstimator::agree(std::vector<double> &parameters, std::pair<Eigen::Vector3d,Eigen::Vector3d> &data)
{
	
	Eigen::Matrix3d R;
	Eigen::Vector3d T;
	Eigen::Vector3d dif;
	double  E1,E2;
	
	
	R << parameters[0] , parameters[1] , parameters[2],
	     parameters[3] , parameters[4] , parameters[5],
	     parameters[6] , parameters[7] , parameters[8];
	
	T << parameters[9] , parameters[10] , parameters[11];
	

    dif = data.first - R*data.second + T; //X21
	E1 = dif.transpose()*dif;
	
	dif = data.second - R.inverse() * (data.first-T); //X12
	E2 = dif.transpose()*dif;
	
	//std::cout << "E1= " << E1 << "\nE2= " << E2 << "\nE1+E2= "<< E1+E2 << " " << this->deltaSquared <<"\n";
	
	return ( (E1 + E2) < this->deltaSquared);
}
コード例 #4
0
Eigen::Matrix3d LinearAlgebra::invMatrixM(const Eigen::Matrix3d& M) {
	Eigen::Matrix3d result;

	// TODO: return the inverse of matrix M
	result = M.inverse();

	return result;
}
コード例 #5
0
ファイル: keyframe.cpp プロジェクト: itsuhane/VINS-Mobile
void KeyFrame::cam2Imu(Eigen::Vector3d T_c_w,
                       Eigen::Matrix3d R_c_w,
                       Eigen::Vector3d &t_w_i,
                       Eigen::Matrix3d &r_w_i)
{
    r_w_i = (qic * R_c_w).inverse();
    t_w_i = -R_c_w.inverse() * T_c_w - r_w_i * tic;
}
コード例 #6
0
ファイル: fitting_cylinder_pdm.cpp プロジェクト: 4ker/pcl
ON_NurbsSurface
FittingCylinder::initNurbsCylinderWithAxes (int order, NurbsDataSurface *data, Eigen::Matrix3d &axes)
{
  Eigen::Vector3d mean;

  unsigned s = unsigned (data->interior.size ());
  mean = NurbsTools::computeMean (data->interior);

  data->mean = mean;

  Eigen::Vector3d v_max (0.0, 0.0, 0.0);
  Eigen::Vector3d v_min (DBL_MAX, DBL_MAX, DBL_MAX);
  for (unsigned i = 0; i < s; i++)
  {
    Eigen::Vector3d p (axes.inverse () * (data->interior[i] - mean));

    if (p (0) > v_max (0))
      v_max (0) = p (0);
    if (p (1) > v_max (1))
      v_max (1) = p (1);
    if (p (2) > v_max (2))
      v_max (2) = p (2);

    if (p (0) < v_min (0))
      v_min (0) = p (0);
    if (p (1) < v_min (1))
      v_min (1) = p (1);
    if (p (2) < v_min (2))
      v_min (2) = p (2);
  }

  int ncpsU (order);
  int ncpsV (2 * order + 4);
  ON_NurbsSurface nurbs (3, false, order, order, ncpsU, ncpsV);
  nurbs.MakeClampedUniformKnotVector (0, 1.0);
  nurbs.MakePeriodicUniformKnotVector (1, 1.0 / (ncpsV - order + 1));

  double dcu = (v_max (0) - v_min (0)) / (ncpsU - 1);
  double dcv = (2.0 * M_PI) / (ncpsV - order + 1);

  double ry = std::max<double> (std::fabs (v_min (1)), std::fabs (v_max (1)));
  double rz = std::max<double> (std::fabs (v_min (2)), std::fabs (v_max (2)));

  Eigen::Vector3d cv_t, cv;
  for (int i = 0; i < ncpsU; i++)
  {
    for (int j = 0; j < ncpsV; j++)
    {
      cv (0) = v_min (0) + dcu * i;
      cv (1) = ry * sin (dcv * j);
      cv (2) = rz * cos (dcv * j);
      cv_t = axes * cv + mean;
      nurbs.SetCV (i, j, ON_3dPoint (cv_t (0), cv_t (1), cv_t (2)));
    }
  }

  return nurbs;
}
コード例 #7
0
const Eigen::Matrix3d CMiniVisionToolbox::getFundamental( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo, const Eigen::Matrix3d& p_matIntrinsic )
{
    //ds get essential matrix
    const Eigen::Matrix3d matEssential( CMiniVisionToolbox::getEssential( p_matTransformationFrom, p_matTransformationTo ) );

    //ds compute fundamental matrix: http://en.wikipedia.org/wiki/Fundamental_matrix_%28computer_vision%29
    const Eigen::Matrix3d matIntrinsicInverse( p_matIntrinsic.inverse( ) );
    return matIntrinsicInverse.transpose( )*matEssential*matIntrinsicInverse;
}
コード例 #8
0
    std::unique_ptr<Image> leastSquarePatch(const std::unique_ptr<Image>& image)
    {
        // process grayscale patches only
        if (image->getChannelNum() > 1)
            return std::unique_ptr<Image>();

        auto data_ptr = image->getValues(0);
        const auto w = image->getWidth();
        const auto h = image->getHeight();

        // least square fit to linear plane for light compensation
        float xsum_orig = 0;
        float ysum_orig = 0;
        float csum_orig = 0;
        for (size_t j = 0; j < h; j++)
        {
            for (size_t i = 0; i < w; i++)
            {
                xsum_orig += (i * data_ptr[j*w + i]);
                ysum_orig += (j * data_ptr[j*w + i]);
                csum_orig += (data_ptr[j*w + i]);
            }
        }
        Eigen::Vector3d vsum(xsum_orig, ysum_orig, csum_orig);

        float x2sum = 0, y2sum = 0, xysum = 0, xsum = 0, ysum = 0;
        float csum = w*h;
        for (size_t j = 0; j < h; j++)
        {
            for (size_t i = 0; i < w; i++)
            {
                x2sum += (i*i);
                y2sum += (j*j);
                xysum += (i*j);
                xsum += i;
                ysum += j;
            }
        }
        Eigen::Matrix3d msum;
        msum << x2sum, xysum, xsum,
             xysum, y2sum, ysum,
             xsum, ysum, csum;
        
        auto vcoeff = msum.inverse() * vsum;
        
        auto newImage = std::make_unique<Image>(w, h, 1);
        for (size_t j = 0; j < h; j++)
        {
            for (size_t i = 0; i < w; i++)
            {
                (newImage->getValues(0))[j*w + i] = data_ptr[j*w + i]
                    - i*vcoeff[0] - j*vcoeff[1] - vcoeff[2];
            }
        }
        return newImage;
    }
コード例 #9
0
int hormodular::Orientation::getRelativeOrientation(int connector, hormodular::Orientation localOrient, hormodular::Orientation remoteOrient)
{
    //-- Create rotation matrices for local orientation:
    Eigen::AngleAxisd rollAngle( deg2rad(localOrient.getRoll()), Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd pitchAngle( deg2rad(localOrient.getPitch()), Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle( deg2rad(localOrient.getYaw()), Eigen::Vector3d::UnitX());

    Eigen::Quaterniond q0 = yawAngle * pitchAngle * rollAngle ;
    Eigen::Matrix3d rotationMatrix = q0.matrix();

    //-- Create rotation matrices for the other orientation:
    Eigen::AngleAxisd otherRollAngle(  deg2rad(remoteOrient.getRoll()), Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd otherPitchAngle( deg2rad(remoteOrient.getPitch()), Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd otherYawAngle(   deg2rad(remoteOrient.getYaw()), Eigen::Vector3d::UnitX());

    Eigen::Quaterniond q1 = otherYawAngle * otherPitchAngle * otherRollAngle;
    Eigen::Matrix3d otherRotationMatrix = q1.matrix();

    Eigen::Matrix3d relativeRotation = rotationMatrix.inverse() * otherRotationMatrix;

    Eigen::Vector3d new_z = relativeRotation * Eigen::Vector3d::UnitZ();

//    std::cout << "New_z: " << std::endl << new_z << std::endl << std::endl;

    //-- Get connector base vector for the rotations:
    Eigen::Vector3d base_vector;
    if ( connector == 0 || connector == 2)
    {
        //-- Y axis
        base_vector = Eigen::Vector3d::UnitY();
    }
    else if ( connector == 1 || connector == 3)
    {
        //-- X axis
        base_vector = Eigen::Vector3d::UnitX();
    }

    //-- Try for rotations to fit the vector
    for ( int i = 0; i < 4; i++)
    {
        Eigen::AngleAxisd rotAngle( deg2rad(i*90), base_vector);
        Eigen::Matrix3d rotMatrix = rotAngle.matrix();
        Eigen::Vector3d result_vector = rotMatrix * Eigen::Vector3d::UnitZ();

//        std::cout << "i = " << i << std::endl << result_vector << std::endl << std::endl;

        if ( vector_equal(result_vector, new_z) )
            return i;
    }

    return -1;
}
コード例 #10
0
ファイル: ProcessLaserScan.hpp プロジェクト: lixiao89/MBot
        // overloading constructor
        ProcessLaserScan(ros::NodeHandle& nh,Eigen::Matrix3d initialPose, Eigen::Matrix3d neighborIP,std::string laserTopic)
        {
            nh_ = nh;
            
            // initialize subscriber and publisher
            scanSub_ = nh_.subscribe<sensor_msgs::LaserScan>(laserTopic, 1000, &ProcessLaserScan::laserScanCallback,this); 
            Eigen::Matrix3d relPose;

            relPose = initialPose.inverse()*neighborIP;

            minAngle = atan2(relPose(2-1,1-1),relPose(2-1,2-1));

            minRange = sqrt(pow(relPose(1-1,3-1),2)+pow(relPose(2-1,3-1),2));
        }
コード例 #11
0
ファイル: main.cpp プロジェクト: kunyue/robot_tracking
Vector3d get_robot_position(Vector3d position)
{
    Rb2w = att_body.normalized().toRotationMatrix();
    Rc2w = Rb2w * Rc2b;
    Rc2i = Ri2w.inverse() * Rc2w;
    //pos_i is normlized coordinate; the POS_I is the real coordinate; both are in intermidiate frame
    Vector3d pos_i, POS_I, POS_W, CAM_W;
    pos_i = Rc2i * position.normalized();
    pos_i = pos_i / pos_i.z();
    POS_I = pos_body.z() * pos_i;
    CAM_W = pos_body + Rb2w * cam_off_b;
    POS_W = Ri2w * POS_I + CAM_W;
    return POS_W;
}
コード例 #12
0
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::OrientationConstraint::decide(const robot_state::RobotState &state, bool verbose) const
{
  if (!link_model_)
    return ConstraintEvaluationResult(true, 0.0);

  const robot_state::LinkState *link_state = state.getLinkState(link_model_->getName());

  if (!link_state)
  {
    logWarn("No link in state with name '%s'", link_model_->getName().c_str());
    return ConstraintEvaluationResult(false, 0.0);
  }

  Eigen::Vector3d xyz;
  if (mobile_frame_)
  {
    Eigen::Matrix3d tmp;
    tf_->transformRotationMatrix(state, desired_rotation_frame_id_, desired_rotation_matrix_, tmp);
    Eigen::Affine3d diff(tmp.inverse() * link_state->getGlobalLinkTransform().rotation());
    xyz = diff.rotation().eulerAngles(0, 1, 2);
    // 0,1,2 corresponds to ZXZ, the convention used in sampling constraints
  }
  else
  {
    Eigen::Affine3d diff(desired_rotation_matrix_inv_ * link_state->getGlobalLinkTransform().rotation());
    xyz = diff.rotation().eulerAngles(0, 1, 2); // 0,1,2 corresponds to ZXZ, the convention used in sampling constraints
  }

  xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0)));
  xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1)));
  xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - fabs(xyz(2)));
  bool result = xyz(2) < absolute_z_axis_tolerance_+std::numeric_limits<double>::epsilon()
    && xyz(1) < absolute_y_axis_tolerance_+std::numeric_limits<double>::epsilon()
    && xyz(0) < absolute_x_axis_tolerance_+std::numeric_limits<double>::epsilon();

  if (verbose)
  {
    Eigen::Quaterniond q_act(link_state->getGlobalLinkTransform().rotation());
    Eigen::Quaterniond q_des(desired_rotation_matrix_);
    logInform("Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion actual: %f %f %f %f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f",
             result ? "satisfied" : "violated", link_model_->getName().c_str(),
             q_des.x(), q_des.y(), q_des.z(), q_des.w(),
             q_act.x(), q_act.y(), q_act.z(), q_act.w(), xyz(0), xyz(1), xyz(2),
             absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, absolute_z_axis_tolerance_);
  }

  return ConstraintEvaluationResult(result, constraint_weight_ * (xyz(0) + xyz(1) + xyz(2)));
}
bool isPositivelyDecomposable(const Point_3 &a, const Point_3 &b,
		const Point_3 &c, const Point_3 &decomposed)
{
	DEBUG_START;
	Eigen::Matrix3d matrix;
	matrix << a.x(), b.x(), c.x(),
	       a.y(), b.y(), c.y(),
	       a.z(), b.z(), c.z();
	Eigen::Vector3d vector;
	vector << decomposed.x(), decomposed.y(), decomposed.z();
	Eigen::Vector3d coefficients = matrix.inverse() * vector;
	std::cout << "Tried to decompose vector, result: " << std::endl
		<< coefficients << std::endl;
	DEBUG_END;
	return coefficients(0) >= 0.
		&& coefficients(1) >= 0.
		&& coefficients(2) >= 0.;
}
コード例 #14
0
bool extractPCFromColorModel(const PCRGB::Ptr& pc_in, PCRGB::Ptr& pc_out,
                             Eigen::Vector3d& mean, Eigen::Matrix3d& cov, double std_devs)
{
    double hue_weight;
    ros::param::param<double>("~hue_weight", hue_weight, 1.0);
    printf("%f\n", hue_weight);

    Eigen::Vector3d x, x_m;
    Eigen::Matrix3d cov_inv = cov.inverse();
    for(uint32_t i=0;i<pc_in->size();i++) {
        extractHSL(pc_in->points[i].rgb, x(0), x(1), x(2));
        x_m = x - mean;
        x_m(0) *= hue_weight;
        double dist = std::sqrt(x_m.transpose() * cov_inv * x_m);
        if(dist <= std_devs) 
            pc_out->points.push_back(pc_in->points[i]);
    }
}
コード例 #15
0
void inverseTransform(Eigen::Vector3d &src, cspace config, Eigen::Vector3d &dest)
{
    Eigen::Matrix3d rotationC;
    rotationC << cos(config[5]), -sin(config[5]), 0,
               sin(config[5]), cos(config[5]), 0,
               0, 0, 1;
    Eigen::Matrix3d rotationB;
    rotationB << cos(config[4]), 0 , sin(config[4]),
               0, 1, 0,
               -sin(config[4]), 0, cos(config[4]);
    Eigen::Matrix3d rotationA;
    rotationA << 1, 0, 0 ,
               0, cos(config[3]), -sin(config[3]),
               0, sin(config[3]), cos(config[3]);
    Eigen::Vector3d transitionV(config[0], config[1], config[2]);
    Eigen::Matrix3d rotationM = rotationC * rotationB * rotationA;
    dest = rotationM.inverse() * (src - transitionV);
}
コード例 #16
0
ファイル: VoxelGrid.cpp プロジェクト: billow06/Autoware
void VoxelGrid<PointSourceType>::computeCentroidAndCovariance()
{
	for (int i = 0; i < voxel_num_; i++) {
		int ipoint_num = points_id_[i].size();
		double point_num = static_cast<double>(ipoint_num);
		Eigen::Vector3d pt_sum = centroid_[i];

		if (ipoint_num > 0) {
			centroid_[i] /= point_num;
		}

		if (ipoint_num >= min_points_per_voxel_) {

			covariance_[i] = (covariance_[i] - 2 * (pt_sum * centroid_[i].transpose())) / point_num + centroid_[i] * centroid_[i].transpose();
			covariance_[i] *= (point_num - 1.0) / point_num;

			SymmetricEigensolver3x3 sv(covariance_[i]);

			sv.compute();
			Eigen::Matrix3d evecs = sv.eigenvectors();
			Eigen::Matrix3d evals = sv.eigenvalues().asDiagonal();

			if (evals(0, 0) < 0 || evals(1, 1) < 0 || evals(2, 2) <= 0) {
				points_per_voxel_[i] = -1;
				continue;
			}

			double min_cov_eigvalue = evals(2, 2) * 0.01;

			if (evals(0, 0) < min_cov_eigvalue) {
				evals(0, 0) = min_cov_eigvalue;

				if (evals(1, 1) < min_cov_eigvalue) {
					evals(1, 1) = min_cov_eigvalue;
				}

				covariance_[i] = evecs * evals * evecs.inverse();
			}

			icovariance_[i] = covariance_[i].inverse();
		}
	}
}
static Vector_3 leastSquaresPoint(const std::vector<unsigned> activeGroup,
		const std::vector<SupportItem> &items)
{
	DEBUG_START;
	Eigen::Matrix3d matrix;
	Eigen::Vector3d vector;
	for (unsigned i = 0; i < 3; ++i)
	{
		vector(i) = 0.;
		for (unsigned j = 0; j < 3; ++j)
			matrix(i, j) = 0.;
	}

	std::cout << "Calculating least squares points for the following items"
		<< std::endl;
	for (unsigned iPlane : activeGroup)
	{
		SupportItem item = items[iPlane];
		Vector_3 u = item.direction;
		double value = item.value;
		std::cout << "  Item #" << iPlane << ": u = " << u << "; h = "
			<< value << std::endl;
		for (unsigned i = 0; i < 3; ++i)
		{
			for (unsigned j = 0; j < 3; ++j)
				matrix(i, j) += u.cartesian(i)
					* u.cartesian(j);
			vector(i) += u.cartesian(i) * value;
		}
	}
	Eigen::Vector3d solution = matrix.inverse() * vector;

	Vector_3 result(solution(0), solution(1), solution(2));
	std::cout << "Result: " << result << std::endl;
	for (unsigned iPlane : activeGroup)
	{
		SupportItem item = items[iPlane];
		double delta = item.direction * result - item.value;
		std::cout << "  delta = " << delta << std::endl;
	}
	DEBUG_END;
	return result;
}
コード例 #18
0
ファイル: Simulation.cpp プロジェクト: henriquetmaia/posDyn
	void Simulation :: dampVelocities( void )
	{
		// [ Mueller - 2007 Sec: 3.5 ]
		for( unsigned mIdx = 0; mIdx < m_meshes.size(); ++mIdx ){

			Vector x_cm;
			Vector v_cm;
			double sumMass = 0.;
		    for( VertexIter v = m_meshes[mIdx]->vertices.begin(); v != m_meshes[mIdx]->vertices.end(); ++v ){
		    	sumMass += v->mass;
		    	x_cm += v->position * v->mass;
		    	v_cm += v->velocity * v->mass;
		    }
			x_cm /= sumMass;
			v_cm /= sumMass;

			Vector r;
			Vector L;
			Eigen::Matrix3d I = Eigen::Matrix3d::Zero();
		    for( VertexIter v = m_meshes[mIdx]->vertices.begin(); v != m_meshes[mIdx]->vertices.end(); ++v ){
		    	r = v->position - x_cm;
		    	L += cross( r, v->mass * v->velocity );
		    	Eigen::Matrix3d singleI = Eigen::Matrix3d::Zero();
		    	singleI << 0.,  r[2], -r[1],
		    			-r[2],    0.,  r[0],
		    			 r[2], -r[0],    0.;
		    	I += singleI * singleI.transpose() * v->mass;
			}
			Eigen::Vector3d L_tmp ( L[0], L[1], L[2] );
			Eigen::Vector3d omegaTemp = I.inverse() * L_tmp;
			Vector ang_vel_omega( omegaTemp[0], omegaTemp[1], omegaTemp[2] );

		    for( VertexIter v = m_meshes[mIdx]->vertices.begin(); v != m_meshes[mIdx]->vertices.end(); ++v ){
		    	r = v->position - x_cm;
		    	Vector delta_v = v_cm + cross( ang_vel_omega, r ) - v->velocity;
		    	v->velocity = v->velocity + ( m_meshes[mIdx]->dampingStiffness() * delta_v );
			}

		}
	}
コード例 #19
0
ファイル: keyframe.cpp プロジェクト: itsuhane/VINS-Mobile
void KeyFrame::initPtsByReprojection(Eigen::Vector3d Ti_predict,
                                     Eigen::Matrix3d Ri_predict,
                                     std::vector<cv::Point2f> &measurements_predict)
{
    measurements_predict.clear();
    Vector3d pts_predict;
    for(int i = 0; i < (int)point_clouds.size(); i++)
    {
        Eigen::Vector3d pts_w = point_clouds[i];
        Eigen::Vector3d pts_imu_j = Ri_predict.inverse() * (pts_w - Ti_predict);
        Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
        pts_predict <<  pts_camera_j.x()/pts_camera_j.z(),
        pts_camera_j.y()/pts_camera_j.z(),
        1.0;
        Vector2d point_uv;
        point_uv.x() = FOCUS_LENGTH_X * pts_predict.x() + PX;
        point_uv.y() = FOCUS_LENGTH_Y * pts_predict.y() + PY;
        measurements_predict.push_back(cv::Point2f(point_uv.x(), point_uv.y()));
    }
    if(measurements_predict.size() == 0)
    {
        measurements_predict = measurements;
    }
}
コード例 #20
0
ファイル: gicp.hpp プロジェクト: 87west/pcl
template <typename PointSource, typename PointTarget> inline void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
{
  pcl::IterativeClosestPoint<PointSource, PointTarget>::initComputeReciprocal ();
  using namespace std;
  // Difference between consecutive transforms
  double delta = 0;
  // Get the size of the target
  const size_t N = indices_->size ();
  // Set the mahalanobis matrices to identity
  mahalanobis_.resize (N, Eigen::Matrix3d::Identity ());
  // Compute target cloud covariance matrices
  if (target_covariances_.empty ())
    computeCovariances<PointTarget> (target_, tree_, target_covariances_);
  // Compute input cloud covariance matrices
  if (input_covariances_.empty ())
    computeCovariances<PointSource> (input_, tree_reciprocal_, input_covariances_);

  base_transformation_ = guess;
  nr_iterations_ = 0;
  converged_ = false;
  double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
  std::vector<int> nn_indices (1);
  std::vector<float> nn_dists (1);

  while(!converged_)
  {
    size_t cnt = 0;
    std::vector<int> source_indices (indices_->size ());
    std::vector<int> target_indices (indices_->size ());

    // guess corresponds to base_t and transformation_ to t
    Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero ();
    for(size_t i = 0; i < 4; i++)
      for(size_t j = 0; j < 4; j++)
        for(size_t k = 0; k < 4; k++)
          transform_R(i,j)+= double(transformation_(i,k)) * double(guess(k,j));

    Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();

    for (size_t i = 0; i < N; i++)
    {
      PointSource query = output[i];
      query.getVector4fMap () = guess * query.getVector4fMap ();
      query.getVector4fMap () = transformation_ * query.getVector4fMap ();

      if (!searchForNeighbors (query, nn_indices, nn_dists))
      {
        PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[i]);
        return;
      }
      
      // Check if the distance to the nearest neighbor is smaller than the user imposed threshold
      if (nn_dists[0] < dist_threshold)
      {
        Eigen::Matrix3d &C1 = input_covariances_[i];
        Eigen::Matrix3d &C2 = target_covariances_[nn_indices[0]];
        Eigen::Matrix3d &M = mahalanobis_[i];
        // M = R*C1
        M = R * C1;
        // temp = M*R' + C2 = R*C1*R' + C2
        Eigen::Matrix3d temp = M * R.transpose();        
        temp+= C2;
        // M = temp^-1
        M = temp.inverse ();
        source_indices[cnt] = static_cast<int> (i);
        target_indices[cnt] = nn_indices[0];
        cnt++;
      }
    }
    // Resize to the actual number of valid correspondences
    source_indices.resize(cnt); target_indices.resize(cnt);
    /* optimize transformation using the current assignment and Mahalanobis metrics*/
    previous_transformation_ = transformation_;
    //optimization right here
    try
    {
      rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_);
      /* compute the delta from this iteration */
      delta = 0.;
      for(int k = 0; k < 4; k++) {
        for(int l = 0; l < 4; l++) {
          double ratio = 1;
          if(k < 3 && l < 3) // rotation part of the transform
            ratio = 1./rotation_epsilon_;
          else
            ratio = 1./transformation_epsilon_;
          double c_delta = ratio*fabs(previous_transformation_(k,l) - transformation_(k,l));
          if(c_delta > delta)
            delta = c_delta;
        }
      }
    } 
    catch (PCLException &e)
    {
      PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ());
      break;
    }
    nr_iterations_++;
    // Check for convergence
    if (nr_iterations_ >= max_iterations_ || delta < 1)
    {
      converged_ = true;
      previous_transformation_ = transformation_;
      PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
                 getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ());
    } 
    else
      PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ());
  }
  //for some reason the static equivalent methode raises an error
  // final_transformation_.block<3,3> (0,0) = (transformation_.block<3,3> (0,0)) * (guess.block<3,3> (0,0));
  // final_transformation_.block <3, 1> (0, 3) = transformation_.block <3, 1> (0, 3) + guess.rightCols<1>.block <3, 1> (0, 3);
  final_transformation_.topLeftCorner (3,3) = previous_transformation_.topLeftCorner (3,3) * guess.topLeftCorner (3,3);
  final_transformation_(0,3) = previous_transformation_(0,3) + guess(0,3);
  final_transformation_(1,3) = previous_transformation_(1,3) + guess(1,3);
  final_transformation_(2,3) = previous_transformation_(2,3) + guess(2,3);
}
コード例 #21
0
ファイル: fitting_cylinder_pdm.cpp プロジェクト: 5irius/pcl
ON_NurbsSurface
FittingCylinder::initNurbsPCACylinder (int order, NurbsDataSurface *data)
{
  Eigen::Vector3d mean;
  Eigen::Matrix3d eigenvectors;
  Eigen::Vector3d eigenvalues;

  unsigned s = unsigned (data->interior.size ());

  NurbsTools::pca (data->interior, mean, eigenvectors, eigenvalues);

  data->mean = mean;
  data->eigenvectors = eigenvectors;

  eigenvalues = eigenvalues / s; // seems that the eigenvalues are dependent on the number of points (???)

  Eigen::Vector3d v_max (0.0, 0.0, 0.0);
  Eigen::Vector3d v_min (DBL_MAX, DBL_MAX, DBL_MAX);
  for (unsigned i = 0; i < s; i++)
  {
    Eigen::Vector3d p (eigenvectors.inverse () * (data->interior[i] - mean));

    if (p (0) > v_max (0))
      v_max (0) = p (0);
    if (p (1) > v_max (1))
      v_max (1) = p (1);
    if (p (2) > v_max (2))
      v_max (2) = p (2);

    if (p (0) < v_min (0))
      v_min (0) = p (0);
    if (p (1) < v_min (1))
      v_min (1) = p (1);
    if (p (2) < v_min (2))
      v_min (2) = p (2);
  }

  int ncpsU (order);
  int ncpsV (2 * order + 4);
  ON_NurbsSurface nurbs (3, false, order, order, ncpsU, ncpsV);
  nurbs.MakeClampedUniformKnotVector (0, 1.0);
  nurbs.MakePeriodicUniformKnotVector (1, 1.0 / (ncpsV - order + 1));

  double dcu = (v_max (0) - v_min (0)) / (ncpsU - 1);
  double dcv = (2.0 * M_PI) / (ncpsV - order + 1);

  double ry = std::max<double> (std::fabs (v_min (1)), std::fabs (v_max (1)));
  double rz = std::max<double> (std::fabs (v_min (2)), std::fabs (v_max (2)));

  Eigen::Vector3d cv_t, cv;
  for (int i = 0; i < ncpsU; i++)
  {
    for (int j = 0; j < ncpsV; j++)
    {
      cv (0) = v_min (0) + dcu * i;
      cv (1) = ry * sin (dcv * j);
      cv (2) = rz * cos (dcv * j);
      cv_t = eigenvectors * cv + mean;
      nurbs.SetCV (i, j, ON_3dPoint (cv_t (0), cv_t (1), cv_t (2)));
    }
  }

  return nurbs;
}
コード例 #22
0
ON_NurbsSurface
FittingSurface::initNurbsPCABoundingBox (int order, NurbsDataSurface *m_data, Eigen::Vector3d z)
{
  Eigen::Vector3d mean;
  Eigen::Matrix3d eigenvectors;
  Eigen::Vector3d eigenvalues;

  unsigned s = m_data->interior.size ();
  m_data->interior_param.clear ();

  NurbsTools::pca (m_data->interior, mean, eigenvectors, eigenvalues);

  m_data->mean = mean;
  m_data->eigenvectors = eigenvectors;

  bool flip (false);
  if (eigenvectors.col (2).dot (z) < 0.0)
    flip = true;

  eigenvalues = eigenvalues / s; // seems that the eigenvalues are dependent on the number of points (???)
  Eigen::Matrix3d eigenvectors_inv = eigenvectors.inverse ();

  Eigen::Vector3d v_max (0.0, 0.0, 0.0);
  Eigen::Vector3d v_min (DBL_MAX, DBL_MAX, DBL_MAX);
  for (unsigned i = 0; i < s; i++)
  {
    Eigen::Vector3d p = eigenvectors_inv * (m_data->interior[i] - mean);
    m_data->interior_param.push_back (Eigen::Vector2d (p (0), p (1)));

    if (p (0) > v_max (0))
      v_max (0) = p (0);
    if (p (1) > v_max (1))
      v_max (1) = p (1);
    if (p (2) > v_max (2))
      v_max (2) = p (2);

    if (p (0) < v_min (0))
      v_min (0) = p (0);
    if (p (1) < v_min (1))
      v_min (1) = p (1);
    if (p (2) < v_min (2))
      v_min (2) = p (2);
  }

  for (unsigned i = 0; i < s; i++)
  {
    Eigen::Vector2d &p = m_data->interior_param[i];
    if (v_max (0) > v_min (0) && v_max (0) > v_min (0))
    {
      p (0) = (p (0) - v_min (0)) / (v_max (0) - v_min (0));
      p (1) = (p (1) - v_min (1)) / (v_max (1) - v_min (1));
    }
    else
    {
      throw std::runtime_error ("[NurbsTools::initNurbsPCABoundingBox] Error: v_max <= v_min");
    }
  }

  ON_NurbsSurface nurbs (3, false, order, order, order, order);
  nurbs.MakeClampedUniformKnotVector (0, 1.0);
  nurbs.MakeClampedUniformKnotVector (1, 1.0);

  double dcu = (v_max (0) - v_min (0)) / (nurbs.Order (0) - 1);
  double dcv = (v_max (1) - v_min (1)) / (nurbs.Order (1) - 1);

  Eigen::Vector3d cv_t, cv;
  for (int i = 0; i < nurbs.Order (0); i++)
  {
    for (int j = 0; j < nurbs.Order (1); j++)
    {
      cv (0) = v_min (0) + dcu * i;
      cv (1) = v_min (1) + dcv * j;
      cv (2) = 0.0;
      cv_t = eigenvectors * cv + mean;
      if (flip)
        nurbs.SetCV (nurbs.Order (0) - 1 - i, j, ON_3dPoint (cv_t (0), cv_t (1), cv_t (2)));
      else
        nurbs.SetCV (i, j, ON_3dPoint (cv_t (0), cv_t (1), cv_t (2)));
    }
  }
  return nurbs;
}
コード例 #23
0
ファイル: opennurbs_fit.cpp プロジェクト: cogitokat/brlcad
ON_NurbsSurface
FittingSurface::initNurbsPCABoundingBox (int order, NurbsDataSurface *m_data, ON_3dVector z)
{
  ON_3dVector mean;
  Eigen::Matrix3d eigenvectors;
  Eigen::Vector3d eigenvalues;

  unsigned s = m_data->interior.size ();
  m_data->interior_param.clear ();

  NurbsTools::pca (m_data->interior, mean, eigenvectors, eigenvalues);

  m_data->mean = mean;
  //m_data->eigenvectors = (*eigenvectors);

  bool flip (false);
  Eigen::Vector3d ez(z[0],z[1],z[2]);
  if (eigenvectors.col (2).dot (ez) < 0.0)
    flip = true;

  eigenvalues = eigenvalues / s; // seems that the eigenvalues are dependent on the number of points (???)
  Eigen::Matrix3d eigenvectors_inv = eigenvectors.inverse ();

  ON_3dVector v_max(0.0, 0.0, 0.0);
  ON_3dVector v_min(DBL_MAX, DBL_MAX, DBL_MAX);
  Eigen::Vector3d emean(mean[0], mean[1], mean[2]);
  for (unsigned i = 0; i < s; i++)
  {
    Eigen::Vector3d eint(m_data->interior[i][0], m_data->interior[i][1], m_data->interior[i][2]);
    Eigen::Vector3d ep = eigenvectors_inv * (eint - emean);
    ON_3dPoint p(ep (0), ep (1), ep(2));
    m_data->interior_param.push_back (ON_2dPoint(p[0], p[1]));

    if (p[0] > v_max[0])
      v_max[0] = p[0];
    if (p[1] > v_max[1])
      v_max[1] = p[1];
    if (p[2] > v_max[2])
      v_max[2] = p[2];

    if (p[0] < v_min[0])
      v_min[0] = p[0];
    if (p[1] < v_min[1])
      v_min[1] = p[1];
    if (p[2] < v_min[2])
      v_min[2] = p[2];
  }

  for (unsigned i = 0; i < s; i++)
  {
    ON_2dVector &p = m_data->interior_param[i];
    if (v_max[0] > v_min[0] && v_max[0] > v_min[0])
    {
      p[0] = (p[0] - v_min[0]) / (v_max[0] - v_min[0]);
      p[1] = (p[1] - v_min[1]) / (v_max[1] - v_min[1]);
    }
    else
    {
      throw std::runtime_error ("[NurbsTools::initNurbsPCABoundingBox] Error: v_max <= v_min");
    }
  }

  ON_NurbsSurface nurbs (3, false, order, order, order, order);

  nurbs.MakeClampedUniformKnotVector (0, 1.0);
  nurbs.MakeClampedUniformKnotVector (1, 1.0);

  double dcu = (v_max[0] - v_min[0]) / (nurbs.Order (0) - 1);
  double dcv = (v_max[1] - v_min[1]) / (nurbs.Order (1) - 1);

  ON_3dPoint cv_t, cv;
  Eigen::Vector3d ecv_t2, ecv2;
  Eigen::Vector3d emean2(mean[0],mean[1],mean[2]);
  for (int i = 0; i < nurbs.Order (0); i++)
  {
    for (int j = 0; j < nurbs.Order (1); j++)
    {
      cv[0] = v_min[0] + dcu * i;
      cv[1] = v_min[1] + dcv * j;
      cv[2] = 0.0;
      ecv2 (0) = cv[0];
      ecv2 (1) = cv[1];
      ecv2 (2) = cv[2];
      ecv_t2 = eigenvectors * ecv2 + emean2;
      if (flip)
	nurbs.SetCV (nurbs.Order (0) - 1 - i, j, cv_t);
      else
	nurbs.SetCV (i, j, cv_t);
    }
  }
  return nurbs;
}
コード例 #24
0
ファイル: lineslam.cpp プロジェクト: caomw/LineSLAM
void Node::detect3DLines(const cv::Mat& gray_uchar, const cv::Mat& depth_float, double line2d_len_thres, 
                      const cv::Mat& K,double ratio_of_collinear_pts, double line_3d_len_thres_m, double depth_scaling, string algorithm)
{
//	MyTimer tm; tm.start();	
	
	vector<FrameLine> allLines; // 2d and 3d lines
	if(algorithm == "LSD") { // using LSD, 100+ ms
		IplImage pImg = gray_uchar;
		ntuple_list  lsdOut;	
		lsdOut = callLsd(&pImg);// use LSD method
		int dim = lsdOut->dim;
		double a,b,c,d;
		allLines.reserve(lsdOut->size);
		for(int i=0; i<lsdOut->size; i++) {// store LSD output to lineSegments 
			a = lsdOut->values[i*dim];
			b = lsdOut->values[i*dim+1];
			c = lsdOut->values[i*dim+2];
			d = lsdOut->values[i*dim+3];
			if ( sqrt((a-c)*(a-c)+(b-d)*(b-d)) > line2d_len_thres) {
				allLines.push_back(FrameLine(cv::Point2d(a,b), cv::Point2d(c,d)));
			}
		}	
		
	}
    
	if(algorithm == "EDLINES") { // using EDlines, 15 ms
		int n;
		LS* ls = callEDLines(gray_uchar, &n);
		allLines.reserve(n);
		for(int i=0; i<n; i++) {// store output to lineSegments 
			if ((ls[i].sx-ls[i].ex)*(ls[i].sx-ls[i].ex) +(ls[i].sy-ls[i].ey)*(ls[i].sy-ls[i].ey) 
				> line2d_len_thres*line2d_len_thres) {
				allLines.push_back(FrameLine(cv::Point2d(ls[i].sx,ls[i].sy), cv::Point2d(ls[i].ex,ls[i].ey)));
			}
		}
	}

	Eigen::Matrix3d eK;
	for(int i=0; i<3; ++i)
		for(int j=0; j<3; ++j)
			eK(i,j) = K.at<double>(i,j);
	Eigen::Matrix3d Kinv = eK.inverse();	
 

 	
 	int depth_CVMatDepth = depth_float.depth();
  	#pragma omp parallel for
	for(int i=0; i<allLines.size(); ++i)	{ // 20 -30 ms
		double len = cv::norm(allLines[i].p - allLines[i].q);		
		// iterate through a line
		double numSmp = min(max(len/sysPara.line_sample_interval, (double)sysPara.line_sample_min_num), (double)sysPara.line_sample_max_num);  // smaller numSmp for fast speed, larger numSmp should be more accurate	
   		vector<cv::Point3d> pts3d; pts3d.reserve(numSmp);
		for(int j=0; j<=numSmp; ++j) {
			// use nearest neighbor to querry depth value
			// assuming position (0,0) is the top-left corner of image, then the
			// top-left pixel's center would be (0.5,0.5)
			cv::Point2d pt = allLines[i].p * (1-j/numSmp) + allLines[i].q * (j/numSmp);
			if(pt.x<0 || pt.y<0 || pt.x >= depth_float.cols || pt.y >= depth_float.rows ) continue;
			int row, col; // nearest pixel for pt
			if((floor(pt.x) == pt.x) && (floor(pt.y) == pt.y)) {// boundary issue
				col = max(int(pt.x-1),0);
				row = max(int(pt.y-1),0);
			} else {
				col = int(pt.x);
				row = int(pt.y);
			}
			double zval = -1;
			double depval;
			if(depth_CVMatDepth == CV_32F) 
				depval = depth_float.at<float>(row,col);
			else if (depth_CVMatDepth == CV_64F) 
				depval = depth_float.at<double>(row,col);
			else {
				cerr<<"Node::extractLineDepth: depth image matrix type is not float/double\n";
				exit(0);	
			}	
			if(depval < EPS || isnan((float)depval)) { // no depth info

			} else {
				zval = depval/depth_scaling; // in meter, z-value
			}

			if (zval > 0 ) {
				Eigen::Vector3d ept(pt.x, pt.y, 1);
				Eigen::Vector3d xy3d = Kinv * ept;
				xy3d = xy3d/xy3d(2);
				pts3d.push_back(cv::Point3d(xy3d(0)*zval, xy3d(1)*zval, zval));								
			}
		}
		if (pts3d.size() < max(10.0, numSmp *ratio_of_collinear_pts))
			continue;

		RandomLine3d tmpLine;		
		vector<RandomPoint3d> rndpts3d;
		rndpts3d.reserve(pts3d.size());
		// compute uncertainty of 3d points
		for(int j=0; j<pts3d.size();++j) {
			rndpts3d.push_back(compPt3dCov(pts3d[j], K, asynch_time_diff_sec_));
		}
		// using ransac to extract a 3d line from 3d pts
		tmpLine = extract3dline_mahdist(rndpts3d);
		
		if(tmpLine.pts.size()/numSmp > ratio_of_collinear_pts	&&
			cv::norm(tmpLine.A - tmpLine.B) > line_3d_len_thres_m) {
				allLines[i].haveDepth = true;
				allLines[i].line3d = tmpLine;
				
		}		
	}	

//// prepare for compute msld line descriptors
	cv::Mat xGradImg, yGradImg;
	int ddepth = CV_64F;	
	cv::Sobel(gray_uchar, xGradImg, ddepth, 1, 0, 5); // Gradient X
	cv::Sobel(gray_uchar, yGradImg, ddepth, 0, 1, 5); // Gradient Y
	double	*xG, *yG;
	if(xGradImg.isContinuous() && yGradImg.isContinuous()) {
		xG = (double*) xGradImg.data;
		yG = (double*) yGradImg.data;
	} else { 
		xG = new double[xGradImg.rows*xGradImg.cols];
 		yG = new double[yGradImg.rows*yGradImg.cols];
 		int idx = 0;
    	for(int i=0; i<xGradImg.rows; ++i) {
    		for(int j=0; j<xGradImg.cols; ++j) {
    			xG[idx] = xGradImg.at<double>(i,j);
    			yG[idx] = yGradImg.at<double>(i,j);
    			++idx;
    		}
    	}
	}

	lines.reserve(allLines.size());
	// NOTE this for-loop mustnot be parallized
	for(int i=0; i<allLines.size(); ++i) {
		if(allLines[i].haveDepth) { 
			lines.push_back(allLines[i]);
			lines.back().lid = lines.size()-1;
			lines.back().complineEq2d();				
			lines.back().r = lines.back().getGradient(&xGradImg, &yGradImg);
		}
	}
	

	#pragma omp parallel for
	for(int i=0; i<lines.size(); ++i) {		// 60 ms, 0.6ms/line
		computeMSLD(lines[i], xG, yG, gray_uchar.cols, gray_uchar.rows); // 0.1 ms/line
		MLEstimateLine3d(lines[i].line3d, sysPara.line3d_mle_iter_num);
		vector<RandomPoint3d> ().swap(lines[i].line3d.pts); // swap with a empty vector effectively free up the memory, clear() doesn't.
	}
//	tm.end(); cout<<"detect 3d lines "<<tm.time_ms<<" ms\n";

	if(!xGradImg.isContinuous() || !yGradImg.isContinuous()) { // dynamic allocation
		delete[] xG; 
		delete[] yG;
	}

}
コード例 #25
0
bool MyWindow::transformSegmentAtSample(std::string segmentName,
                                        int timeStep)
{

  Eigen::VectorXd segmentConfig;
  if (mInputMotion->getSegmentConfig(timeStep, segmentName, &segmentConfig))
  {
    if (segmentName == "root")
    {

      // Transform Root
      Eigen::Vector6d r_t = segmentConfig;

      // r_t:       head(3)->translation, tail(3)->rotation
      // FreeJoint: head(3)->rotation, tail(3)->translation

      Eigen::Isometry3d tf;
      tf.linear() = dart::math::eulerXYZToMatrix(r_t.tail(3)*deg_to_rad);
      //tf.linear() = dart::math::eulerZYXToMatrix(r_t.tail(3)*deg_to_rad);
      tf.translation() = r_t.head(3)*unit;
      mSkel->getJoint("root_joint")
           ->setPositions(dart::dynamics::FreeJoint::convertToPositions(tf));
      return true;
    }
    else
    {
      // other bones
      // prepare reference frame
      Eigen::Vector3d segmentAxes;
      if (mAsfData->getSegmentAxes(segmentName, &segmentAxes))
      {
        Eigen::Matrix3d refFrame = dart::math::eulerXYZToMatrix(segmentAxes*deg_to_rad);
        //Eigen::Matrix3d refFrame = dart::math::eulerZYXToMatrix(segmentAxes*deg_to_rad);
        // calculate motion transformation
        Eigen::Matrix3d motionRot = dart::math::eulerXYZToMatrix(segmentConfig*deg_to_rad);
        //Eigen::Matrix3d motionRot = dart::math::eulerZYXToMatrix(segmentConfig*deg_to_rad);
        Eigen::Isometry3d finalTF;
        //finalTF.linear() = refFrame.inverse() * motionRot * refFrame;
        finalTF.linear() = refFrame * motionRot * refFrame.inverse();
        //finalTF.linear() = motionRot;

        // transform
        mSkel->getBodyNode(segmentName)->getParentBodyNode()->getParentJoint()
             ->setPositions(dart::dynamics::BallJoint::convertToPositions(finalTF.linear()));


        std::cout << segmentName << std::endl;
        std::cout << mSkel->getBodyNode(segmentName)->getParentJoint()
            ->getName() << std::endl;
        std::cout << mSkel->getBodyNode(segmentName)->getRelativeTransform().linear()
                  << std::endl;


        std::cout << mSkel->getJoint(segmentName+"_joint")->getPositions() << std::endl;

        return true;

      }
      std::cout << "segment transform fail" << std::endl;
      return false;
    }
  }
  return false;
}
コード例 #26
0
ファイル: pcgmmreg_func.cpp プロジェクト: NBXApp/tum-lsr-dhri
double PCGMMReg_func::weight_l2(PCObject &model, PCObject &scene)
{
    // reference :
    // Robust Point Set Registration Using Gaussian Mixture Models
    // Bing Jina, and Baba C. Vemuri
    // IEEE Transactions on Pattern Analysis and Machine Intelligence 2010

    double energy1 = 0.;
    for(int i=0;i<m;i++){
        for(int j=0;j<m;j++){
            Eigen::Matrix3d cov = model.gmm.at(i).covariance + model.gmm.at(j).covariance;
            Eigen::Vector3d mean = model.gmm.at(i).mean - model.gmm.at(j).mean;
            Eigen::Matrix3d invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,3)*cov.determinant())*exp(-0.5*a);
            energy1 += model.gmm.at(i).weight*model.gmm.at(j).weight*gauss;
        }
    }
//    cout<<"m "<<m<<endl;
//    cout<<"s "<<s<<endl;
    double energy2 = 0.;
    for(int i=0;i<m;i++){
        double sum[3] = {0.,0.,0.};
        for(int j=0;j<s;j++){
            Eigen::Matrix3d cov = model.gmm.at(i).covariance + scene.gmm.at(j).covariance;
            Eigen::Vector3d mean = model.gmm.at(i).mean - scene.gmm.at(j).mean;
            Eigen::Matrix3d invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,3)*cov.determinant())*exp(-0.5*a);
            energy2 += model.gmm.at(i).weight*scene.gmm.at(j).weight*gauss;
//            cout<<"weight i "<<model.gmm.at(i).weight<<endl;
//            cout<<"weight j "<<scene.gmm.at(j).weight<<endl;
//            cout<<"a "<<a<<endl;
//            cout<<"gauss "<<gauss<<endl;


            // gradient [m,d]
            double derv_x = -0.5*(2*mean[0]*invij(0,0) + mean[1]*(invij(0,1)+invij(1,0)) + mean[2]*(invij(0,2)+invij(2,0)));
            double derv_y = -0.5*(mean[0]*(invij(1,0)+invij(0,1)) + 2*mean[1]*invij(1,1) + mean[2]*(invij(1,2)+invij(2,1)));
            double derv_z = -0.5*(mean[0]*(invij(2,0)+invij(0,2)) + mean[1]*(invij(2,1)+invij(1,2)) + 2*mean[2]*invij(2,2));

            sum[0] += scene.gmm.at(j).weight*gauss*derv_x;
            sum[1] += scene.gmm.at(j).weight*gauss*derv_y;
            sum[2] += scene.gmm.at(j).weight*gauss*derv_z;

        }
        gradient[i][0] = -2.*model.gmm.at(i).weight*sum[0];
        gradient[i][1] = -2.*model.gmm.at(i).weight*sum[1];
        gradient[i][2] = -2.*model.gmm.at(i).weight*sum[2];
    }
    double energy3 = 0.;
    for(int i=0;i<s;i++){
        for(int j=0;j<s;j++){
            Eigen::Matrix3d cov = scene.gmm.at(i).covariance + scene.gmm.at(j).covariance;
            Eigen::Vector3d mean = scene.gmm.at(i).mean - scene.gmm.at(j).mean;
            Eigen::Matrix3d invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,3)*cov.determinant())*exp(-0.5*a);
            energy3 += scene.gmm.at(i).weight*scene.gmm.at(j).weight*gauss;
        }
    }
    return energy1 - 2*energy2 + energy3;
//    cout<<"energy2 "<<energy2<<endl;
//    return -2*energy2;
}
コード例 #27
0
ファイル: hsh.cpp プロジェクト: rti-capture/rti-chi
int Hsh::loadData(FILE* file, int width, int height, int basisTerm, bool urti, CallBackPos * cb,const QString& text)
{
	type = "HSH";
	w = width;
	h = height;

	ordlen = basisTerm;
	bands = 3;
	fread(gmin, sizeof(float), basisTerm, file);
	fread(gmax, sizeof(float), basisTerm, file);

	if (feof(file))
		return -1;

	int offset = 0;

	int size = w * h * basisTerm;
	float* redPtr = new float[size];
	float* greenPtr = new float[size];
	float* bluePtr = new float[size];
	
	unsigned char c;

	if (!urti)
	{
		for(int j = 0; j < h; j++)
		{
			if (cb != NULL && j % 50 == 0)(*cb)(j * 50.0 / h, text);
			for(int i = 0; i < w; i++)
			{				
				offset = j * w + i;
				for (int k = 0; k < basisTerm; k++)
				{
					if (feof(file))
						return -1;
					fread(&c, sizeof(unsigned char), 1, file);
					redPtr[offset*basisTerm + k] = (((float)c) / 255.0) * (gmax[k] - gmin[k]) + gmin[k];
				}
				for (int k = 0; k < basisTerm; k++)
				{
					if (feof(file))
						return -1;
					fread(&c, sizeof(unsigned char), 1, file);
					greenPtr[offset*basisTerm + k] = (((float)c) / 255.0) * (gmax[k] - gmin[k]) + gmin[k];
				}
				for (int k = 0; k < basisTerm; k++)
				{
					if (feof(file))
						return -1;
					fread(&c, sizeof(unsigned char), 1, file);
					bluePtr[offset*basisTerm + k] = (((float)c) / 255.0) * (gmax[k] - gmin[k]) + gmin[k];
				}
			}
		}
	}
	else
	{
		for(int j = 0; j < h; j++)
		{
			if (cb != NULL && j % 50 == 0)(*cb)(j * 50 / h, text);
			for(int i = 0; i < w; i++)
			{				
				offset = j * w + i;
				for (int k = 0; k < basisTerm; k++)
				{
					if (feof(file))
						return -1;
					fread(&c, sizeof(unsigned char), 1, file);
					redPtr[offset*basisTerm + k] = (((float)c) / 255.0) * gmin[k] + gmax[k];
				}
				for (int k = 0; k < basisTerm; k++)
				{
					if (feof(file))
						return -1;
					fread(&c, sizeof(unsigned char), 1, file);
					greenPtr[offset*basisTerm + k] = (((float)c) / 255.0) * gmin[k] + gmax[k];
				}
				for (int k = 0; k < basisTerm; k++)
				{
					if (feof(file))
						return -1;
					fread(&c, sizeof(unsigned char), 1, file);
					bluePtr[offset*basisTerm + k] = (((float)c) / 255.0) * gmin[k] + gmax[k];
				}
			}
		}
	}
	
	fclose(file);

	mipMapSize[0] = QSize(w, h);

	redCoefficients.setLevel(redPtr, size, 0);
	greenCoefficients.setLevel(greenPtr, size, 0);
	blueCoefficients.setLevel(bluePtr, size, 0);
	
	// Computes mip-mapping.
	if (cb != NULL)	(*cb)(50, "Mip mapping generation...");
	
	for (int level = 1; level < MIP_MAPPING_LEVELS; level++)
	{
		int width = mipMapSize[level - 1].width();
		int height = mipMapSize[level - 1].height();
		int width2 = ceil(width / 2.0);
		int height2 = ceil(height / 2.0);
		size = width2*height2*basisTerm;
		redCoefficients.setLevel(new float[size], size, level);
		greenCoefficients.setLevel(new float[size], size, level);
		blueCoefficients.setLevel(new float[size], size, level);
		int th_id;
		#pragma omp parallel for
		for (int i = 0; i < height - 1; i+=2)
		{
			th_id = omp_get_thread_num();
			if (th_id == 0)
			{
				if (cb != NULL && i % 50 == 0)	(*cb)(50 + (level-1)*8 + i*8.0/height, "Mip mapping generation...");
			}
			for (int j = 0; j < width - 1; j+=2)
			{
				int index1 = (i * width + j)*ordlen;
				int index2 = (i * width + j + 1)*ordlen;
				int index3 = ((i + 1) * width + j)*ordlen;
				int index4 = ((i + 1) * width + j + 1)*ordlen;
				int offset = (i/2 * width2 + j/2)*ordlen;
				for (int k = 0; k < basisTerm; k++)
				{
					redCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k, index3 + k , index4 + k);
					greenCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k, index3 + k , index4 + k);
					blueCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k, index3 + k , index4 + k);
				}
			}
		}
		if (width2 % 2 != 0)
		{
			for (int i = 0; i < height - 1; i+=2)
			{
				int index1 = ((i + 1) * width - 1)*ordlen;
				int index2 = ((i + 2) * width - 1)*ordlen;
				int offset = ((i/2 + 1) * width2 - 1)*ordlen;
				for (int k = 0; k < basisTerm; k++)
				{
					redCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
					greenCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
					blueCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
				}
			}
		}
		if (height % 2 != 0)
		{
			for (int i = 0; i < width - 1; i+=2)
			{
				int index1 = ((height - 1) * width + i)*ordlen;
				int index2 = ((height - 1) * width + i + 1)*ordlen;
				int offset = ((height2 - 1) * width2 + i/2)*ordlen;
				for (int k = 0; k < basisTerm; k++)
				{
					redCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
					greenCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
					blueCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
				}
			}
		}
		if (height % 2 != 0 && width % 2 != 0)
		{
			int index1 = (height*width - 1)*ordlen;
			int offset = (height2*width2 - 1)*ordlen;
			for (int k = 0; k < basisTerm; k++)
			{
				redCoefficients.calcMipMapping(level, offset + k, index1 + k);
				greenCoefficients.calcMipMapping(level, offset + k, index1 + k);
				blueCoefficients.calcMipMapping(level, offset + k, index1 + k);
			}
		}
		mipMapSize[level] = QSize(width2, height2);
	}

	//Compute normals.
	if (cb != NULL) (*cb)(75 , "Normals generation...");
	Eigen::Vector3d l0(sin(M_PI/4)*cos(M_PI/6), sin(M_PI/4)*sin(M_PI/6), cos(M_PI/4));
	Eigen::Vector3d l1(sin(M_PI/4)*cos(5*M_PI / 6), sin(M_PI/4)*sin(5*M_PI / 6), cos(M_PI/4));
	Eigen::Vector3d l2(sin(M_PI/4)*cos(3*M_PI / 2), sin(M_PI/4)*sin(3*M_PI / 2), cos(M_PI/4));
    float hweights0[16], hweights1[16], hweights2[16];
	getHSH(M_PI / 4, M_PI / 6, hweights0, ordlen);
	getHSH(M_PI / 4, 5*M_PI / 6, hweights1, ordlen);
	getHSH(M_PI / 4, 3*M_PI / 2, hweights2, ordlen);
	
	
	Eigen::Matrix3d L;
	L.setIdentity();
	L.row(0) = l0;
	L.row(1) = l1;
	L.row(2) = l2;
	Eigen::Matrix3d LInverse = L.inverse();
	
	for (int level = 0; level < MIP_MAPPING_LEVELS; level++)
	{
		const float* rPtr = redCoefficients.getLevel(level);
		const float* gPtr = greenCoefficients.getLevel(level);
		const float* bPtr = blueCoefficients.getLevel(level);
		vcg::Point3f* temp = new vcg::Point3f[mipMapSize[level].width()*mipMapSize[level].height()];
		if (cb != NULL) (*cb)(75 + level*6, "Normal generation...");

		#pragma omp parallel for
		for (int y = 0; y < mipMapSize[level].height(); y++)
		{
			for (int x = 0; x < mipMapSize[level].width(); x++)
			{
				int offset= y * mipMapSize[level].width() + x;
				Eigen::Vector3d f(0, 0, 0);
				for (int k = 0; k < ordlen; k++)
				{
					f(0) += rPtr[offset*ordlen + k] * hweights0[k];
					f(1) += rPtr[offset*ordlen + k] * hweights1[k];
					f(2) += rPtr[offset*ordlen + k] * hweights2[k];
				}
				for (int k = 0; k < ordlen; k++)
				{
					f(0) += gPtr[offset*ordlen + k] * hweights0[k];
					f(1) += gPtr[offset*ordlen + k] * hweights1[k];
					f(2) += gPtr[offset*ordlen + k] * hweights2[k];
				}
				for (int k = 0; k < ordlen; k++)
				{
					f(0) += bPtr[offset*ordlen + k] * hweights0[k];
					f(1) += bPtr[offset*ordlen + k] * hweights1[k];
					f(2) += bPtr[offset*ordlen + k] * hweights2[k];
				}
				f /= 3.0;
				Eigen::Vector3d normal = LInverse * f;
				temp[offset] = vcg::Point3f(normal(0), normal(1), normal(2));
				temp[offset].Normalize();
			}
		}
		normals.setLevel(temp, mipMapSize[level].width()*mipMapSize[level].height(), level);
	}
	

	return 0;

}
コード例 #28
0
int main(int /*argc*/, char** /*argv*/)
{
  try
  {
    imp::ImageCv32fC1::Ptr cv_im0;
    imp::cvBridgeLoad(cv_im0,
                      "/home/mwerlberger/data/epipolar_stereo_test/00000.png",
                      imp::PixelOrder::gray);

    imp::ImageCv32fC1::Ptr cv_im1;
    imp::cvBridgeLoad(cv_im1,
                      "/home/mwerlberger/data/epipolar_stereo_test/00001.png",
                      imp::PixelOrder::gray);

    // rectify images for testing

    imp::cu::ImageGpu32fC1::Ptr im0 = std::make_shared<imp::cu::ImageGpu32fC1>(*cv_im0);
    imp::cu::ImageGpu32fC1::Ptr im1 = std::make_shared<imp::cu::ImageGpu32fC1>(*cv_im1);


    Eigen::Quaterniond q_world_im0(0.14062777, 0.98558398, 0.02351040, -0.09107859);
    Eigen::Quaterniond q_world_im1(0.14118687, 0.98569744, 0.01930722, -0.08996696);
    Eigen::Vector3d t_world_im0(-0.12617580, 0.50447008, 0.15342121);
    Eigen::Vector3d t_world_im1(-0.11031053, 0.50314023, 0.15158643);

    imp::cu::PinholeCamera cu_cam(414.09, 413.799, 355.567, 246.337);

    // im0: fixed image; im1: moving image
    imp::cu::Matrix3f F_fix_mov;
    imp::cu::Matrix3f F_mov_fix;
    Eigen::Matrix3d F_fm, F_mf;
    { // compute fundamental matrix
      Eigen::Matrix3d R_world_mov = q_world_im1.matrix();
      Eigen::Matrix3d R_world_fix = q_world_im0.matrix();
      Eigen::Matrix3d R_fix_mov = R_world_fix.inverse()*R_world_mov;

      // in ref coordinates
      Eigen::Vector3d t_fix_mov = R_world_fix.inverse()*(-t_world_im0 + t_world_im1);

      Eigen::Matrix3d tx_fix_mov;
      tx_fix_mov << 0, -t_fix_mov[2], t_fix_mov[1],
          t_fix_mov[2], 0, -t_fix_mov[0],
          -t_fix_mov[1], t_fix_mov[0], 0;
      Eigen::Matrix3d E_fix_mov = tx_fix_mov * R_fix_mov;
      Eigen::Matrix3d K;
      K << cu_cam.fx(), 0, cu_cam.cx(),
          0, cu_cam.fy(), cu_cam.cy(),
          0, 0, 1;

      Eigen::Matrix3d Kinv = K.inverse();
      F_fm = Kinv.transpose() * E_fix_mov * Kinv;
      F_mf = F_fm.transpose();
    } // end .. compute fundamental matrix
    // convert the Eigen-thingy to something that we can use in CUDA
    for(size_t row=0; row<F_fix_mov.rows(); ++row)
    {
      for(size_t col=0; col<F_fix_mov.cols(); ++col)
      {
        F_fix_mov(row,col) = (float)F_fm(row,col);
        F_mov_fix(row,col) = (float)F_mf(row,col);
      }
    }

    // compute SE3 transformation
    imp::cu::SE3<float> T_world_fix(
          static_cast<float>(q_world_im0.w()), static_cast<float>(q_world_im0.x()),
          static_cast<float>(q_world_im0.y()), static_cast<float>(q_world_im0.z()),
          static_cast<float>(t_world_im0.x()), static_cast<float>(t_world_im0.y()),
          static_cast<float>(t_world_im0.z()));
    imp::cu::SE3<float> T_world_mov(
          static_cast<float>(q_world_im1.w()), static_cast<float>(q_world_im1.x()),
          static_cast<float>(q_world_im1.y()), static_cast<float>(q_world_im1.z()),
          static_cast<float>(t_world_im1.x()), static_cast<float>(t_world_im1.y()),
          static_cast<float>(t_world_im1.z()));
    imp::cu::SE3<float> T_mov_fix = T_world_mov.inv() * T_world_fix;
    // end .. compute SE3 transformation

    std::cout << "T_mov_fix:\n" << T_mov_fix << std::endl;


    std::unique_ptr<imp::cu::VariationalEpipolarStereo> stereo(
          new imp::cu::VariationalEpipolarStereo());

    stereo->parameters()->verbose = 1;
    stereo->parameters()->solver = imp::cu::StereoPDSolver::EpipolarPrecondHuberL1;
    stereo->parameters()->ctf.scale_factor = 0.8f;
    stereo->parameters()->ctf.iters = 30;
    stereo->parameters()->ctf.warps  = 5;
    stereo->parameters()->ctf.apply_median_filter = true;
    stereo->parameters()->lambda = 20;

    stereo->addImage(im0);
    stereo->addImage(im1);

    imp::cu::ImageGpu32fC1::Ptr cu_mu = std::make_shared<imp::cu::ImageGpu32fC1>(im0->size());
    imp::cu::ImageGpu32fC1::Ptr cu_sigma2 = std::make_shared<imp::cu::ImageGpu32fC1>(im0->size());
    cu_mu->setValue(-5.f);
    cu_sigma2->setValue(0.0f);

    stereo->setFundamentalMatrix(F_mov_fix);
    stereo->setIntrinsics({cu_cam, cu_cam});
    stereo->setExtrinsics(T_mov_fix);
    stereo->setDepthProposal(cu_mu, cu_sigma2);
    
    stereo->solve();

    std::shared_ptr<imp::cu::ImageGpu32fC1> d_disp = stereo->getDisparities();


    {
      imp::Pixel32fC1 min_val,max_val;
      imp::cu::minMax(*d_disp, min_val, max_val);
      std::cout << "disp: min: " << min_val.x << " max: " << max_val.x << std::endl;
    }

    imp::cu::cvBridgeShow("im0", *im0);
    imp::cu::cvBridgeShow("im1", *im1);
//    *d_disp *= -1;
    {
      imp::Pixel32fC1 min_val,max_val;
      imp::cu::minMax(*d_disp, min_val, max_val);
      std::cout << "disp: min: " << min_val.x << " max: " << max_val.x << std::endl;
    }

    imp::cu::cvBridgeShow("disparities", *d_disp, -18.0f, 11.0f);
    imp::cu::cvBridgeShow("disparities minmax", *d_disp, true);
    cv::waitKey();
  }
  catch (std::exception& e)
  {
    std::cout << "[exception] " << e.what() << std::endl;
    assert(false);
  }

  return EXIT_SUCCESS;

}