วัดผังอาคารด้วยมือถือ แล้วหาทิศเหนือจริงด้วย GNSS

พอดีผมต้องขึ้นแบบอาคารเก่าที่ไม่มีแบบเหลืออยู่แล้ว ต้องเข้าไปวัดเอง งานแบบนี้ปกติใช้ตลับเมตรกับเลเซอร์วัดระยะ ซึ่งได้ระยะแม่นดี แต่กว่าจะประกอบเป็นผังรวมทั้งชั้นก็เสียเวลามาก และห้องที่รูปทรงไม่ตั้งฉากจะยากเป็นพิเศษ

ผมเลยลองเขียนแอปเล็ก ๆ บน Android ที่ใช้ ARCore เก็บมุมห้อง คือเดินไปจ่อกล้องที่มุมห้องแล้วกดปุ่ม มันเก็บพิกัดสามมิติของจุดนั้นไว้ให้ ได้ผังออกมาเป็นรูปหลายเหลี่ยมพร้อมพื้นที่กับความยาวรอบรูป

ปัญหาโผล่ทันทีตอนเอาผังไปวางบนโมเดลที่ผูกพิกัดโลกไว้แล้ว คือผังมันหันมั่ว บทความนี้คือวิธีที่ผมแก้เรื่องนั้น

ปัญหา: ARCore ไม่รู้ว่าทิศเหนืออยู่ไหน

ARCore สร้างระบบพิกัดของตัวเองขึ้นมาตอนเริ่มเซสชัน แกน y ชี้ขึ้นตามแรงโน้มถ่วง อันนี้แน่นอน แต่แกน x กับ z หันไปทางไหนขึ้นกับว่าตอนเปิดแอปเราถือเครื่องหันหน้าไปทางไหน ไม่มีอะไรผูกกับทิศเหนือจริงเลย

ผลคือผังที่ได้รูปร่างถูก ระยะถูก แต่หมุนอยู่มุมหนึ่งที่เราไม่รู้ค่า พอเอาไปวางบนโมเดลที่ผูกพิกัดโลก อาคารจะเฉียงไปจากของจริง ถ้าเดาหมุนเอาก็ได้ค่าที่ "ดูเข้าท่า" แต่ไม่มีอะไรยืนยัน

ทางแก้ที่คนนึกถึงก่อนคือใช้เข็มทิศในเครื่อง แต่เข็มทิศในอาคารเชื่อไม่ค่อยได้ครับ โครงเหล็ก ตู้ไฟ มอเตอร์ ดึงกันหมด ผมเก็บค่าเข็มทิศไว้ด้วยจริง แต่ใช้เป็นตัวตรวจสอบข้าง ๆ ไม่ใช้เป็นคำตอบ

แนวคิดที่ใช้แก้คือเดินเป็นเส้นตรง แล้วให้ดาวเทียมบอกทิศให้

ระหว่างที่เดินเก็บมุมห้อง เครื่องมันได้ค่า GNSS มาเรื่อย ๆ อยู่แล้ว ทุก fix ที่ได้คือคู่ข้อมูลฟรี หนึ่งคู่ คือ "ตอนนั้นอยู่ที่ไหนในโลก AR" กับ "ตอนนั้นอยู่ที่ไหนบนโลกจริง"

เก็บคู่แบบนี้ไว้หลาย ๆ คู่ตลอดเส้นทางเดิน ก็จะได้ชุดจุดสองชุดที่เป็นรูปร่างเดียวกัน หน่วยเมตรเหมือนกัน ต่างกันแค่หมุนกับเลื่อน หาการหมุนระหว่างสองชุดนี้ได้ ก็ได้ทิศของเซสชันนั้น

เก็บตัวอย่างระหว่างทำงาน ไม่ต้องมีขั้นตอนคาลิเบรตแยก

