07:36:32 AM

added parseParams to sub;
09:32:01 AM
added printParams and fixed several inverted indexing bugs;
11:40:53 AM
tried many things but the out of range map problem remains;
12:16:12 PM
finally fixed the issue;

Examples/ROS/ORB_SLAM2/src/ros_mono_pub.cc

@@ -134,7 +134,9 @@ int main(int argc, char **argv)
 			//std::vector<ORB_SLAM2::MapPoint*> map_points = SLAM.getMap()->GetAllMapPoints();
 			std::vector<ORB_SLAM2::MapPoint*> map_points = SLAM.GetTrackedMapPoints();
 			int n_map_pts = map_points.size();
-			printf("n_map_pts: %d\n", n_map_pts);
+
+			//printf("n_map_pts: %d\n", n_map_pts);
+
 			//pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
 			geometry_msgs::PoseArray pt_array;
@@ -153,7 +155,7 @@ int main(int argc, char **argv)
 
 			pt_array.poses.push_back(camera_pose);
 
-			printf("Done getting camera pose\n");
+			//printf("Done getting camera pose\n");
 
 			for (int pt_id = 1; pt_id <= n_map_pts; ++pt_id){
 
@@ -180,7 +182,9 @@ int main(int argc, char **argv)
 			//pcl::toROSMsg(*pcl_cloud, ros_cloud);
 			//ros_cloud.header.frame_id = "1";
 			//ros_cloud.header.seq = ni;
-			printf("valid map pts: %lu\n", pt_array.poses.size()-1);
+
+			//printf("valid map pts: %lu\n", pt_array.poses.size()-1);
+
 			//printf("ros_cloud size: %d x %d\n", ros_cloud.height, ros_cloud.width);
 			//pub_cloud.publish(ros_cloud);
 			pt_array.header.frame_id = "1";

Examples/ROS/ORB_SLAM2/src/ros_mono_sub.cc

@@ -22,20 +22,20 @@
 #include <Converter.h>
 
 // parameters
-const float scale_factor = 3;
-const float resize_factor = 5;
-const float grid_max_x = 29.0;
-const float grid_min_x = -25.0;
-const float grid_max_z = 48.0;
-const float grid_min_z = -12.0;
-
-const float free_thresh = 0.55;
-const float occupied_thresh = 0.50;
-
-const float upper_left_x = -1.5;
-const float upper_left_y = -2.5;
+float scale_factor = 3;
+float resize_factor = 5;
+float grid_max_x = 29.0;
+float grid_min_x = -25.0;
+float grid_max_z = 48.0;
+float grid_min_z = -12.0;
+float free_thresh = 0.55;
+float occupied_thresh = 0.50;
+float upper_left_x = -1.5;
+float upper_left_y = -2.5;
 const int resolution = 10;
+
 cv::Mat occupied_counter, visit_counter;
+cv::Mat grid_map, grid_map_thresh, grid_map_thresh_resized;
 float norm_factor_x, norm_factor_z;
 int h, w;
 
@@ -53,39 +53,52 @@ void kfCallback(const geometry_msgs::PoseStamped::ConstPtr& camera_pose){
 		camera_pose->header.seq);
 }
 void ptCallback(const geometry_msgs::PoseArray::ConstPtr& pts_and_pose){
-	ROS_INFO("I heard: [%s]{%d}", pts_and_pose->header.frame_id.c_str(),
-		pts_and_pose->header.seq);
+	//ROS_INFO("Received points and pose: [%s]{%d}", pts_and_pose->header.frame_id.c_str(),
+	//	pts_and_pose->header.seq);
 	updateGridMap(pts_and_pose);
 }
