C++ Mat::colRange方法代码示例

// The image is seperated into two parts, each part's length maximum loc+1;
// x is the seperation line, both two part have x;
float ReidDescriptor::sym_div(int x, cv::Mat img, cv::Mat MSK, int loc, float alpha)
{
	int H = img.rows;
	int W = img.cols;
	int chs = img.channels();

	cv::Mat imgL = img.colRange(0, x + 1); // [0,x]
	cv::Mat imgR = img.colRange(x, img.cols);
	cv::Mat MSK_L = MSK.colRange(0, x + 1);
	cv::Mat MSK_R = MSK.colRange(x, MSK.cols);

	int dimLoc = min(min(x + 1, MSK_R.cols), loc + 1);

	if (dimLoc != 0)
	{
		cv::Mat imgLloc = img.colRange(x - dimLoc + 1, x + 1);//[x-dimLoc+1,x]
		cv::Mat imgRloc;
		cv::flip(imgR.colRange(0, dimLoc), imgRloc, 1);
		cv::Mat temp;
		cv::pow(imgLloc - imgRloc, 2, temp);
		float ans = alpha * sqrt(cv::sum(temp.reshape(1))[0]) / dimLoc +
					(1 - alpha) * (abs(sum(MSK_R)[0] - sum(MSK_L)[0])) / max(MSK_R.rows * MSK_R.cols, MSK_L.rows * MSK_L.cols);

		return ans;
	}
	else
	{
		return 1;
	}
}

void RandomFerns::get_linepoint(cv::Mat img,cv::Mat init_pose,std::vector<l1l2f1> line_pids, cv::Mat& feats){
	int cs = init_pose.cols;
	int w = img.cols;
	int h = img.rows;
	assert(cs==posdim);
	assert(line_pids.size()==num_fea_locs);
	feats = cv::Mat(num_fea_locs,1,CV_32FC1);
	cv::Mat xs = init_pose.colRange(0,cs/2).clone();
	cv::Mat ys = init_pose.colRange(cs/2,cs).clone();
	float mx = cv::mean(xs).val[0];
	float my = cv::mean(ys).val[0];
	xs -= mx;
	ys -= my;
	for (int k =0; k < line_pids.size(); k++){
		float p1x = xs.at<float>(0,line_pids[k].id1-1);
		float p1y = ys.at<float>(0,line_pids[k].id1-1);
		float p2x = xs.at<float>(0,line_pids[k].id2-1);
		float p2y = ys.at<float>(0,line_pids[k].id2-1);
		float a = (p2y-p1y)/(p2x-p1x);
		float b = p1y - a*p1x;
		float distx = (p2x-p1x)/2;
		float ctrx = p1x + distx;
		float cs1 = ctrx+line_pids[k].t1*distx;
		float rs1 = a*cs1+b;
		int cs = (int)(cs1+mx);
		int rs = (int)(rs1+my);
//		std::cout<< rs  << " " << cs << std::endl;
		feats.at<float>(k,0) = img.at<float>(max(0,min(h-1,rs)),max(0,min(w-1,cs)));
//		std::cout << feats.at<float>(k,0) << std::endl;
	}

}

	void data_transformer_util::rotate_band(
		cv::Mat image,
		int shift_x_to_left,
		int shift_y_to_top)
	{
		int actual_shift_x = (shift_x_to_left % image.cols);
		if (actual_shift_x < 0)
			actual_shift_x += image.cols;
		int actual_shift_y = (shift_y_to_top % image.rows);
		if (actual_shift_y < 0)
			actual_shift_y += image.rows;
		if ((actual_shift_x == 0) && (actual_shift_y == 0))
			return;

		cv::Mat cloned_image = image.clone();

		if (actual_shift_y == 0)
		{
			cloned_image.colRange(actual_shift_x, image.cols).copyTo(image.colRange(0, image.cols - actual_shift_x));
			cloned_image.colRange(0, actual_shift_x).copyTo(image.colRange(image.cols - actual_shift_x, image.cols));
		}
		else if (actual_shift_x == 0)
		{
			cloned_image.rowRange(actual_shift_y, image.rows).copyTo(image.rowRange(0, image.rows - actual_shift_y));
			cloned_image.rowRange(0, actual_shift_y).copyTo(image.rowRange(image.rows - actual_shift_y, image.rows));
		}
		else
		{
			cloned_image.colRange(actual_shift_x, image.cols).rowRange(actual_shift_y, image.rows).copyTo(image.colRange(0, image.cols - actual_shift_x).rowRange(0, image.rows - actual_shift_y));
			cloned_image.colRange(0, actual_shift_x).rowRange(actual_shift_y, image.rows).copyTo(image.colRange(image.cols - actual_shift_x, image.cols).rowRange(0, image.rows - actual_shift_y));
			cloned_image.colRange(actual_shift_x, image.cols).rowRange(0, actual_shift_y).copyTo(image.colRange(0, image.cols - actual_shift_x).rowRange(image.rows - actual_shift_y, image.rows));
			cloned_image.colRange(0, actual_shift_x).rowRange(0, actual_shift_y).copyTo(image.colRange(image.cols - actual_shift_x, image.cols).rowRange(image.rows - actual_shift_y, image.rows));
		}
	}