ตอนแรกผมทำเป็นปุ่ม "คาลิเบรต" ให้กดแล้วเดินเป็นเส้นตรง แต่ใช้จริงแล้วน่ารำคาญ เพราะต้องหยุดงานมาเดินอย่างเดียว ผมเลยเปลี่ยนเป็นเก็บไปเรื่อย ๆ ระหว่างทำงานปกติ ยิ่งเดินมากยิ่งดี

เงื่อนไขการเก็บมีสามข้อ ตัดของเสียตั้งแต่ต้นทาง

private val locCb = object : LocationCallback() {
    override fun onLocationResult(r: LocationResult) {
        val l = r.lastLocation ?: return
        lastLoc = l
        // ทิ้ง fix ที่แย่ และทิ้งช่วงที่ AR ยังจับพื้นไม่ได้
        if (!renderer.tracking || l.accuracy > 15f) return
        val last = calSamples.lastOrNull()
        // เก็บเฉพาะตอนที่เครื่อง "ขยับจริง" ไม่งั้นได้ตัวอย่างซ้ำที่จุดเดิมเป็นร้อย
        if (last != null &&
            hypot(renderer.camX - last.arX, renderer.camZ - last.arZ) <= 0.8) return

        calSamples.add(GeoSolver.Sample(renderer.camX, renderer.camZ,
            l.latitude, l.longitude, l.accuracy.toDouble()))

        if (calSamples.size >= GeoSolver.MIN_SAMPLES && calSamples.size - solvedAt >= 3) {
            solvedAt = calSamples.size
            GeoSolver.solve(calSamples.toList())?.let { r2 ->
                val cur = georef
                // เก็บ fit ที่ความไม่แน่นอนของทิศต่ำที่สุด ไม่ใช่ตัวล่าสุด
                if (cur == null || r2.headingErrDeg <= cur.headingErrDeg) georef = r2
            }
        }
    }
}

จุดที่ผมชอบที่สุดของแบบนี้คือค่าทิศดีขึ้นเองระหว่างทำงาน เดินไปเก็บห้องเพิ่ม ระยะเดินรวมก็ยาวขึ้น ค่าที่ได้ก็แน่นขึ้นตาม โดยไม่ต้องทำอะไรเป็นพิเศษ

ตัวคำนวณ: Kabsch สองมิติ ถ่วงน้ำหนักด้วยความแม่นของ fix

ตัวสมการเป็นการหา rotation ที่ทำให้สองชุดจุดทับกันดีที่สุด แบบสองมิติมันสั้นมาก ไม่ต้องใช้ SVD ด้วยซ้ำ แค่รวม cross product กับ dot product แล้ว atan2 เอา

หัวใจอยู่ที่น้ำหนัก ครับ fix ที่บอกว่าแม่น 3 เมตร กับ fix ที่บอกว่าแม่น 11 เมตร ไม่ควรมีเสียงเท่ากัน ผมถ่วงด้วย 1 ส่วนความแม่นยกกำลังสอง ตามธรรมเนียมการถ่วงด้วยความแปรปรวน

// plan frame: app x -> right, -z -> forward. ENU frame: east, north.
val px = s.map { it.arX }
val py = s.map { -it.arZ }
val gx = s.map { (it.lon - mLon) * mpl }        // เมตรทางตะวันออก
val gy = s.map { (it.lat - mLat) * M_PER_LAT }  // เมตรทางเหนือ

val mpx = px.average(); val mpy = py.average()
val mgx = gx.average(); val mgy = gy.average()

var sc = 0.0; var ss = 0.0
for (i in s.indices) {
    val w = 1.0 / (s[i].acc * s[i].acc)         // fix ที่แม่นกว่าออกเสียงดังกว่า
    val ax = px[i] - mpx; val ay = py[i] - mpy
    val bx = gx[i] - mgx; val by = gy[i] - mgy
    sc += w * (ax * bx + ay * by)
    ss += w * (ax * by - ay * bx)
}
val th = atan2(ss, sc)                          // มุมหมุนจากแกน AR ไปแกน ENU

