change for initial frame update

corelib/src/odometry/OdometryMSCKF.cpp

@@ -672,18 +672,17 @@ class MsckfVioNoROS: public msckf_vio::MsckfVio
 		T_i_w.linear() = msckf_vio::quaternionToRotation(imu_state.orientation).transpose();
 		T_i_w.translation() = imu_state.position;
 
-		Eigen::Isometry3d T_b_w = msckf_vio::IMUState::T_imu_body * T_i_w *
-				msckf_vio::IMUState::T_imu_body.inverse();
+		Eigen::Isometry3d T_b_w = T_i_w * msckf_vio::IMUState::T_imu_body.inverse();
 		Eigen::Vector3d body_velocity =
 				msckf_vio::IMUState::T_imu_body.linear() * imu_state.velocity;
 
 		// Publish tf
 		/*if (publish_tf) {
-	    tf::Transform T_b_w_tf;
-	    tf::transformEigenToTF(T_b_w, T_b_w_tf);
-	    tf_pub.sendTransform(tf::StampedTransform(
-	          T_b_w_tf, time, fixed_frame_id, child_frame_id));
-	  }*/
+			tf::Transform T_b_w_tf;
+			tf::transformEigenToTF(T_b_w, T_b_w_tf);
+			tf_pub.sendTransform(tf::StampedTransform(
+				T_b_w_tf, time, fixed_frame_id, child_frame_id));
+		}*/
 
 		// Publish the odometry
 		nav_msgs::Odometry odom_msg;
@@ -725,20 +724,18 @@ class MsckfVioNoROS: public msckf_vio::MsckfVio
 		// Publish the 3D positions of the features that
 		// has been initialized.
 		feature_msg_ptr.reset(new pcl::PointCloud<pcl::PointXYZ>());
-	  feature_msg_ptr->header.frame_id = fixed_frame_id;
-	  feature_msg_ptr->height = 1;
-	  for (const auto& item : map_server) {
-	    const auto& feature = item.second;
-	    if (feature.is_initialized) {
-	      Eigen::Vector3d feature_position =
-	        msckf_vio::IMUState::T_imu_body.linear() * feature.position;
-	      feature_msg_ptr->points.push_back(pcl::PointXYZ(
-	            feature_position(0), feature_position(1), feature_position(2)));
-	    }
-	  }
-	  feature_msg_ptr->width = feature_msg_ptr->points.size();
-
-	  //feature_pub.publish(feature_msg_ptr);
+		feature_msg_ptr->header.frame_id = fixed_frame_id;
+		feature_msg_ptr->height = 1;
+		for (const auto& item : map_server) {
+			const auto& feature = item.second;
+			if (feature.is_initialized) {
+				feature_msg_ptr->points.push_back(pcl::PointXYZ(
+					feature.position(0), feature.position(1), feature.position(2)));
+			}
+		}
+		feature_msg_ptr->width = feature_msg_ptr->points.size();
+
+		// feature_pub.publish(feature_msg_ptr);
 
 		return odom_msg;
 	}
@@ -755,7 +752,7 @@ OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) :
 imageProcessor_(0),
 msckf_(0),
 parameters_(parameters),
-fixPoseRotation_(0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, 0),
+fixPoseRotation_(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0),
 previousPose_(Transform::getIdentity()),
 initGravity_(false)
 #endif