+void parseParams(int argc, char **argv);
+void printParams();
 
 int main(int argc, char **argv){
 	ros::init(argc, argv, "Monosub");
 	ros::start();
 
+	parseParams(argc, argv);
+	printParams();
+
 	double grid_res_x = grid_max_x - grid_min_x, grid_res_z = grid_max_z - grid_min_z;
 
-	h = grid_res_z*scale_factor;
-	w = grid_res_x*scale_factor;
+	h = grid_res_z;
+	w = grid_res_x;
 	printf("grid_size: (%d, %d)\n", h, w);
 
 	occupied_counter.create(h, w, CV_32SC1);
 	visit_counter.create(h, w, CV_32SC1);
 	occupied_counter.setTo(cv::Scalar(0));
 	visit_counter.setTo(cv::Scalar(0));
 
+	grid_map.create(h, w, CV_32FC1);
+	grid_map_thresh.create(h, w, CV_8UC1);
+	grid_map_thresh_resized.create(h*resize_factor, w*resize_factor, CV_8UC1);
 
 	norm_factor_x = float(grid_res_x - 1) / float(grid_max_x - grid_min_x);
 	norm_factor_z = float(grid_res_z - 1) / float(grid_max_z - grid_min_z);
+	printf("norm_factor_x: %f\n", norm_factor_x);
+	printf("norm_factor_z: %f\n", norm_factor_z);
 
 	ros::NodeHandle nodeHandler;
-	ros::Subscriber sub_cloud = nodeHandler.subscribe("cloud_in", 1000, cloudCallback);
-	//ros::Subscriber sub_kf = nodeHandler.subscribe("camera_pose", 1000, kfCallback);
 	ros::Subscriber sub_pt_array = nodeHandler.subscribe("pts_and_pose", 1000, ptCallback);
+	//ros::Subscriber sub_cloud = nodeHandler.subscribe("cloud_in", 1000, cloudCallback);
+	//ros::Subscriber sub_kf = nodeHandler.subscribe("camera_pose", 1000, kfCallback);
 	//ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage, &igb);
 
 	ros::spin();
 	ros::shutdown();
 
+	cv::imwrite("grid_map_thresh.jpg", grid_map_thresh);
+	cv::imwrite("grid_map_thresh_resized.jpg", grid_map_thresh_resized);
+
 	return 0;
 }
 