cv::Rect_<T> get_enclosing_bbox(cv::Mat landmarks)
{
	auto num_landmarks = landmarks.cols / 2;
	double min_x_val, max_x_val, min_y_val, max_y_val;
	cv::minMaxLoc(landmarks.colRange(0, num_landmarks), &min_x_val, &max_x_val);
	cv::minMaxLoc(landmarks.colRange(num_landmarks, landmarks.cols), &min_y_val, &max_y_val);
	return cv::Rect_<T>(min_x_val, min_y_val, max_x_val - min_x_val, max_y_val - min_y_val);
};

void projectpose(cv::Mat pose1, cv::Mat& pose2, cv::Rect bbox){
	float sx = bbox.width/2.0;
	float sy = bbox.height/2.0;
	float ctrx = bbox.x + sx;
	float ctry = bbox.y + sy;
	pose2 = pose1.clone();
	int nc = pose1.cols;
	pose2.colRange(0,nc/2) -= ctrx;
	pose2.colRange(0,nc/2) /= sx;
	pose2.colRange(nc/2,nc) -= ctry;
	pose2.colRange(nc/2,nc) /= sy;
}

void circshift(cv::Mat &A, int shitf_row, int shift_col) {
    int row = A.rows, col = A.cols;
    shitf_row = (row + (shitf_row % row)) % row;
    shift_col = (col + (shift_col % col)) % col;
    cv::Mat temp = A.clone();
    if (shitf_row){
        temp.rowRange(row - shitf_row, row).copyTo(A.rowRange(0, shitf_row));
        temp.rowRange(0, row - shitf_row).copyTo(A.rowRange(shitf_row, row));
    }
    if (shift_col){
        temp.colRange(col - shift_col, col).copyTo(A.colRange(0, shift_col));
        temp.colRange(0, col - shift_col).copyTo(A.colRange(shift_col, col));
    }
    return;
}

void mergePoints(const std::vector<cv::Mat> &in_descriptors, const std::vector<cv::Mat> &in_points,
                 cv::Mat &out_descriptors, cv::Mat &out_points) {
  // Figure out the number of points
  size_t n_points = 0, n_images = in_descriptors.size();
  for (size_t image_id = 0; image_id < n_images; ++image_id)
    n_points += in_descriptors[image_id].rows;
  if (n_points == 0)
    return;

  // Fill the descriptors and 3d points
  out_descriptors = cv::Mat(n_points, in_descriptors[0].cols, in_descriptors[0].depth());
  out_points = cv::Mat(1, n_points, CV_32FC3);
  size_t row_index = 0;
  for (size_t image_id = 0; image_id < n_images; ++image_id) {
    // Copy the descriptors
    const cv::Mat & descriptors = in_descriptors[image_id];
    int n_points = descriptors.rows;
    cv::Mat sub_descriptors = out_descriptors.rowRange(row_index, row_index + n_points);
    descriptors.copyTo(sub_descriptors);
    // Copy the 3d points
    const cv::Mat & points = in_points[image_id];
    cv::Mat sub_points = out_points.colRange(row_index, row_index + n_points);
    points.copyTo(sub_points);

    row_index += n_points;
  }
}

