system fix

lara-unb · Dec 14, 2023 · 337349c · 337349c
1 parent 091c0d0
commit 337349c
Show file tree

Hide file tree

Showing 5 changed files with 15 additions and 12 deletions.
diff --git a/catkin_ws_camera/src/kalman_filter/src/filter_variables.cpp b/catkin_ws_camera/src/kalman_filter/src/filter_variables.cpp
@@ -66,4 +66,5 @@ previous_id(1)
 void cameraBasis::updateTfPrevious(poseTransform new_marker_tf, poseTransform  marker_tf_from_previous, int new_previous_id) {
     previous_id = new_previous_id;
     previous_tf = new_marker_tf * marker_tf_from_previous.inverse();
+    previous_tf = previous_tf;
 }
diff --git a/catkin_ws_camera/src/kalman_filter/src/kalman_filter.hpp b/catkin_ws_camera/src/kalman_filter/src/kalman_filter.hpp
@@ -24,7 +24,7 @@
 //	K[k] = P[k] * H^{T} * ((H * P[k] * H^{T} + R)^{-1})	// Calculating Kalman gain
 //	X[k] = X[k] + K[k] * (Y[k] - H * X[k])				// after measuring Y[k], calculating a posteriori state estimate
 //	P[k] = (I - K[k] * H) * P[k]						// Calculate a posteriori error covariance estimate
-int multiplier = 1;
+
 class filter{
 
 	public:
@@ -72,7 +72,7 @@ class filter{
 			}
 
 			R.resize(dim, dim);
-			R0 = Eigen::MatrixXd::Identity(POSE_VECTOR_SIZE, POSE_VECTOR_SIZE)*multiplier;
+			R0 = Eigen::MatrixXd::Identity(POSE_VECTOR_SIZE, POSE_VECTOR_SIZE);
 		}
 
 		void resetWorld(int worldAddr){
@@ -122,7 +122,7 @@ class filter{
 			}
 
 			// Regarding the measurement noise covariance matrix
-			R = Eigen::MatrixXd::Identity(R.rows() + size_difference, R.cols() + size_difference)*multiplier;
+			R = Eigen::MatrixXd::Identity(R.rows() + size_difference, R.cols() + size_difference);
 
 			// if(X.rows() == POSE_VECTOR_SIZE){
 			// 	X.setZero();

diff --git a/catkin_ws_camera/src/kalman_filter/src/ros_filter.cpp b/catkin_ws_camera/src/kalman_filter/src/ros_filter.cpp
@@ -52,7 +52,7 @@ ros::Timer RosFilter::createTimer(ros::Duration period) {
 
 void RosFilter::insertCameraBasis(int camera_id) {
     // Verifying if this camera is already tracked. If not, insert in the system
-    poseTransform tf = poseTransform(tf::Quaternion(0.5,0.5,0.5,0.5), tf::Vector3(0,0,0));
+    poseTransform tf = poseTransform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)); // Transformation to convert rviz axis representation to aruco
     if(!camera_poses.contains(camera_id)) {
         cameraBasis aux((tracked_poses++) * POSE_VECTOR_SIZE,
                          tfToPose(tf),
@@ -103,6 +103,9 @@ Eigen::VectorXd RosFilter::msgToPose(geometry_msgs::Pose pose) {
                                     pose.orientation.w);
 
     transform = poseTransform(msg_orientation, msg_pose);
+    adjust_rotation = tf::Quaternion(0,0,1,0);   // Rotation on the z axis to adjust to our system from what we receive from aruco_ros
+    auto adjust_rotation_tf =  poseTransform(adjust_rotation);
+    transform = adjust_rotation_tf * transform;
     return tfToPose(transform);
 }
 
@@ -192,12 +195,9 @@ void RosFilter::insertPosesOnStateVector(Eigen::VectorXd& newState, int cam_id)
     camera_poses[cam_id].pose = tfToPose(previous_tf * poseToTf(camera_poses[previous_id].pose));
     std::cout << "camera " << cam_id << " POSE: " << camera_poses[cam_id].pose << std::endl; 
     newState.segment(camera_poses[cam_id].stateVectorAddr, POSE_VECTOR_SIZE) = camera_poses[cam_id].pose;
-
-    auto adjust_rotation = tf::Quaternion(0, 0, 1, 0);   // Rotation on the z axis to adjust to our system from what we receive from aruco_ros
-    auto adjust_rotation_tf =  poseTransform(adjust_rotation);
-
-    for(const auto& marker : cam_markers[cam_id]){
-        newState.segment(marker.stateVectorAddr, POSE_VECTOR_SIZE) = tfToPose(adjust_rotation_tf * poseToTf(marker.pose));
+
+    for(const auto& marker : cam_markers[cam_id]){  
+        newState.segment(marker.stateVectorAddr, POSE_VECTOR_SIZE) = tfToPose( poseToTf(camera_poses[cam_id].pose) * poseToTf(marker.pose) );
         inserted_markers.push_back(marker.arucoId);                                                                                
     }
 
@@ -212,7 +212,7 @@ void RosFilter::insertPosesOnStateVector(Eigen::VectorXd& newState, int cam_id)
                 if(std::find(inserted_markers.begin(),
                              inserted_markers.end(), 
                              marker.arucoId) == inserted_markers.end()) {
-                    newState.segment(marker.stateVectorAddr, POSE_VECTOR_SIZE) = tfToPose(poseToTf(marker.pose));
+                    newState.segment(marker.stateVectorAddr, POSE_VECTOR_SIZE) = tfToPose( poseToTf(camera_poses[id].pose) * poseToTf(marker.pose));
                 }
             }
         }

diff --git a/catkin_ws_camera/src/marker_viz/src/marker_viz.cpp b/catkin_ws_camera/src/marker_viz/src/marker_viz.cpp
@@ -93,6 +93,8 @@ void VisualizationHandler::add_markers(std::string topic){
 // TF_translation = [x,y,z]
 // TF_rotation(quaternion) = [x,y,z,w]
 void  VisualizationHandler::send_tfs(){
+    auto map = tf::StampedTransform(tf::Transform(tf::Quaternion(0.5,0.5,0.5,0.5), tf::Vector3(0,0,0)) , ros::Time::now(),"map", "world_aruco");
+    br.sendTransform(map);
     for(auto it = sys_cameras.begin(); it != sys_cameras.end(); it++){
         br.sendTransform(it->get_tf_stamped());
     }

diff --git a/catkin_ws_camera/src/marker_viz/src/visualization_objects.cpp b/catkin_ws_camera/src/marker_viz/src/visualization_objects.cpp
@@ -126,7 +126,7 @@ Camera::Camera(tf::Vector3 tr, tf::Quaternion rot, std::string id){
 }
 
 tf::StampedTransform Camera::get_tf_stamped() const {
-    return tf::StampedTransform(tf, ros::Time::now(),"map", frame_id);
+    return tf::StampedTransform(tf, ros::Time::now(),"world_aruco", frame_id);
 }
 
 bool Camera::operator==(const Camera& obj){