ได้มุมแล้วก็หา translation ที่ทำให้จุดทับกัน แล้วย้อนกลับไปว่า origin ของโลก AR (จุด 0,0) อยู่ที่ละติจูดลองจิจูดเท่าไร ตรงนี้แหละที่ทำให้แปลงมุมห้องทุกจุดเป็น lat/lon ได้ทีหลัง

val c = cos(th); val sn = sin(th)
val tE = mgx - (c * mpx - sn * mpy)
val tN = mgy - (sn * mpx + c * mpy)

var sse = 0.0
for (i in s.indices) {
    val e = c * px[i] - sn * py[i] + tE
    val n = sn * px[i] + c * py[i] + tN
    sse += (e - gx[i]) * (e - gx[i]) + (n - gy[i]) * (n - gy[i])
}
val rms = sqrt(sse / s.size)                    // ค่าตกค้างของ fit หน่วยเมตร

val originLat = mLat + tN / M_PER_LAT
val originLon = mLon + tE / mPerLon(mLat)
val headErr = Math.toDegrees(atan2(rms.coerceAtLeast(0.5), walk))

ความแม่นของทิศ = atan(ความคลาดของ GNSS / ระยะที่เดิน)

บรรทัดสุดท้ายข้างบนคือสิ่งที่ผมคิดว่าสำคัญที่สุดในทั้งเรื่อง มันเป็นสูตรง่าย ๆ ที่บอกได้ทันทีว่าค่าทิศที่ได้เชื่อได้แค่ไหน

คิดง่าย ๆ ว่าเรามีคานยาวเท่ากับระยะที่เดิน ปลายคานสั่นอยู่เท่ากับความคลาดของ GNSS มุมที่คานส่ายได้ก็คือ arctangent ของสองค่านี้ คานยิ่งยาว ปลายสั่นเท่าเดิมก็ส่ายเป็นมุมน้อยลง

ระยะเดินตัวนี้ผมคิดจากคู่จุดที่ห่างกันที่สุดในชุดตัวอย่าง ไม่ใช่ระยะทางสะสม เพราะเดินวนอยู่ในห้องเล็ก ๆ ระยะสะสมเยอะแต่คานสั้น ค่าทิศก็ยังไม่ดีขึ้น

/** Straight-line extent of the AR track - the lever arm the fit depends on. */
fun walkExtent(s: List<Sample>): Double {
    if (s.size < 2) return 0.0
    var best = 0.0
    for (i in s.indices) for (j in i + 1 until s.size) {
        val d = hypot(s[i].arX - s[j].arX, s[i].arZ - s[j].arZ)
        if (d > best) best = d
    }
    return best
}

อ่าน GnssStatus ให้รู้ว่าควรเชื่อค่าไหม

ค่า accuracy ที่ระบบคืนมาเป็นตัวเลขตัวเดียว มันไม่บอกว่าเบื้องหลังเป็นยังไง ผมเลยดึง GnssStatus มาแยกดูรายกลุ่มดาวเทียมด้วย ว่าเห็นกี่ดวง ใช้จริงกี่ดวง และมีดวงที่ส่งความถี่ที่สองไหม

override fun onSatelliteStatusChanged(status: GnssStatus) {
    val map = LinkedHashMap<String, Con>()
    var seen = 0; var used = 0; var nDual = 0
    for (i in 0 until status.satelliteCount) {
        val k = label(status.getConstellationType(i))   // GPS / BDS / GAL / GLO / QZS / IRN
        val c2 = map.getOrPut(k) { Con(k) }
        c2.seen++; seen++
        if (status.usedInFix(i)) { c2.used++; used++ }
        val cn0 = status.getCn0DbHz(i)
        if (cn0 > c2.bestCn0) c2.bestCn0 = cn0
        if (Build.VERSION.SDK_INT >= 26 && status.hasCarrierFrequencyHz(i)) {
            // L1 อยู่ราว 1575.42 MHz อะไรที่ต่ำกว่านี้ชัดเจนคือย่านที่สอง (L5/E5a/B2a)
            if (status.getCarrierFrequencyHz(i) < 1.3e9f) nDual++
        }
    }
    cons = map.values.sortedByDescending { it.used }
    totalSeen = seen; totalUsed = used; dualCount = nDual
}