void AkazeSymmetryMatcher::apply(cv::Mat &srcGray, cv::Mat &dst) {

    // clear points
    leftKeyPoints.clear();
    rightKeyPoints.clear();

    try {

        detector_descriptor->detectAndCompute(srcGray.colRange(0, srcGray.cols / 2), cv::Mat(),
                                              leftKeyPoints, leftDescriptors, false);
        detector_descriptor->detectAndCompute(srcGray.colRange(srcGray.cols / 2, srcGray.cols),
                                              cv::Mat(), rightKeyPoints, rightDescriptors, false);

        if(leftKeyPoints.size() < 1 || rightKeyPoints.size() < 1 ) return;

        matches.clear();

        descriptorMatcher->match(leftDescriptors, rightDescriptors, matches, cv::Mat());


        //Select the best matching points and draw them
        float min_dist = 100;
        for( int i = 0; i < leftDescriptors.rows; i++ )
        {
            float dist = matches[i].distance;
            if( dist < min_dist ) min_dist = dist;
        }

        bestMatches.clear();
        for( int i = 0; i < leftDescriptors.rows; i++ )
        {
            if( matches[i].distance <= 3 * min_dist )
            {
                bestMatches.push_back( matches[i]);
            }
        }


        cv::drawMatches(srcGray.colRange(0, srcGray.cols / 2), leftKeyPoints,
                        srcGray.colRange(srcGray.cols / 2, srcGray.cols), rightKeyPoints,
                        bestMatches, dst);

    } catch (cv::Exception& e){
       // LOGD(LOG_TAG, e.msg.c_str());
    }

}

bool checkCheirality (cv::Mat P, cv::Mat X) // cheirality constraint
	//	input: Project Matrix P 4x3, P = [R | t], Homogeneous 3D point X 4x1 
	//  output: true - if X is in front of camera
{
	if(X.cols * X.rows < 4) {
		X = (cv::Mat_<double>(4,1)<<X.at<double>(0),X.at<double>(1),X.at<double>(2),1);
	}
	cv::Mat x = P*X;
	return cv::determinant(P.colRange(0,3))*x.at<double>(2)*X.at<double>(3)>0;
}

		NearestNeighbor(std::vector<cv::Mat> vm) : 
		confusion_Mat(3,3,CV_64F), NN_Mat(vm.size(),vm[0].cols,CV_64F)
		{
			std::cout << "cols are: " << vm[0].cols << std::endl;
			for(size_t i = 0; i < vm.size();i++)
			{
				cv::Mat dest(NN_Mat.colRange(0,vm[0].cols));
				vm[i].copyTo(dest);
			}
		}

