test

libs/trajectory/trajectory.cpp

@@ -35,6 +35,7 @@ bool Trajectory::init()
     boost::function<void(const vector<string>&, uint8_t)> parse_callback =
             boost::bind(&Trajectory::parseAndPush, this, _1, _2);
     io_->setParseCallback(parse_callback);
+    multiTrajectory_.resize(std::thread::hardware_concurrency());
 
     if(!io_){
         LOG(WARNING) << "io_ is nullptr!";
@@ -340,11 +341,6 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
         trajPoint.rotation = EulerToMatrix3d(Vector3d(ppk_raw_info.pitch, ppk_raw_info.roll, ppk_raw_info.yaw));
         trajPoint.nodeId = trajectory_.size();
         trajPoint.timestamp = gpsToUtc(ppk_raw_info.gps_week, ppk_raw_info.gps_sec);
-        if((index + 1) >= multiTrajectory_.size()){
-            boost::unique_lock<boost::shared_mutex> writeLock(Mtx_);
-            multiTrajectory_.resize(index + 1);
-        }
-        boost::shared_lock<boost::shared_mutex> readLock(Mtx_);
         if(multiTrajectory_[index].size() == 0){
             multiTrajectory_[index].push_back(trajPoint);
         }else{