การเห็นตัวเลขรายกลุ่มมีประโยชน์กว่าที่คิด เวลาอยู่ริมอาคารสูงแล้วค่าเริ่มเพี้ยน จะเห็นเลยว่าจำนวนที่ใช้จริงหล่นฮวบทั้งที่ยังเห็นเยอะอยู่ ต่างจากตอนอยู่กลางแจ้งที่ used กับ seen ไล่กันติด

ของงานจริงรอบที่ผมเก็บได้ดีที่สุด ตัวเลขออกมาแบบนี้

หมายเหตุว่า 4.3 องศาที่ระยะเดินแค่ 19.9 เมตร ดีกว่าที่สูตรคร่าว ๆ ทำนายไว้ เพราะตัวอย่าง 24 จุดช่วยเฉลี่ยความคลาดของแต่ละ fix ลงไปเยอะ สูตร atan(acc/walk) เป็นค่าหยาบสำหรับกรณีตัวอย่างน้อย ๆ ส่วนของจริงให้ดูที่ rms ของ fit

เข็มทิศเป็นผู้ตรวจสอบ ไม่ใช่คำตอบ

ผมให้แอปคำนวณทิศจากเข็มทิศไว้อีกทางแบบแยกกันเลย แล้วเอาสองค่ามาเทียบกันบนหน้าจอ ถ้าต่างกันเกิน 25 องศาผมถือว่าไม่เชื่อทั้งคู่ แล้วเปลี่ยนสีตัวเลขเป็นแดงเตือน

การมีสองวิธีที่ไม่เกี่ยวกันเลยแล้วเอามาเทียบ ช่วยจับกรณีที่ผิดแบบเงียบ ๆ ได้ดีมาก ส่วนตัวผมว่าคุ้มกว่าการพยายามทำให้วิธีใดวิธีหนึ่งแม่นขึ้นอีกนิดหน่อย

ทำความสะอาดข้อมูลก่อนเข้าโมเดล

ไฟล์ที่ส่งออกมาจากมือถือดิบกว่าที่คิด มีสามอาการที่ต้องล้างก่อน

1. เซสชันเดียวส่งออกหลายรอบ ARCore ปรับ pose ของ anchor เรื่อย ๆ ระหว่างเซสชัน ไฟล์ที่ส่งออกทีหลังเลยเป็นตัวที่ถูกกว่า ต้องรู้ว่าไฟล์ไหนแทนที่ไฟล์ไหน ผมดูจากรายชื่อห้อง ถ้ารายชื่อของไฟล์หนึ่งขึ้นต้นด้วยรายชื่อของอีกไฟล์ แปลว่าเซสชันเดียวกัน
2. ปิดลูปแล้วมีจุดซ้อนจุดแรก เดินครบรอบห้องแล้วกดที่มุมแรกอีกที เพื่อวัด drift จุดท้ายเลยไปทับจุดแรก ต้องตัดทิ้งก่อนคิดพื้นที่
3. ห้องเดียวยิงซ้ำสองรอบ เอามาเฉลี่ยกันได้ ถ้ารูปร่างใกล้กันพอ

เกณฑ์ระยะสองตัวที่ผมใช้คือ

CLOSE_TOL = 0.60     # ม. - จุดท้ายที่ใกล้จุดแรกกว่านี้ = ปิดลูป ตัดทิ้ง
DUP_TOL   = 1.20     # ม. - ระยะเฉลี่ยรายมุมต่ำกว่านี้ = ห้องเดียวกันยิงซ้ำ เอามาเฉลี่ย