void ofxIcp::compute_transformation(const vector<cv::Point3d> & input, const vector<cv::Point3d> & target, cv::Mat & transformation){
    
    vector<cv::Point3d> transformed;
    cv::Mat rotation, translation;
    double error;
    
    vector<cv::Point3d> closest_current;
    vector<cv::Point3d> closest_target;
    
    int increment = input.size()/5;
    for(int i = 0; i < input.size(); i += increment){
        closest_current.push_back(input[i]);
        closest_target.push_back(target[i]);
    }
    
    cv::Mat centroid_current;
    cv::Mat centroid_target;
    
    cv::reduce(cv::Mat(closest_current, false), centroid_current, 0, CV_REDUCE_AVG);
    cv::reduce(cv::Mat(closest_target, false), centroid_target, 0, CV_REDUCE_AVG);
    
    centroid_current = centroid_current.reshape(1);
    centroid_target = centroid_target.reshape(1);
    
    compute_rigid_transformation(closest_current, centroid_current, closest_target, centroid_target, rotation, translation);
    
    vector<cv::Point3d> transformed_closest_current;
    vector<cv::Point3d> transformed_current;
    
    transform(closest_current, rotation, translation, transformed_closest_current);
    
    compute_error(transformed_closest_current, closest_target, error);
    
    transformation = (cv::Mat_<double>(4,4) << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
    cv::Mat rotation_tmp = transformation.colRange(0,3).rowRange(0,3);
    cv::Mat translation_tmp = transformation.colRange(3,4).rowRange(0,3);
    
    cv::Mat translation_t = translation.reshape(1).t();
    
    rotation.copyTo(rotation_tmp);
    translation_t.copyTo(translation_tmp);
}

void convolveDFT(const cv::Mat& imgOriginal, const cv::Mat& kernel, cv::Mat& out, const bool& corr, const bool& full)
{ //this method is also valid for complex image and kernel, set DFT_COMPLEX_OUTPUT then
  //convolution in fourier space, keep code for future use
  //CONVOLUTION_FULL: Return the full convolution, including border
  //to completeley emulate filter2D operation, image should be first double sized and then cut back to origianl size
  cv::Mat source, kernelPadded;
  const int marginSrcTop = corr ? std::ceil((kernel.rows-1)/2.0) : std::floor((kernel.rows-1)/2.0);
  const int marginSrcBottom = corr ? std::floor((kernel.rows-1)/2.0) : std::ceil((kernel.rows-1)/2.0);
  const int marginSrcLeft = corr ? std::ceil((kernel.cols-1)/2.0) : std::floor((kernel.cols-1)/2.0);
  const int marginSrcRight = corr ? std::floor((kernel.cols-1)/2.0) : std::ceil((kernel.cols-1)/2.0);
  cv::copyMakeBorder(imgOriginal, source, marginSrcTop, marginSrcBottom, marginSrcLeft, marginSrcRight, cv::BORDER_CONSTANT);

  const int marginKernelTop = std::ceil((source.rows-kernel.rows)/2.0);
  const int marginKernelBottom = std::floor((source.rows-kernel.rows)/2.0);
  const int marginKernelLeft = std::ceil((source.cols-kernel.cols)/2.0);
  const int marginKernelRight = std::floor((source.cols-kernel.cols)/2.0);
  cv::copyMakeBorder(kernel, kernelPadded, marginKernelTop, marginKernelBottom, marginKernelLeft, marginKernelRight, cv::BORDER_CONSTANT);

  //Divided by 2.0 instead of 2 to consider the result as double instead of as an int
  //The +1 in the shift changes slightly the finest plane in the wavelet,
  shift(kernelPadded, kernelPadded, std::ceil(kernelPadded.cols/2.0), std::ceil(kernelPadded.rows/2.0));

  cv::Mat kernelPadded_ft, input_ft, output_ft;
  cv::dft(kernelPadded, kernelPadded_ft, cv::DFT_COMPLEX_OUTPUT + cv::DFT_SCALE);
  cv::dft(source, input_ft, cv::DFT_COMPLEX_OUTPUT + cv::DFT_SCALE);
  cv::mulSpectrums(input_ft, kernelPadded_ft.mul(kernelPadded.total()), output_ft, cv::DFT_COMPLEX_OUTPUT, corr);
  
  if(imgOriginal.channels() == 1 && kernel.channels() == 1) cv::idft(output_ft, out, cv::DFT_REAL_OUTPUT);
  else cv::idft(output_ft, out, cv::DFT_COMPLEX_OUTPUT);
  
  if(!full)
  {
    //colRange and rowRange are semi-open intervals. first included, last is not
    //this frist option is what i think should be the correct one, but the next is what filter2D function gives for this inputs
//    out = out.colRange(marginSrcLeft, out.cols - marginSrcRight).
//              rowRange(marginSrcTop, out.rows - marginSrcBottom);
        out = out.colRange(marginSrcRight, out.cols - marginSrcLeft).
                  rowRange(marginSrcBottom, out.rows - marginSrcTop);
  }
}

void conv2D(const cv::Mat &img, cv::Mat& dest, const cv::Mat& kernel, ConvolutionType ctype, int btype) {

    cv::Mat source = img;
    cv::Mat flip_kernel(kernel.rows,kernel.cols,kernel.type());
    cv::flip(kernel,flip_kernel,-1);

    if(ctype == CONVOLUTION_FULL) {
        source = cv::Mat();
        const int additionalRows = kernel.rows-1, additionalCols = kernel.cols-1;
        cv::copyMakeBorder(img, source, (additionalRows+1)/2, additionalRows/2, (additionalCols+1)/2, additionalCols/2, btype, cv::Scalar(0));
    }

    cv::Point anchor(kernel.cols - kernel.cols/2 - 1, kernel.rows - kernel.rows/2 - 1);

    int borderMode = btype;
    cv::filter2D(source, dest, img.depth(), flip_kernel, anchor, 0, borderMode);

    if(ctype == CONVOLUTION_VALID) {
        dest = dest.colRange((kernel.cols-1)/2, dest.cols - kernel.cols/2)
                .rowRange((kernel.rows-1)/2, dest.rows - kernel.rows/2);
    }

}

//.........这里部分代码省略.........
			}
		}
		if(debugLevel >= 1){
			cout << "segment id = " << pixels[begIm].imageId << ", begIm = " << begIm << ", endIm = " << endIm << endl;
		}
		values = Mat(1, endIm - begIm, CV_8UC3);
		for(int p = begIm; p < endIm; p++){
			values.at<Vec3b>(p - begIm) = imageHSV.at<Vec3b>(pixels[p].r, pixels[p].c);
		}

		int begTer = endTer;
		if(begTer < terrainRegion.size()){
			while(terrainRegion[begTer].first != pixels[begIm].imageId){
				begTer++;
				if(begTer >= terrainRegion.size()){
					break;
				}
			}
		}
		endTer = begTer;
		if(endTer < terrainRegion.size()){
			while(terrainRegion[begTer].first == terrainRegion[endTer].first){
				endTer++;
				if(endTer >= terrainRegion.size()){
					break;
				}
			}
		}
		if(endTer - begTer > 0){
			valuesTer = Mat(terrain.rows, endTer - begTer, CV_32FC1);
			Mat tmpImageBGR(imageBGR);
			for(int p = begTer; p < endTer; p++){
				//cout << terrainRegion[p].second << endl;
				terrain.colRange(terrainRegion[p].second, terrainRegion[p].second + 1).copyTo(valuesTer.colRange(p - begTer, p - begTer + 1));
				//cout << "terrainRegion[p].second = " << terrainRegion[p].second << endl;
				//int imageRow = round(terrainPointsImage.at<float>(1 ,terrainRegion[p].second));
				//int imageCol = round(terrainPointsImage.at<float>(0, terrainRegion[p].second));
				//cout << "Point: " << imageRow << ", " << imageCol << endl;
				//tmpImageBGR.at<Vec3b>(imageRow, imageCol) = Vec3b(0x00, 0x00, 0xff);
			}
			//cout << "ImageId = " << pixels[begIm].imageId << endl;
			//imshow("imageBGR", imageBGR);
			//waitKey();
		}
		else{
			//cout << "Warning - no terrain values for imageId " << pixels[begIm].imageId << endl;
			valuesTer = Mat(6, 1, CV_32FC1, Scalar(0));
		}

		if(endIm - begIm > 0){
			int channelsHS[] = {0, 1};
			float rangeH[] = {0, 60};
			float rangeS[] = {0, 256};
			const float* rangesHS[] = {rangeH, rangeS};
			int sizeHS[] = {histHLen, histSLen};
			int channelsV[] = {2};
			float rangeV[] = {0, 256};
			const float* rangesV[] = {rangeV};
			int sizeV[] = {histVLen};
			calcHist(&values, 1, channelsHS, Mat(), histogramHS, 2, sizeHS, rangesHS);
			calcHist(&values, 1, channelsV, Mat(), histogramV, 1, sizeV, rangesV);
			histogramHS = histogramHS.reshape(0, 1);
			histogramV = histogramV.reshape(0, 1);
			normalize(histogramHS, histogramHS);
			normalize(histogramV, histogramV);

void ConstraintsHelpers::processPointCloud(cv::Mat hokuyoData,
											cv::Mat& pointCloudOrigMapCenter,
											std::queue<PointsPacket>& pointsInfo,
											std::chrono::high_resolution_clock::time_point hokuyoTimestamp,
											std::chrono::high_resolution_clock::time_point curTimestamp,
											cv::Mat curPosOrigMapCenter,
											std::mutex& mtxPointCloud,
											cv::Mat cameraOrigLaser,
											cv::Mat cameraOrigImu,
											ros::NodeHandle &nh)
{
	Mat hokuyoCurPoints(6, hokuyoData.cols, CV_32FC1);
	int minLaserDist, pointCloudTimeout;

	nh.getParam("minLaserDist", minLaserDist);
	nh.getParam("pointCloudTimeout", pointCloudTimeout);

	//cout << "Copying hokuyo data" << endl;
	int countPoints = 0;
	for(int c = 0; c < hokuyoData.cols; c++){
		//cout << hokuyoData.at<int>(2, c) << endl;
		if(hokuyoData.at<int>(2, c) > minLaserDist){
			hokuyoCurPoints.at<float>(0, countPoints) = -hokuyoData.at<int>(1, c);
			hokuyoCurPoints.at<float>(1, countPoints) = 0.0;
			hokuyoCurPoints.at<float>(2, countPoints) = hokuyoData.at<int>(0, c);
			hokuyoCurPoints.at<float>(3, countPoints) = 1.0;
			hokuyoCurPoints.at<float>(4, countPoints) = hokuyoData.at<int>(2, c);
			hokuyoCurPoints.at<float>(5, countPoints) = hokuyoData.at<int>(3, c);
			countPoints++;
		}
	}
	hokuyoCurPoints = hokuyoCurPoints.colRange(0, countPoints);
//	cout << "hokuyoCurPoints.size() = " << hokuyoCurPoints.size() << endl;
//	cout << "hokuyoData.size() = " << hokuyoData.size() << endl;

	//cout << "Removing old points" << endl;
	//remove all points older than pointCloudTimeout ms
	int pointsSkipped = 0;
	if(pointsInfo.size() > 0){
		//cout << "dt = " << std::chrono::duration_cast<std::chrono::milliseconds>(curTimestamp - pointsQueue.front().timestamp).count() << endl;
		while(std::chrono::duration_cast<std::chrono::milliseconds>(curTimestamp - pointsInfo.front().timestamp).count() > pointCloudTimeout){
			pointsSkipped += pointsInfo.front().numPoints;
			pointsInfo.pop();
			if(pointsInfo.size() == 0){
				break;
			}
		}
	}

	std::unique_lock<std::mutex> lck(mtxPointCloud);
	//cout << "Moving pointCloudOrigMapCenter, pointsSkipped = " << pointsSkipped << endl;
	Mat tmpAllPoints(hokuyoCurPoints.rows, pointCloudOrigMapCenter.cols + hokuyoCurPoints.cols - pointsSkipped, CV_32FC1);
	if(!pointCloudOrigMapCenter.empty()){
		//cout << "copyTo" << endl;
		if(pointsSkipped <	pointCloudOrigMapCenter.cols){
			pointCloudOrigMapCenter.colRange(pointsSkipped,
												pointCloudOrigMapCenter.cols).
							copyTo(tmpAllPoints.colRange(0, pointCloudOrigMapCenter.cols - pointsSkipped));
		}
	}
//	if(debugLevel >= 1){
//		cout << "countPoints = " << countPoints << ", hokuyoCurPoints.size = " << hokuyoCurPoints.size() << endl;
//	}
	if(countPoints > 0){

		//cout << "Moving to camera orig" << endl;
		hokuyoCurPoints.rowRange(0, 4) = cameraOrigLaser.inv()*hokuyoCurPoints.rowRange(0, 4);
		//cout << "Addding hokuyoCurPoints" << endl;
//		if(debugLevel >= 1){
//			cout << "Addding hokuyoCurPoints" << endl;
//		}
		Mat curPointCloudOrigMapCenter(hokuyoCurPoints.rows, hokuyoCurPoints.cols, CV_32FC1);
		Mat tmpCurPoints = curPosOrigMapCenter*cameraOrigImu*hokuyoCurPoints.rowRange(0, 4);
		tmpCurPoints.copyTo(curPointCloudOrigMapCenter.rowRange(0, 4));
		hokuyoCurPoints.rowRange(4, 6).copyTo(curPointCloudOrigMapCenter.rowRange(4, 6));
		//cout << hokuyoCurPointsGlobal.channels() << ", " << hokuyoAllPointsGlobal.channels() << endl;
//		cout << pointCloudOrigMapCenter.size() << " " << curPointCloudCameraMapCenter.size() << " " << tmpAllPoints.size() << endl;
//		if(debugLevel >= 1){
//			cout << "pointsSkipped = " << pointsSkipped << endl;
//		}
		curPointCloudOrigMapCenter.copyTo(tmpAllPoints.colRange(pointCloudOrigMapCenter.cols - pointsSkipped,
																	pointCloudOrigMapCenter.cols + hokuyoCurPoints.cols - pointsSkipped));
//		if(debugLevel >= 1){
//			cout << "curPointCloudCameraMapCenter copied" << endl;
//		}
		pointsInfo.push(PointsPacket(hokuyoTimestamp, hokuyoCurPoints.cols));

		pointCloudOrigMapCenter = tmpAllPoints;
	}
	lck.unlock();
}