@@ -113,147 +126,169 @@ void updateGridMap(const geometry_msgs::PoseArray::ConstPtr& pts_and_pose){
 	// Init Grid Statictics
 	// We set the resolution as 10mm, all points and keyframes are in the range of (-3, -4) to (3, 1),
 	// so the grid map size is 600x500
+
 	//geometry_msgs::Point min_pt, max_pt;
 	//getMixMax(pts_and_pose, min_pt, max_pt);
+	//printf("max_pt: %f, %f\t min_pt: %f, %f\n", max_pt.x*scale_factor, max_pt.z*scale_factor, 
+	//	min_pt.x*scale_factor, min_pt.z*scale_factor);
+	//printf("min_pt: %f, %f\n", min_pt.x*scale_factor, min_pt.z*scale_factor);
 	//double grid_res_x = max_pt.x - min_pt.x, grid_res_z = max_pt.z - min_pt.z;
 
-
 	const geometry_msgs::Point &kf_location = pts_and_pose->poses[0].position;
-	const geometry_msgs::Quaternion &kf_orientation = pts_and_pose->poses[0].orientation;
+	//const geometry_msgs::Quaternion &kf_orientation = pts_and_pose->poses[0].orientation;
 
-	float okf_pos_x = kf_location.x;
-	float okf_pos_y = kf_location.z;
+	float kf_pos_x = kf_location.x*scale_factor;
+	float kf_pos_z = kf_location.z*scale_factor;
 
-	int okf_pos_grid_x = int(floor((okf_pos_x - grid_min_x) * norm_factor_x));
-	int okf_pos_grid_y = int(floor((okf_pos_y - grid_min_z) * norm_factor_z));
+	int kf_pos_grid_x = int(floor((kf_pos_x - grid_min_x) * norm_factor_x));
+	int kf_pos_grid_z = int(floor((kf_pos_z - grid_min_z) * norm_factor_z));
 
-	if (okf_pos_grid_x < 0 || okf_pos_grid_x >= w)
+	if (kf_pos_grid_x < 0 || kf_pos_grid_x >= w)
 		return;
 
-	if (okf_pos_grid_y < 0 || okf_pos_grid_y >= h)
+	if (kf_pos_grid_z < 0 || kf_pos_grid_z >= h)
 		return;
 
-	for (int i = 1; i < pts_and_pose->poses.size(); i++){
+	for (int pt_id = 1; pt_id < pts_and_pose->poses.size(); ++pt_id){
 
-		const geometry_msgs::Point &curr_pt = pts_and_pose->poses[i].position;
+		const geometry_msgs::Point &curr_pt = pts_and_pose->poses[pt_id].position;
 
-		float mp_pos_x = curr_pt.x*scale_factor;
-		float mp_pos_y = curr_pt.z*scale_factor;
+		float pt_pos_x = curr_pt.x*scale_factor;
+		float pt_pos_z = curr_pt.z*scale_factor;
 
-		int mp_pos_grid_x = int(floor((mp_pos_x - grid_min_x) * norm_factor_x));
-		int mp_pos_grid_y = int(floor((mp_pos_y - grid_min_z) * norm_factor_z));
+		int pt_pos_grid_x = int(floor((pt_pos_x - grid_min_x) * norm_factor_x));
+		int pt_pos_grid_z = int(floor((pt_pos_z - grid_min_z) * norm_factor_z));
 
 
-		if (mp_pos_grid_x < 0 || mp_pos_grid_x >= w)
+		if (pt_pos_grid_x < 0 || pt_pos_grid_x >= w)
 			continue;
 
-		if (mp_pos_grid_y < 0 || mp_pos_grid_y >= h)
+		if (pt_pos_grid_z < 0 || pt_pos_grid_z >= h)
 			continue;
 
 		// Increment the occupency account of the grid cell where map point is located
-		++occupied_counter.at<int>(mp_pos_grid_y, mp_pos_grid_x);
+		++occupied_counter.at<int>(pt_pos_grid_z, pt_pos_grid_x);
 
 		//cout << "----------------------" << endl;
 		//cout << okf_pos_grid_x << " " << okf_pos_grid_y << endl;
 
 		// Get all grid cell that the line between keyframe and map point pass through
-		int x0 = okf_pos_grid_x;
-		int y0 = okf_pos_grid_y;
-		int x1 = mp_pos_grid_x;
-		int y1 = mp_pos_grid_y;
+		int x0 = kf_pos_grid_x;
+		int y0 = kf_pos_grid_z;
+		int x1 = pt_pos_grid_x;
+		int y1 = pt_pos_grid_z;
 		bool steep = (abs(y1 - y0) > abs(x1 - x0));
 		if (steep){
-			x0 = okf_pos_grid_y;
-			y0 = okf_pos_grid_x;
-			x1 = mp_pos_grid_y;
-			y1 = mp_pos_grid_x;
+			swap(x0, y0);
+			swap(x1, y1);
 		}
 		if (x0 > x1){
-			x0 = mp_pos_grid_y;
-			x1 = okf_pos_grid_y;
-			y0 = mp_pos_grid_x;
-			y1 = okf_pos_grid_x;
+			swap(x0, x1);
+			swap(y0, y1);
 		}
-		int deltax = x1 - x0;
-		int deltay = abs(y1 - y0);
+		int dx = x1 - x0;
+		int dy = abs(y1 - y0);
 		double error = 0;
-		double deltaerr = ((double)deltay) / ((double)deltax);
+		double deltaerr = ((double)dy) / ((double)dx);
 		int y = y0;
 		int ystep = (y0 < y1) ? 1 : -1;
-		for (int x = x0; x <= x1; x++){
+		for (int x = x0; x <= x1; ++x){
 			if (steep) {
 				++visit_counter.at<int>(x, y);
 				//++visit_counter[y][x];
-			}
+			}
 			else {
 				++visit_counter.at<int>(y, x);
 				//++visit_counter[x][y];
 			}
 			error = error + deltaerr;
-			if (error >= 0.5)
-			{
+			if (error >= 0.5){
 				y = y + ystep;
 				error = error - 1.0;
 			}
 		}
 	}
-	cv::Mat grid_map(h, w, CV_32FC1, cv::Scalar(0));
-	cv::Mat grid_map_thresh(h, w, CV_8UC1, cv::Scalar(0));
-
-	for (int i = 0; i < h; i++){
-		for (int j = 0; j < w; j++){
-			int visits = visit_counter.at<int>(j, i);
-			int occupieds = occupied_counter.at<int>(j, i);
+	for (int row = 0; row < h; ++row){
+		for (int col = 0; col < w; ++col){
+			int visits = visit_counter.at<int>(row, col);
+			int occupieds = occupied_counter.at<int>(row, col);
 
 			if (visits == 0 || occupieds == 0){
-				grid_map.at<float>(j, i) = 0.5;
+				grid_map.at<float>(row, col) = 0.5;
 			} else {
-				grid_map.at<float>(j, i) = 1 - float(occupieds / visits);
+				grid_map.at<float>(row, col) = 1 - float(occupieds / visits);
 			}
-			if (grid_map.at<float>(j, i) >= free_thresh) {
-				grid_map_thresh.at<uchar>(j, i) = 255;
+			if (grid_map.at<float>(row, col) >= free_thresh) {
+				grid_map_thresh.at<uchar>(row, col) = 255;
 			}
-			else if (grid_map.at<float>(j, i) < free_thresh && grid_map.at<float>(j, i) >= occupied_thresh) {
-				grid_map_thresh.at<uchar>(j, i) = 128;
+			else if (grid_map.at<float>(row, col) < free_thresh && grid_map.at<float>(row, col) >= occupied_thresh) {
+				grid_map_thresh.at<uchar>(row, col) = 128;
 			} else {
-				grid_map_thresh.at<uchar>(j, i) = 0;
+				grid_map_thresh.at<uchar>(row, col) = 0;
 			}
 		}
 	}
-	cv::imshow("grid_map_thresh", grid_map_thresh);
-	cv::waitKey(1);
+	cv::resize(grid_map_thresh, grid_map_thresh_resized, grid_map_thresh_resized.size());
+	cv::imshow("grid_map_thresh_resized", grid_map_thresh_resized);
+	int key = cv::waitKey(1);
+	if(key==27) {
+		ros::shutdown();
+		exit(0);
+	}
+	else if (key == 'f') {
+		free_thresh -= 1;
+		printf("Setting free_thresh to: %f\n", free_thresh);
+	}
+	else if (key == 'F') {
+		free_thresh += 1;
+		printf("Setting free_thresh to: %f\n", free_thresh);
+	}
+	else if (key == 'o') {
+		occupied_thresh -= 1;
+		printf("Setting occupied_thresh to: %f\n", occupied_thresh);
+	}
+	else if (key == 'O') {
+		occupied_thresh -= 1;
+		printf("Setting occupied_thresh to: %f\n", occupied_thresh);
+	}
 	//cout << endl << "Grid map saved!" << endl;
 }
 
-void LoadImages(const string &strPathToSequence, vector<string> &vstrImageFilenames, vector<double> &vTimestamps){
-	ifstream fTimes;
-	string strPathTimeFile = strPathToSequence + "/times.txt";
-	fTimes.open(strPathTimeFile.c_str());
-	while (!fTimes.eof())
-	{
-		string s;
-		getline(fTimes, s);
-		if (!s.empty())
-		{
-			stringstream ss;
-			ss << s;
-			double t;
-			ss >> t;
-			vTimestamps.push_back(t);
-		}
+void parseParams(int argc, char **argv) {
+	int arg_id = 1;
+	if (argc > arg_id){
+		scale_factor = atof(argv[arg_id++]);
 	}
-
-	string strPrefixLeft = strPathToSequence + "/image_0/";
-
-	const int nTimes = vTimestamps.size();
-	vstrImageFilenames.resize(nTimes);
-
-	for (int i = 0; i < nTimes; i++)
-	{
-		stringstream ss;
-		ss << setfill('0') << setw(6) << i;
-		vstrImageFilenames[i] = strPrefixLeft + ss.str() + ".png";
+	if (argc > arg_id){
+		resize_factor = atof(argv[arg_id++]);
+	}
+	if (argc > arg_id){
+		grid_max_x = atof(argv[arg_id++]);
+	}
+	if (argc > arg_id){
+		grid_min_x = atof(argv[arg_id++]);
+	}	
+	if (argc > arg_id){
+		grid_max_z = atof(argv[arg_id++]);
+	}
+	if (argc > arg_id){
+		grid_min_z = atof(argv[arg_id++]);
+	}
+	if (argc > arg_id){
+		free_thresh = atof(argv[arg_id++]);
+	}
+	if (argc > arg_id){
+		occupied_thresh = atof(argv[arg_id++]);
 	}
 }
 
+void printParams() {
+	printf("Using params:\n");
+	printf("scale_factor: %f\n", scale_factor);
+	printf("resize_factor: %f\n", resize_factor);
+	printf("grid_max: %f, %f\t grid_min: %f, %f\n", grid_max_x, grid_max_z, grid_min_x, grid_min_z);
+	//printf("grid_min: %f, %f\n", grid_min_x, grid_min_z);
+	printf("free_thresh: %f\n", free_thresh);
+	printf("occupied_thresh: %f\n", occupied_thresh);
+}
 

ORB_SLAM2.sln.DotSettings.user

@@ -0,0 +1,2 @@
+<wpf:ResourceDictionary xml:space="preserve" xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml" xmlns:s="clr-namespace:System;assembly=mscorlib" xmlns:ss="urn:shemas-jetbrains-com:settings-storage-xaml" xmlns:wpf="http://schemas.microsoft.com/winfx/2006/xaml/presentation">
+	<s:Boolean x:Key="/Default/Housekeeping/Search/HighlightUsagesHintUsed/@EntryValue">True</s:Boolean></wpf:ResourceDictionary>

gitu.sh

@@ -0,0 +1,4 @@
+#!/bin/bash -v
+git add --all .
+git commit
+git push origin master