def drop_closure(pts):
    p = list(pts)
    while len(p) > 3 and d2(p[-1], p[0]) < CLOSE_TOL:
        p.pop()
    q = [p[0]]                       # ยุบจุดที่ติดกันเกินไปด้วย (กดปุ่มซ้ำ)
    for v in p[1:]:
        if d2(v, q[-1]) > 0.15:
            q.append(v)
    return q

ตัวเลข 0.60 กับ 1.20 ไม่ใช่เลขสวย ๆ ครับ มาจากการดูค่าจริงที่ได้แล้วเลือกจุดที่แยกสองกลุ่มออกจากกันชัดที่สุด งานอื่นอาจต้องขยับ

ตัวไฟล์ที่ส่งออกหน้าตาประมาณนี้ (ปิดพิกัดจริงไว้ ใส่ค่าตัวอย่างแทน)

{
  "app": "CornerMap",
  "created": "2026-07-27T06:57:47+07:00",
  "axes": "ARCore world: y=up (gravity). floor plan = (x,z) metres.",
  "georef": {
    "origin_lat": 14.2000000, "origin_lon": 100.6000000,
    "heading_deg": -58.685, "axis_bearing_deg": 148.685,
    "rms_m": 1.5009, "samples": 24, "walk_m": 19.873, "heading_err_deg": 4.319
  },
  "gnss_status": {
    "used": 49, "seen": 64, "dual_band": true,
    "constellations": {
      "BDS": {"used": 16, "seen": 19, "best_cn0": 45.3},
      "GPS": {"used": 12, "seen": 21, "best_cn0": 42.0},
      "GAL": {"used": 11, "seen": 12, "best_cn0": 42.6}
    }
  },
  "rooms": [
    {"name": "ห้อง A", "closed": true, "area_m2": 620.57, "perimeter_m": 99.31,
     "corners": [
       {"seq": 1, "level": "floor", "x": -5.831, "y": -1.384, "z": 3.030,
        "lat": 14.2000512, "lon": 100.6000431,
        "gnss": {"acc_m": 5.18, "age_s": 0.148}}
     ]}
  ]
}

สังเกตว่าแต่ละมุมมีทั้งพิกัด AR ดิบ และ lat/lon ที่แปลงแล้ว ผมเก็บทั้งสองอย่างเสมอ เพราะถ้าวันหลังได้ค่าทิศที่ดีกว่า ก็เอาพิกัดดิบมาแปลงใหม่ได้โดยไม่ต้องกลับไปวัดหน้างาน

นำเข้า SketchUp แยก layer ต่อหนึ่งไฟล์

ในโมเดลที่ผูกพิกัดโลกไว้แล้ว การวางผังทำได้ตรง ๆ ด้วย latlong_to_point ตัวเดียวจบ ที่ผมเพิ่มเข้าไปคือหนึ่งไฟล์เท่ากับหนึ่ง layer เพราะเก็บมาหลายรอบหลายวัน อยากเปิดปิดเทียบกันได้จาก Tags tray

require 'json'
m = Sketchup.active_model
raise 'model is not geo-located' unless m.georeferenced?
m.start_operation("CornerMapLayers", true)

mtoi = 39.37007874
# ล้างของรอบก่อนทั้งกลุ่มและ layer เพื่อให้รันซ้ำได้ ไม่ทับซ้อน
m.entities.grep(Sketchup::Group).to_a.each { |g|
  g.erase! if g.valid? && g.name.start_with?("CM ") }
m.layers.to_a.each { |l| m.layers.remove(l) rescue nil if l.name.start_with?("CM ") }

Dir.glob(($cm_dir || '.') + '/cornermap_*.json').sort.each { |path|
  data = JSON.parse(File.read(path, :encoding => 'UTF-8')) rescue next
  grp = m.entities.add_group
  grp.name = "CM " + File.basename(path)
  (data['rooms'] || []).each { |r|
    cor = (r['corners'] || []).select { |c| c['lat'] }.sort_by { |c| c['seq'].to_i }
    next if cor.size < 3
    pts = cor.map { |c| p = m.latlong_to_point([c['lon'], c['lat']])
                        Geom::Point3d.new(p.x, p.y, 0.05.m) }   # ยกกันจมภาพพื้น
    # ตัดจุดปิดลูปฝั่ง Ruby อีกชั้น เผื่อไฟล์ยังไม่ผ่านตัวล้าง
    pts.pop while pts.size > 3 &&
      Math.hypot((pts[-1].x - pts[0].x).to_f, (pts[-1].y - pts[0].y).to_f) < 0.6 * mtoi
    g = grp.entities.add_group
    g.name = r['name'].to_s
    g.entities.add_edges(pts + [pts.first])
    g.entities.grep(Sketchup::Face).each { |f| f.erase! if f.valid? }   # เอาหน้าออก เหลือเส้น
  }
  grp.layer = m.layers.add(File.basename(path, '.json'))
}
m.commit_operation

สองบรรทัดที่ต้องมีคือ ยก z ขึ้นเล็กน้อย กันเส้นจมระนาบพื้น และ ลบ face ที่เกิดเอง เพราะวงปิดที่อยู่ระนาบเดียวกัน SketchUp จะสร้างหน้าให้อัตโนมัติ แล้วมันบังทุกอย่างที่อยู่ข้างล่าง

สรุปและข้อจำกัดที่ต้องรู้

วิธีนี้ให้ผังที่รูปร่างกับระยะดีพอสำหรับงานออกแบบเบื้องต้น และให้ทิศกับตำแหน่งบนโลกมาด้วยโดยไม่ต้องมีอุปกรณ์เพิ่ม ใช้แค่มือถือเครื่องเดียว

ข้อจำกัดที่ต้องยอมรับ หนึ่ง ในอาคารลึก ๆ ที่ GNSS ไม่เข้าเลย จะไม่มีตัวอย่างให้ fit ต้องเดินออกมาข้างนอกสักรอบ สอง ARCore drift สะสมในเซสชันยาว ๆ ห้องท้าย ๆ จะเพี้ยนกว่าห้องแรก ต้องดูค่า drift ตอนปิดลูปเป็นตัวบอก สาม ค่าทิศระดับ 4 องศา แปลว่าที่ระยะ 50 เมตร ปลายจะคลาดไปราว 3.5 เมตร ถ้าต้องการดีกว่านี้ต้องเดินให้ไกลกว่านี้

ส่วนตัวผมใช้มันเป็นตัวตั้งต้นครับ ได้ผังรวมทั้งชั้นในเวลาไม่กี่สิบนาที แล้วค่อยเอาเลเซอร์วัดระยะไปเก็บเฉพาะจุดที่ต้องการค่าแม่น ๆ ดีกว่าวัดด้วยมือทั้งหมดตั้งแต่ต้น และดีกว่าเดาทิศเอาเองแน่นอน

เรื่องเอาผังที่ได้ไปทำเป็นแผ่นแบบส่งงานจริง ผมเขียนแยกไว้ที่ ทำชุดแผ่นแบบจากโมเดล SketchUp ส่วนเรื่องถอดแบบจากกระดาษที่วาดไม่ตรงสเกล อยู่ที่ ถอดแบบจาก PDF ที่วาดไม่ตรงสเกล

ใครจะเอาโครงนี้ไปทำต่อก็ตามสะดวก ตัว solver ทั้งก้อนยาวไม่ถึงร้อยบรรทัด เขียนภาษาอะไรก็ได้ ตรงไหนผมอธิบายตกหล่นหรือพิมพ์ผิด เดี๋ยวกลับมาแก้เพิ่มให้ภายหลังครับ